12/20/2023

ROBOT DYNAMICS
Lynch Ch. 8 & Spong Ch. 6

 The dynamic models are important in the

 Design of robots
 More realistic simulation
 Animation of robot motion
 Design of control algorithms
 Rigid robot as the chain of connected bodies modeled by
 Euler-Lagrange, Hamilton’s or the principle of virtual work
 Newton-Euler, recursive formulation

1
 12/20/2023

Euler-Lagrange Equations

 Lagrangian is defined as the difference between kinetic and

potential energies as
L(𝑞, 𝑞)
ሶ = K(𝑞, 𝑞)−ሶ P(𝑞)
 With 𝑞 n-dim generalized coordinates
 For instance, the n-DH joint variables in n-link rigid robot
 𝑞ሶ is the velocity (twist) vector
 The equations of motions may be derived by
𝑑 𝜕L 𝜕L
𝜏𝑘 = −
𝑑𝑡 𝜕𝑞ሶ 𝑘 𝜕𝑞𝑘
 𝜏𝑘 external forces & moments applied to joint 𝑘 = 1, … , 𝑛

 General rigid body’s kinetic energy is

1 1
K = 𝑚𝑣 𝑇 𝑣 + 𝜔𝑇 I𝜔
2 2
 𝑣, 𝜔 are velocities in the inertial frame

 𝑚 is the mass of the body

 I is the inertia tensor about its axis of rotation in inertial frame

2
 12/20/2023

Inertia Tensor

 The inertial tensor I is defined in the inertial frame, however

the inertia of the body can be computed in the rigid body
frame as
𝐼𝑥𝑥 𝐼𝑥𝑦 𝐼𝑥𝑧
𝐼 = 𝐼𝑦𝑥 𝐼𝑦𝑦 𝐼𝑦𝑧
𝐼𝑧𝑥 𝐼𝑧𝑦 𝐼𝑧𝑧
with

Inertia Tensor

 𝐼𝑥𝑥 , 𝐼𝑦𝑦 , 𝐼𝑧𝑧 are principal moments of inertia about body fixed
𝑥, 𝑦, 𝑧 axes, constant for a rigid body
 Off diagonal terms, 𝐼𝑥𝑦 , 𝐼𝑦𝑧 , 𝐼𝑥𝑧 cross products of inertia and
can be made zero, if the body axes are selected by the
symmetric axes
 There exists a diagonal inertia tensor about the axes of
symmetry

3
 12/20/2023

Example

N-link Robot’s Kinetic Energy

 Given the rotaton matrix 𝑅 between the inertial and body frames, the
similarity transformation yields
I = 𝑅𝐼𝑅𝑇
 Then,
1 1
K= 𝑚𝑣 𝑇 𝑣 + 𝜔𝑇 𝑅𝐼𝑅𝑇 𝜔
2 2
 Velocities may be expressed in joint space by the Jacobians as
𝑣𝑖 = 𝐽𝑣𝑖 𝑞 𝑞ሶ 𝜔𝑖 = 𝐽𝜔𝑖 𝑞 𝑞ሶ
 Thus, the n-link rigid robot kinetic energy
𝑛
1 1
K = 𝑞ሶ 𝑇 ෍ 𝑚𝑖 𝐽𝑣𝑖 𝑇 𝐽𝑣𝑖 + 𝐽𝜔𝑖 𝑇 𝑅𝑖 𝐼𝑖 𝑅𝑖𝑇 𝐽𝜔𝑖 𝑞ሶ
2 2
𝑖=1
Or
1 𝑇
K= 𝑞ሶ 𝐷(𝑞)𝑞ሶ
2

4
 12/20/2023

Potential Energy for an n-link Robot

 For each link i, as point mass 𝑚𝑖 the link’s potential energy

𝑃𝑖 = 𝑔𝑇 𝑟𝑐𝑖 𝑚𝑖
 Total potential energy for n-link
𝑛 𝑛

𝑃 = ෍ 𝑃𝑖 = ෍ 𝑔𝑇 𝑟𝑐𝑖 (𝑞1 , … , 𝑞𝑖 )𝑚𝑖

𝑖=1 𝑖=1

Lagrangian

 The inertia matrix is symmetric?

𝑛
1
𝐷 𝑞 = ෍ 𝑚𝑖 𝐽𝑣𝑖 𝑇 𝐽𝑣𝑖 + 𝐽𝜔𝑖 𝑇 𝑅𝑖 𝐼𝑖 𝑅𝑖𝑇 𝐽𝜔𝑖
2
𝑖=1
 Let’s express K in scalar form
𝑛
1 1
K = 𝑞ሶ 𝑇 𝐷 𝑞 𝑞ሶ = ෍ 𝑑𝑖𝑗 𝑞 𝑞ሶ 𝑖 𝑞ሶ 𝑗
2 2
𝑖,𝑗

10

5
 12/20/2023

Partial Derivatives

𝑛
1
L = ෍ 𝑑𝑖𝑗 𝑞 𝑞ሶ 𝑖 𝑞ሶ 𝑗 − 𝑃(𝑞)
2
𝑖,𝑗

𝑑 𝜕L 𝜕L
− = 𝜏𝑘
𝑑𝑡 𝜕𝑞ሶ 𝑘 𝜕𝑞𝑘

𝑛
𝜕L 1
= ෍ 𝑑𝑗𝑘 𝑞 + 𝑑𝑘𝑗 𝑞 𝑞ሶ 𝑗
𝜕 𝑞ሶ 𝑘 2
𝑗

 Thus

𝑑 𝜕L 𝑑 𝜕𝐾
= = ෍ 𝑑𝑘𝑗 𝑞ሷ 𝑗
𝑑𝑡 𝜕𝑞ሶ 𝑘 𝑑𝑡 𝜕𝑞ሶ 𝑘
𝑗

11

𝑛
1
L = ෍ 𝑑𝑖𝑗 𝑞 𝑞ሶ 𝑖 𝑞ሶ 𝑗 − 𝑃(𝑞)
2
𝑖,𝑗

𝑛
𝜕L 1 𝜕𝑑𝑘𝑗 𝜕𝑑𝑘𝑖 𝜕𝑑𝑖𝑗 𝜕𝑃
= ෍ + − 𝑞ሶ 𝑞ሶ −
𝜕𝑞𝑘 2 𝜕𝑞𝑖 𝜕𝑞𝑗 𝜕𝑞𝑘 𝑖 𝑗 𝜕𝑞𝑘
𝑖,𝑗
 Christoffel symbols of the first kind

1 𝜕𝑑𝑘𝑗 𝜕𝑑𝑘𝑖 𝜕𝑑𝑖𝑗

𝑐𝑖𝑗𝑘 = + −
2 𝜕𝑞𝑖 𝜕𝑞𝑗 𝜕𝑞𝑘

𝜕𝑃
𝜙𝑘 =
𝜕𝑞𝑘

12

6
 12/20/2023

The equations of motion

 The equations of motion for link k

𝑛 𝑛

෍ 𝑑𝑘𝑗 𝑞ሷ 𝑗 + ෍ 𝑐𝑖𝑗𝑘 𝑞ሶ 𝑖 𝑞ሶ 𝑗 + 𝜙𝑘 = 𝜏𝑘 , 𝑘 = 1, … , 𝑛
𝑗=1 𝑖,𝑗

 In matrix form, the Euler-Lagrange robot equations become compact as

𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏

 The 𝑘, 𝑗 th element of the 𝐶 𝑞, 𝑞ሶ matrix is

𝑐𝑘𝑗 = ෍ 𝑐𝑖𝑗𝑘 𝑞ሶ 𝑖
𝑖=1

13

Example

 Find the dynamic model of PP arm

 The generalized (joint) variables
𝑞 = 𝑞1 𝑞2 𝑇

 Velocity kinematics
𝑣𝑐1 = 𝐽𝑣𝑐1 𝑞ሶ
𝑣𝑐2 = 𝐽𝑣𝑐2 𝑞ሶ
No angular velocities and prismatic joints Jacobians

0 0 0 0
𝐽𝑣𝑐1 = 𝑧0 0 = 0 0 𝐽𝑣𝑐2 = 𝑧0 𝑧1 = 0 1
1 0 1 0

14

7
 12/20/2023

 The kinetic energy

𝑛
1
K = 𝑞ሶ 𝑇 ෍ 𝑚𝑖 𝐽𝑣𝑖 𝑇 𝐽𝑣𝑖 𝑞ሶ
2
𝑖=1

1 0 1 0
𝐽𝑣𝑇𝑐1 𝐽𝑣𝑐1 = 𝐽𝑣𝑇𝑐2 𝐽𝑣𝑐2 =
0 0 0 1

1 𝑇 1 0 1 0
K= 𝑞ሶ 𝑚1 + 𝑚2 𝑞ሶ
2 0 0 0 1

Then

1 𝑇 𝑚1 + 𝑚2 0
K= 𝑞ሶ 𝑞ሶ
2 0 𝑚2

𝐷(𝑞)

15

The potential energy

𝑃 = 𝑃1 + 𝑃2 = 𝑔𝑚1 𝑞1 + 𝑔𝑚2 𝑞1 + 𝑐

𝜕𝑃 𝜕𝑃
𝜙1 = = 𝑔 𝑚1 + 𝑚2 , 𝜙2 = =0
𝜕𝑞1 𝜕𝑞2
 Therefore, the matrices

𝑚1 + 𝑚2 0 0 0 𝑔 𝑚1 + 𝑚2
𝐷= ,𝐶 = 𝐺=
0 𝑚2 0 0 0
 The EoM
𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺(𝑞) = 𝜏

𝑚1 + 𝑚2 0 𝑞ሷ 1 𝑔 𝑚1 + 𝑚2 𝑓1
+ =
0 𝑚2 𝑞ሷ 2 0 𝑓2

16

8
 12/20/2023

Example 2: Planar Elbow

• Find the dynamic model of RR arm

• The generalized (joint) variables

𝑇
𝑞 = 𝑞1 𝑞2

• Linear velocity kinematics

𝑣𝑐1 = 𝐽𝑣𝑐1 𝑞ሶ
𝑣𝑐2 = 𝐽𝑣𝑐2 𝑞ሶ

17

Example 2: Jacobian at 𝑐1

 For the revolute joint

𝐽𝑣𝑐1 = 𝑧0 × 𝑜𝑐1 − 𝑜0 0

0 −1 0 𝑙𝑐1 𝑐1
 𝑧0 × 𝑜𝑐1 − 𝑜0 = 1 0 0 𝑙𝑐1 𝑠1
0 0 0 0

−𝑙𝑐1 𝑠1 0
𝐽𝑣𝑐1 = 𝑙𝑐1 𝑐1 0
0 0

18

9
 12/20/2023

Example 2: Jacobian at 𝑐2

𝐽𝑣𝑐2 = 𝑧0 × 𝑜𝑐2 − 𝑜0 𝑧1 × 𝑜𝑐2 − 𝑜1

0 −1 0 𝑙1 𝑐1 + 𝑙𝑐2 𝑐12 −𝑙1 𝑠1 − 𝑙𝑐2 𝑠12

𝑧0 × 𝑜𝑐2 − 𝑜0 = 1 0 0 𝑙1 𝑠1 + 𝑙𝑐2 𝑠12 = 𝑙1 𝑐1 + 𝑙𝑐2 𝑐12
0 0 0 0 0

0 −1 0 𝑙𝑐2 𝑐12 −𝑙𝑐2 𝑠12

𝑧1 × 𝑜𝑐2 − 𝑜1 = 1 0 0 𝑙𝑐2 𝑠12 = 𝑙𝑐2 𝑐12
0 0 0 0 0

−𝑙1 𝑠1 − 𝑙𝑐2𝑠12 −𝑙𝑐2 𝑠12

𝐽𝑣𝑐2 = 𝑙1 𝑐1 + 𝑙𝑐2 𝑐12 𝑙𝑐2 𝑐12
0 0

19

 Angular joint velocities

𝑇 𝑇
𝜔1 = 0, 0, 𝑞ሶ 1 𝜔2 = 0, 0, 𝑞ሶ 1 + 𝑞ሶ 2
 Jacobians of the revolute joints
0 0 0 0
𝐽𝜔𝑐1 = 0 0 𝐽𝜔𝑐2 = 0 0
1 0 1 1

𝐷 = 𝑚1 𝐽𝑣𝑇𝑐1 𝐽𝑣𝑐1 + 𝑚2 𝐽𝑣𝑇2 𝐽𝑣2 + 𝐽𝜔1 𝑇 𝐼1 𝐽𝜔1 + 𝐽𝜔2 𝑇 𝐼2 𝐽𝜔2

1 0 2 1 1
𝐷 = 𝑚1 𝐽𝑣𝑇𝑐1 𝐽𝑣𝑐1 + 𝑚2 𝐽𝑣𝑇2 𝐽𝑣2 + 𝐼𝑧𝑧
1
+ 𝐼𝑧𝑧
0 0 1 1

20

10
 12/20/2023

1 𝜕𝑑𝑘𝑗 𝜕𝑑𝑘𝑖 𝜕𝑑𝑖𝑗

𝑐𝑖𝑗𝑘 = + −
2 𝜕𝑞𝑖 𝜕𝑞𝑗 𝜕𝑞𝑘

1 𝜕𝑑11
𝑐111 = =0
2 𝜕𝑞1

1 𝜕𝑑12 𝜕𝑑11 𝜕𝑑12 1 𝜕𝑑11

𝑐121 = 𝑐211 = + − = = −𝑚2 𝑙1 𝑙𝑐2 sin 𝑞2 ≝ ℎ
2 𝜕𝑞1 𝜕𝑞2 𝜕𝑞1 2 𝜕𝑞2

1 𝜕𝑑12 𝜕𝑑12 𝜕𝑑22

𝑐221 = + − =ℎ
2 𝜕𝑞2 𝜕𝑞2 𝜕𝑞1

1 𝜕𝑑21 𝜕𝑑21 𝜕𝑑11

𝑐112 = + − = −ℎ
2 𝜕𝑞1 𝜕𝑞2 𝜕𝑞2

21

1 𝜕𝑑21 𝜕𝑑21 𝜕𝑑12

 𝑐122 = 𝑐212 = 2 + − =0
𝜕𝑞2 𝜕𝑞1 𝜕𝑞2

1 𝜕𝑑22 𝜕𝑑22 𝜕𝑑22 1 𝜕𝑑22

 𝑐222 = 2 + − =2 =0
𝜕𝑞2 𝜕𝑞2 𝜕𝑞2 𝜕𝑞2

22

11
 12/20/2023

 Potential energies
 𝑃1 = 𝑚1 𝑔 𝑙𝑐1 sin 𝑞1
 𝑃2 = 𝑚2 𝑔[ 𝑙𝑐1 sin 𝑞1 + 𝑙𝑐2 sin(𝑞1 + 𝑞2 )]
 𝑃 = 𝑃1 + 𝑃2
𝜕𝑃
 𝜙1 = 𝜕𝑞 = 𝑔 𝑚1 𝑙𝑐1 + 𝑚2 𝑙1 cos 𝑞1 + 𝑚2 𝑔𝑙𝑐2 cos(𝑞1 + 𝑞2 )
1
𝜕𝑃
 𝜙2 = 𝜕𝑞 = 𝑚2 𝑔𝑙𝑐2 cos(𝑞1 + 𝑞2 )
2

23

EoM

𝑑11 𝑞ሷ 1 + 𝑑12 𝑞ሷ 2 + 𝑐121 𝑞ሶ 1 𝑞ሶ 2 + c211 𝑞ሶ 2 𝑞ሶ 1 + 𝑐221 𝑞ሶ 22 + 𝜙1 = 𝜏1

𝑑21 𝑞ሷ 1 + 𝑑22 𝑞ሷ 2 + 𝑐112 𝑞ሶ 12 + 𝜙2 = 𝜏2
 In matrix form
𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏
𝑇
 With 𝜏 = 𝜏1 𝜏2
𝑑11 𝑑12
 𝐷 𝑞 =
𝑑12 𝑑22
ℎ𝑞ሶ 2 ℎ(𝑞1ሶ + 𝑞ሶ 2 )
 𝐶 𝑞, 𝑞ሶ =
−ℎ𝑞ሶ 1 0
𝜙1
 𝐺 𝑞 =
𝜙2

24

12
 12/20/2023

Properties of Robot Dynamic Equations

 Let 𝐷(𝑞) be the robot inertia matrix and 𝐶(𝑞, 𝑞)

ሶ the Coriolis
and centripetal matrix.
 Then,
ሶ
𝐷(𝑞) − 2𝐶(𝑞, 𝑞)
ሶ
Is a skew symmetric matrix.
 𝐷(𝑞) matrix upper and lower bounded
𝜆1 𝐼 ≤ 𝐷 ≤ 𝜆𝑛 𝐼
 Implies 𝐷(𝑞) is invertible
 𝜆1 ≤ 𝐷 𝑞 ≤ 𝜆𝑛

25

 Passivity Property

𝑇
න 𝑞ሶ 𝑎 𝑇 𝜏(𝑎)𝑑𝑎 ≥ −𝑏 ∀𝑇 > 0
0

 The term
𝑞ሶ 𝑡 𝑇 𝜏(𝑡)
from input 𝜏 to output 𝑞ሶ is in power form.
 The energy is lower bounded for any 𝑇 > 0
 Can’t generate energy on its own

26

13
 12/20/2023

 Linear-in-the-parameters Property
𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏
May be expressed in linear parameters as
𝑌 𝑞, 𝑞,ሶ 𝑞ሷ Θ = 𝜏
where
 𝑌 𝑞, 𝑞,ሶ 𝑞ሷ Regressor
 Θ parameter vector

27

Example

 For the planar RR arm

𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏
𝑇
 With 𝜏 = 𝜏1 𝜏2

𝑑11 𝑑12
 𝐷 𝑞 =
𝑑12 𝑑22

ℎ𝑞ሶ 2 ℎ(𝑞1ሶ + 𝑞ሶ 2 )
 𝐶 𝑞, 𝑞ሶ = ℎ = −𝑚2 𝑙1 𝑙𝑐2 sin 𝑞2
−ℎ𝑞ሶ 1 0

𝑔 𝑚1 𝑙𝑐1 + 𝑚2 𝑙1 cos 𝑞1 + 𝑚2 𝑔𝑙𝑐2 cos(𝑞1 + 𝑞2 )

 𝐺 𝑞 =
𝑚2 𝑔𝑙𝑐2 cos(𝑞1 + 𝑞2 )

28

14
 12/20/2023

 If rewritten in linear-in-the-parameters form

𝑌 𝑞, 𝑞,ሶ 𝑞ሷ Θ = 𝜏
with

29

Forward and Inverse Dynamics

 Forward dynamics
𝑞
𝜏 Robot FD 𝑞ሶ
𝑞ሷ

𝑞ሷ = 𝐷 −1 𝑞 𝜏 − 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞

 Inverse dynamics
𝑞
𝑞ሶ Robot ID 𝜏
𝑞ሷ

𝜏 = 𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞

30

15
 12/20/2023

Newtonian mechanics

 The Newtonian mechanics can be stated as

1) Every action has an equal and opposite reaction.
2) The rate of change of the linear momentum equals the
total force applied to the body
3) The rate of change of the angular momentum equals the
total torque applied to the body

31

Newton-Euler Formulation

 (2) yields linear motion equations

𝑑 𝑚𝑣
=𝑓
𝑑𝑡
 When the mass is constant

𝑚𝑣ሶ = 𝑓

 In inertial frame

32

16
 12/20/2023

Newton-Euler Formulation

 (3) yields angular motion equations

𝑑 𝐼𝑜 𝜔𝑜
= 𝜏𝑜
𝑑𝑡
 However, 𝐼𝑜 may not be constant
 Angular momentum in body-fixed frame
ℎ = 𝐼𝑜 𝜔𝑜 = 𝑅𝐼𝑅𝑇 𝑅𝜔 = 𝑅𝐼𝜔

With body fixed 𝐼 constant and 𝜔 in body frame

 Angular momentum dynamic equation may be rewritten
𝑑ℎ
= 𝜏𝑜
𝑑𝑡
𝑑
ሶ + 𝑅𝐼 𝜔ሶ = 𝜏𝑜
𝑅𝐼𝜔 = 𝑅𝐼𝜔
𝑑𝑡

33

 We had shown that

ሶ = 𝑆(𝜔𝑜 )
𝑅𝑅
 Substitute 𝑅ሶ = 𝑆 𝜔𝑜 𝑅 to momentum eqn
𝑆 𝜔𝑜 𝑅𝐼𝜔 + 𝑅𝐼 𝜔ሶ = 𝜏𝑜
 Premultiply by 𝑅𝑇
𝑅𝑇 𝑆 𝜔𝑜 𝑅𝐼𝜔 + 𝐼 𝜔ሶ = 𝑅𝑇 𝜏0 = 𝜏
 Yields
𝑆 𝑅𝑇 𝜔𝑜 𝐼𝜔 + 𝐼 𝜔ሶ = 𝐼 𝜔ሶ + 𝑆 𝜔 𝐼𝜔 = 𝜏

𝐼 𝜔ሶ + 𝜔 × 𝐼𝜔 = 𝜏
In body fixed frame

34

17
 12/20/2023

Newton-Euler Applied to Rigid Robot

35

Forces and Moments on Link 𝑖

𝑐𝑖
𝑟𝑖+1,𝑐𝑖
𝑟𝑖,𝑐𝑖
−Rii+1 𝜏𝑖+1
𝜏𝑖 𝑚𝑖 𝑔𝑖
𝑖
−𝑅𝑖+1 𝑓𝑖+1
𝑓𝑖

 Balance equations for link 𝑖, with sums of forces and moments

𝑖
𝑓𝑖 − 𝑅𝑖+1 𝑓𝑖+1 + 𝑚𝑖 𝑔𝑖 = 𝑚𝑖 𝑎𝑐,𝑖

𝑖 𝑖
𝜏𝑖 − 𝑅𝑖+1 𝜏𝑖+1 + 𝑓𝑖 × 𝑟𝑖,𝑐𝑖 − 𝑅𝑖+1 𝑓𝑖+1 × 𝑟𝑖+1,𝑐𝑖 = 𝐼𝑖 𝛼𝑖 + 𝜔𝑖 × (𝐼𝑖 𝜔𝑖 )

36

18
 12/20/2023

 The 𝑖 𝑡ℎ link’s velocity and acceleration terms

𝜔𝑖 , 𝛼𝑖 , 𝑣𝑐,𝑖 , 𝑎𝑐,𝑖
to be expressed in terms of joint space variables
 For revolute joints
𝜔𝑖0 = 𝜔𝑖−1
0
+ 𝑧𝑖−1 𝑞ሶ 𝑖
 In inertial frame, link 𝑖 angular velocity is its parent links
velocity plus angular velocity generated by joint 𝑖
 In frame 𝑖
𝑖 0
𝜔𝑖 = 𝑅𝑖−1 (𝑅0𝑖−1 𝜔𝑖−1 ) + 𝑅0𝑖 𝑧𝑖−1 𝑞ሶ 𝑖

𝑖
𝜔𝑖 = 𝑅𝑖−1 𝜔𝑖−1 + 𝑏𝑖 𝑞ሶ 𝑖

37

 Angular acceleration
𝛼𝑖 = 𝑅0𝑖 𝜔ሶ 𝑖0
𝑑
 Now 𝑑𝑡 (𝜔𝑖0 = 𝜔𝑖−1
0
+ 𝑧𝑖−1 𝑞ሶ 𝑖 ) yields

𝜔ሶ 𝑖0 = 𝜔ሶ 𝑖−1
0
+ 𝑧𝑖−1 𝑞ሷ 𝑖 + 𝑧𝑖−1
ሶ 𝑞ሶ 𝑖

𝜔ሶ 𝑖0 = 𝜔ሶ 𝑖−1
0
+ 𝑧𝑖−1 𝑞ሷ 𝑖 + 𝜔𝑖0 × 𝑧𝑖−1 𝑞ሶ 𝑖

38

19
 12/20/2023

 Sub into ang. Accel.

0
𝛼𝑖 = 𝑅0𝑖 𝜔ሶ 𝑖−1 + 𝑧𝑖−1 𝑞ሷ 𝑖 + 𝜔𝑖0 × 𝑧𝑖−1 𝑞ሶ 𝑖

 Recall 𝑏𝑖 = 𝑅0𝑖 𝑧𝑖−1

𝑖
𝛼𝑖 = 𝑅𝑖−1 𝛼𝑖−1 + 𝑏𝑖 𝑞ሷ 𝑖 + 𝜔𝑖 × 𝑏𝑖 𝑞ሶ 𝑖

39

Constrained Dynamics

 Linear velocity
0 0
𝑣𝑐,𝑖 = 𝑣𝑒,𝑖−1 + 𝜔𝑖0 × 𝑟𝑖,𝑐
0
𝑖

 Its time derivative

0 0
𝑎𝑐,𝑖 = 𝑎𝑒,𝑖−1 + 𝜔ሶ 𝑖0 × 𝑟𝑖,𝑐
0
𝑖
+ 𝜔𝑖0 × 𝑟𝑖,𝑐
ሶ0 𝑖

 Change of coordinates to ith frame 𝑅 𝑎 × 𝑏 = 𝑅𝑎 × 𝑅𝑏

𝑖
𝑎𝑐,𝑖 = 𝑅𝑖−1 𝑎𝑒,𝑖−1 + 𝜔ሶ 𝑖 × 𝑟𝑖,𝑐𝑖 + 𝜔𝑖 × (𝜔𝑖 × 𝑟𝑖,𝑐𝑖 )

40

20
 12/20/2023

 At the end of the link, the linear acceleration becomes

𝑖
𝑎𝑒,𝑖 = 𝑅𝑖−1 𝑎𝑒,𝑖−1 + 𝜔ሶ 𝑖 × 𝑟𝑖,𝑖+1 + 𝜔𝑖 × (𝜔𝑖 × 𝑟𝑖,𝑖+1 )

41

Recursive Formulation Newton Euler Arm Dynamics

 Compute link velocities, acceleration, forces, moments iteratively

𝜔𝑜 = 𝛼𝑜 = 𝑎𝑒,0 = 0 # 𝑖𝑛𝑖𝑡𝑖𝑎𝑙𝑖𝑧𝑒 𝑓𝑜𝑟𝑤𝑎𝑟𝑑 𝑟𝑒𝑐𝑢𝑟𝑠𝑖𝑜𝑛
𝑓𝑜𝑟 𝑖 = 1: 𝑛
𝑖
𝜔𝑖 = 𝑅𝑖−1 𝜔𝑖−1 + 𝑏𝑖 𝑞ሶ 𝑖
𝑖
𝛼𝑖 = 𝑅𝑖−1 𝛼𝑖−1 + 𝑏𝑖 𝑞ሷ 𝑖 + 𝜔𝑖 × 𝑏𝑖 𝑞ሶ 𝑖
𝑖
𝑎𝑒,𝑖 = 𝑅𝑖−1 𝑎𝑒,𝑖−1 + 𝜔ሶ 𝑖 × 𝑟𝑖,𝑖+1 + 𝜔𝑖 × (𝜔𝑖 × 𝑟𝑖,𝑖+1 )
𝑖
𝑎𝑐,𝑖 = 𝑅𝑖−1 𝑎𝑒,𝑖−1 + 𝜔ሶ 𝑖 × 𝑟𝑖,𝑐𝑖 + 𝜔𝑖 × (𝜔𝑖 × 𝑟𝑖,𝑐𝑖 )
𝑓𝑛+1 = 𝜏𝑛+1 = 0 # 𝑖𝑛𝑖𝑡𝑖𝑎𝑙𝑖𝑧𝑒 𝑏𝑎𝑐𝑘𝑤𝑎𝑟𝑑 𝑟𝑒𝑐𝑢𝑟𝑠𝑖𝑜𝑛
𝑓𝑜𝑟 𝑖 = 𝑛: 1
𝑖
𝑓𝑖 = 𝑅𝑖+1 𝑓𝑖+1 − 𝑚𝑖 𝑔𝑖 + 𝑚𝑖 𝑎𝑐,𝑖
𝑖 𝑖
𝜏𝑖 = 𝑅𝑖+1 𝜏𝑖+1 − 𝑓𝑖 × 𝑟𝑖,𝑐𝑖 + 𝑅𝑖+1 𝑓𝑖+1 × 𝑟𝑖+1,𝑐𝑖 + 𝐼𝑖 𝛼𝑖 + 𝜔𝑖 × (𝐼𝑖 𝜔𝑖 )

42

21
 12/20/2023

Example: Planar RR Arm Dynamics NE Approach

 Assume the CoM of links at 𝑙𝑐1 , 𝑙𝑐2 then

43

 Forward recursion for link 1

44

22
 12/20/2023

 Forward recursion for link 2

45

 Backward recursion for link 2

46

23
 12/20/2023

 Backward recursion for link 1

47

24
 7.11.2023

FORWARD KINEMATICS
Ch. 3

Kinematic Chains

 Robot arm is composed of a set of links connected by joints

 We assume links are rigid (or elasticity is negligible)
 All joints have single DoF: revolute or prismatic, in this course
 A generic robot with 𝑛 joints will have 𝑛 + 1 links
𝜃𝑖 revolute joint
𝑞𝑖 = ቊ
𝑑𝑖 prismatic joint
 For kinematic analysis each link will have coordinate freame
attached, for link i
 Joint i is actuated
 Frame attached to link 𝑖 experiencing motion {𝑜𝑖 , 𝑥𝑖 , 𝑦𝑖 , 𝑧𝑖 }

1
 7.11.2023

 Suppose the homogeneous transformation matrix Define the

position of link 𝑖 w.r.t. its parent frame 𝑖 − 1
𝐴𝑖 = 𝐴𝑖 𝑞𝑖 = 𝑇𝑖 𝑖−1
 With
𝑖−1
𝐴𝑖 = 𝑅𝑖 𝑜𝑖𝑖−1
0 1
 Forward transformation from 𝑖-frame to 𝑗 > 𝑖-frame
𝑅𝑗𝑖 𝑜𝑗𝑖
𝑇𝑗𝑖 = 𝐴𝑖+1 𝑞𝑖+1 … 𝐴𝑗 𝑞𝑗 =
0 1
𝑗−1
 𝑅𝑗𝑖 = 𝑅𝑖+1
𝑖
…𝑅𝑗 and
𝑖 𝑖 𝑖 𝑗−1
 𝑜𝑗 = 𝑜𝑗−1 + 𝑅𝑗−1 𝑜𝑗

 For 𝑛 link arm, the position and orientation of end effector

w.r.t. Base frame-0
𝐻 = 𝑇𝑛0 = 𝐴1 𝑞1 𝐴2 𝑞2 … 𝐴𝑛 (𝑞𝑛 )

2
 7.11.2023

DH Coordinate Assignment

 Pose of the robot with 6 Dof in 3D space may be parameterized

conveniently by selecting a suitable frame convention
 Denavit Hartenberg is one of such convention based on four transformations
 Link i transformation matrix Ai may be derived as
𝐴𝑖 = 𝑅𝑜𝑡𝑧𝑖−1 ,𝜃𝑖 𝑇𝑟𝑎𝑛𝑠𝑧𝑖−1 ,𝑑𝑖 𝑇𝑟𝑎𝑛𝑠𝑥𝑖 ,𝑎𝑖 𝑅𝑜𝑡𝑥𝑖,𝛼𝑖
 With
 𝜃𝑖 joint angle
 𝑑𝑖 link offset
 𝑎𝑖 link length
 𝛼𝑖 link twist

𝑜𝑖′
 DH params [Spong’s]
 For link 𝑖
 Parent 𝑜𝑖−1
 Child Parent 𝑜𝑖
 Joint angle about 𝑧𝑖−1 between axes 𝑥𝑖−1 and 𝑥𝑖
𝜽∗𝒊 = 𝑥ෟ
𝑖−1 𝑥𝑖 ቚ
𝑧𝑖

 Link offset on 𝑧𝑖−1 between 𝑜𝑖−1 and 𝑜𝑖′ (the projection of 𝑜𝑖 on axis 𝑧𝑖−1 )
𝒅∗𝒊 = 𝑜𝑖−1 𝑜𝑖′
 Link length on 𝑥𝑖 between 𝑜𝑖′ and 𝑜𝑖
𝒂𝒊 = 𝑜𝑖′ 𝑜𝑖
′
 Twist angle about 𝑥𝑖 between 𝑧𝑖−1 and 𝑧𝑖
𝜶𝒊 = 𝑧෣
′
𝑖−1 𝑧𝑖 ቚ
𝑥𝑖

3
 7.11.2023

Link 1 DH Parameters

 {0} rotated about the 𝑧0 by 𝜃 brings 𝑥0 to 𝑥1

 Shifting along 𝑧0 by 𝑑 moves 𝑜𝑜 onto axis 𝑥1
 Single translation 𝑎 along 𝑥1 brings 𝑜𝑜′ to the origin {1}
 Since both 𝑥 axes are aligned, a single rotation 𝛼 about 𝑥1 can now align
𝑧𝑜 to 𝑧1 , hence {0} to {1}

𝑜𝑜′
 Consecutive xforms of

 𝑅𝑜𝑡 𝑧𝑜 , 𝜃1 𝑇𝑟𝑎𝑛𝑠 𝑧𝑜 , 𝑑1 𝑇𝑟𝑎𝑛𝑠 𝑥1 , 𝑎1 𝑅𝑜𝑡(𝑧1 , 𝛼1 )

DH Coordinate Assignment

 Rule 1: 𝑧𝑖−1 axis is the 𝑖 𝑡ℎ joint axis of actuation, on the parent frame
 Rule 2: 𝑥𝑖 axis is either
 The shortest line segment between 𝑧𝑖−1 and 𝑧𝑖 , when exists
 If 𝑧𝑖−1 and 𝑧𝑖 are parallel, 𝑥𝑖 can be anywhere, pick the most
convenient on 𝑖 𝑡ℎ joint,
 If 𝑧𝑖−1 and 𝑧𝑖 orthogonal, 𝑥𝑖 is selected as the normal to the plane
 𝑥𝑜 (hence 𝑜0 ) is free but for the convenience picked parallel to 𝑥1
 If 𝑧𝑛 of the end effector is free to pick, we select it parallel to 𝑧𝑛−1

4
 7.11.2023

Link i Transformation by DH Params

 Link 𝑖’s coordinate transformation

𝐴𝑖 = 𝑅𝑜𝑡 𝑧𝑖−1 , 𝜃𝑖 𝑇𝑟𝑎𝑛𝑠 𝑧𝑖−1 , 𝑑𝑖 𝑇𝑟𝑎𝑛𝑠 𝑥𝑖 , 𝑎𝑖 𝑅𝑜𝑡(𝑧𝑖 , 𝛼𝑖 )

𝑐𝜃𝑖 −𝑠𝜃𝑖 𝑐𝛼𝑖 𝑠𝜃𝑖 𝑠𝛼𝑖 𝑎𝑖 𝑐𝜃𝑖

𝑠𝜃𝑖 𝑐𝜃𝑖 𝑐𝛼𝑖 −𝑐𝜃𝑖 𝑠𝛼𝑖 𝑎𝑖 𝑠𝜃𝑖
𝐴𝑖 =
0 𝑠𝛼𝑖 𝑐𝛼𝑖 𝑑𝑖
0 0 0 1

 Can be used to find the transfroms from any frame to any

 For instance base to end effector transformation, 𝑇𝑛0 = 𝐴1 … 𝐴𝑛

RR Planar Robot

𝜃2

𝜃1

5
 7.11.2023

 Pick 𝑧0 , 𝑧1 along joints and let 𝑧2 = 𝑧1(coming towards you)

𝑧2

𝜃2

𝜃1

𝑧1

𝑧0

 𝑥0 is free and for 𝑥1 check out 𝑧0 and 𝑧1 they are parallel

 𝑥1 conveniently selected along link 1 𝑧2

𝜃2

𝑥1
𝑜1
𝜃1
𝑥0
𝑧1

𝑧0
 So are 𝑥0 , when 𝜃1 = 0 and
 the end effector 𝑥2 when 𝜃2 = 0

6
 7.11.2023

DH Parameters of Planar Robot

 For link 1
 𝜃1 is 𝑥ෟ
0 𝑥1 ȁ𝑧0 that is shown as 𝜃1

 𝑑1 = 𝑜0 𝑜0′ 𝑧𝑜 =0
 𝑎1 = 𝑜𝑜′ 𝑜1 𝑥1 = 𝑙1
𝑧2
 𝛼1 = 𝑧෣
′′
𝑜 𝑧1 ȁ𝑥1 = 0
𝜃2

𝑥1
𝑜1
𝜃1
𝑥0
𝑧1

𝑧0

DH Parameters of Planar Robot

 For link 2
 𝜃2 is 𝑥ෟ
1 𝑥2 ȁ𝑧1 that is shown as 𝜃2

 𝑑2 = 𝑜1 𝑜1′ 𝑧1 =0
𝑜2
 𝑎2 = 𝑜1′ 𝑜2 𝑥2 = 𝑙2
𝑧2
 𝛼2 = 𝑧෣
′′
1 𝑧2 ȁ𝑥2 = 0
𝜃2

𝑥1
𝑜1
𝜃1
𝑥0
𝑧1

𝑧0

7
 7.11.2023

 The DH Parameter Table

Link-i 𝜽𝒊 𝒅𝒊 𝒂𝒊 𝜶𝒊 𝑐𝜃1 −𝑠𝜃1 0 𝑙1 𝑐𝜃1
0 𝑙1 𝑠𝜃1
1 𝜽𝟏 0 𝒍𝟏 0 𝑇10 = 𝐴1 = 𝑠𝜃1 𝑐𝜃1
0 0 1 0
2 𝜽𝟐 0 𝒍𝟐 0 0 0 0 1

𝑐𝜃2 −𝑠𝜃2 0 𝑙2 𝑐𝜃2

0 𝑙2 𝑠𝜃2
𝑇21 = 𝐴2 = 𝑠𝜃2 𝑐𝜃2
0 0 1 0
0 0 0 1

𝑐𝜃1 −𝑠𝜃1 0 𝑙1 𝑐𝜃1 𝑐𝜃2 −𝑠𝜃2 0 𝑙2 𝑐𝜃2 𝑐12 −𝑠12 0 𝑑𝑥

0 𝑙1 𝑠𝜃1 0 𝑙2 𝑠𝜃2 0 𝑑𝑦
𝑇20 = 𝐴1 𝐴2 = 𝑠𝜃1 𝑐𝜃1 𝑠𝜃2 𝑐𝜃2 = 𝑠12 𝑐12
0 0 1 0 0 0 1 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1

𝑐1 = 𝑐𝜃1 = cos 𝜃1 ; 𝑐12 = cos 𝜃1 + 𝜃2

𝑑𝑥 = 𝑙2 𝑐1 𝑐2 − 𝑙2 𝑠1 𝑠2 + 𝑙1 𝑐1 = 𝑙1 𝑐1 + 𝑙2 𝑐12 𝑑𝑦 = 𝑙2 𝑠1 𝑐2 + 𝑙2 𝑐1 𝑠2 + 𝑙1 𝑠1 = 𝑙1 𝑠1 + 𝑙2 𝑠12

3D Cylindrical Arm

 Axes of actuations and end effector define 𝑧𝑖 ’s

𝑧2 𝑧3

𝑧1

𝑧𝑜

8
 7.11.2023

3D Cylindrical Arm Frames

 For 𝑥𝑖 ’s check out 𝑧𝑖−1 and 𝑧𝑖 for 𝑖 = 1,2,3

𝑑3
𝑧2 𝑧3

𝑥2 𝑥3

𝑧1

𝑑2

𝑥1
𝜃1

𝑥0 𝑧𝑜

3D Cylindrical Arm Frames

 For link 1 𝑑3
𝑧2 𝑧3
 𝜃1 is 𝑥ෟ
0 𝑥1 ȁ𝑧0 that is shown as 𝜃1
𝑥2 𝑥3
 𝑑1 = 𝑜0 𝑜0′ 𝑧𝑜 = 𝑑1
 𝑎1 = 𝑜𝑜′ 𝑜1 𝑥1 =0 𝑧1

 𝛼1 = 𝑧෣
′′
𝑜 𝑧1 ȁ𝑥1 = 0
𝑑2

𝑥1
𝑧𝑜
𝜃1
𝑑1

𝑥0

9
 7.11.2023

3D Cylindrical Arm Frames

 For link 2 𝑑3
𝑧2 𝑧3
 𝜃2 is 𝑥ෟ
1 𝑥2 ȁ𝑧1 selected as 0

 𝑑2 = 𝑜1 𝑜1′ 𝑧1 = 𝑑2∗ 𝑥2 𝑥3

 𝑎2 = 𝑜1′ 𝑜2 𝑥2 =0 𝑧1

 𝛼2 = 𝑧෣
′′
1 𝑧2 ȁ𝑥2 = −90
𝑑2

𝑥1
𝑧𝑜
𝜃1
𝑑1

𝑥0

3D Cylindrical Arm Frames

 For link 3 𝑑3
𝑧2 𝑧3
 𝜃3 is 𝑥ෟ
2 𝑥3 ȁ𝑧2 selected as 0
𝑥2 𝑥3
 𝑑3 = 𝑜2 𝑜2′ 𝑧2 = 𝑑3∗
 𝑎3 = 𝑜2′ 𝑜3 𝑥3 =0 𝑧1

 𝛼3 = 𝑧෣
′′
2 𝑧3 ȁ𝑥3 = 0
𝑑2

𝑥1
𝑧𝑜
𝜃1
𝑑1

𝑥0

10
 7.11.2023

 The DH Parameter Table

Link-i 𝜽𝒊 𝒅𝒊 𝒂𝒊 𝜶𝒊 𝑐𝜃1 −𝑠𝜃1 0 0
𝑠𝜃1 𝑐𝜃1 0 0
1 𝜃1 𝑑1 0 0 𝑇10 = 𝐴1 = 1 𝑑1
0 0
2 0 𝑑2∗ 0 -90 0 0 0 1
3 0 𝑑3∗ 0 0
1 0 0 0
0 0 1 0
𝑇21 = 𝐴2 =
0 −1 0 𝑑2
0 0 0 1
𝑐1 0 −𝑠1 −𝑠1 𝑑3
𝑠1 0 𝑐1 𝑑3 1 0 0 0
𝑇30 = 𝐴1 𝐴2 𝐴3 = 0 𝑑1 + 𝑑2
0 −1 0 1 0 0
𝑇32 = 𝐴3 =
0 0 0 1 0 0 1 𝑑3
0 0 0 1

Spherical Wrist

𝑑6

 Assume first three joints end point has a spherical RRR wrist
attached
 Each joint is perpendicular to the next, and 𝑜4 = 𝑜5 = 𝑜6
 DH Parameters

11
 7.11.2023

Link Transforms

6-DoF Forward Kinematic Example

 Cylindirical robot with spherical wrist

12
 7.11.2023

 We had computed 𝑇30 and 𝑇63 previously. So

𝑇60 = 𝑇30 𝑇63

Given joint space 𝑞 = (𝜃1 , 𝑑2 , 𝑑3 , 𝜃4 , 𝜃5 , 𝜃6 ) the EE pose can be computed

Stanford Manipulator Coordinate Frames

13
 7.11.2023

Stanford Manipulator Exercise

Show

14
 10/13/2023

COORDINATE TRANSFORMATIONS
Robot Modeling and Control by Spong et al.

Conventions

 Depend on the document you read variables are symbolized

as
 Coordinate frames may be shown as either ways
𝑠 = 𝛴𝑠 : 𝑜𝑠 , 𝑥𝑠 , 𝑦𝑠 , 𝑧𝑠 in 3D
 For a stationary frame {𝑠} and body frame {𝑏}
◼A point p in a frame {𝑎} in 3D may be shown in either way
𝑝Ԧ𝑎 ; 𝑝𝑎 ; 𝑝𝑎 ∈ ℝ3×1
◼A Relative transformation: e.g., projections of unit vectors of
𝑏 frame to univ vectors of 𝑎 frame
𝑅𝑏𝑎 ; 𝑅𝑏𝑎
That is a relative vector representation in a-frame w.r.t. (as
seen from) b-brame

1
 10/13/2023

 Consider a point p in a 2D space

 Using {0}=Σ𝑜 : 0𝑜 𝑥𝑜 𝑦𝑜 coordinate frame, the point 𝑝 is expressed as

5
𝑝𝑜 =
6
 That is 5 unit vector(say 𝑖Ԧ𝑜 ) in 𝑥𝑜 and 6 unit vector(say 𝑗Ԧ𝑜 ) in 𝑦𝑜
5
𝑝Ԧ𝑜 = 5Ԧ𝑖𝑜 + 6Ԧ𝑗𝑜 or in matrix form 𝑝𝑜 =
6

 The same point in another 1 = Σ1 : 01 𝑥1 𝑦1 coordinate frame may be

−2
𝑝Ԧ1 = −2Ԧ𝑖1 + 4Ԧ𝑗1 or 𝑝1 =
4

 In engineering apps, we deal with vectors measured or

computed in different coordinate frames
 It is essential to transform any vector in a special frame to a
common frame of interest
 Using matrix operations
𝑝𝑎 = 𝑅𝑏𝑎 𝑝𝑏 = 𝑅𝑏𝑎 𝑝𝑏
 For a vector 𝑝
 𝑝𝑎 in Σ𝑎 frame
 𝑝𝑏 in Σ𝑏 frame
 𝑅𝑏𝑎 = 𝑅𝑏𝑎 transforms any vector in 𝑏 −frame to 𝑎 −frame
Where the frame origins coincide, i.e. 𝑂𝑜 𝑂1 = 0

2
 10/13/2023

Rotation about a Principle Axis

 Consider Σ𝑜 and Σ1 shown and compute the transformation matrix

𝑅1𝑜 (a rotation of 𝜃 about 𝑧𝑜 )
 Unit vector on 𝑥1 → 𝑖11 = 1 0 𝑇
in Σ1 projects to Σ𝑜 as
cos 𝜃
𝑖10 =
sin 𝜃
0 − sin 𝜃
 Similarly, 𝑗11 = may be written as 𝑗10 =
1 cos 𝜃

 To express 𝑅10
𝑅10 = 𝑖10 𝑗10
 Or,
cos 𝜃 − sin 𝜃
𝑅10 =
sin 𝜃 cos 𝜃
Example
3
2.4 Let 𝜃 = 30𝑜 and 𝑝1 = ⇒ 𝑝𝑜 ?
1
cos 30 − sin 30 3
𝑝𝑜 =
𝜃 = 30𝑜
sin 30 cos 30 1
1 3 −1 3 = 2.1
2.1
𝑝𝑜 =
2 1 3 1 2.4

3
 10/13/2023

Directional Cosine Matrix

 Let 𝑖10 = 𝑖10 is the projection of 𝑖Ԧ1 in to Σ𝑜 hence

𝑖Ԧ1 ∙ 𝑖Ԧ𝑜 cos 𝜃 cos 𝜃
𝑖10 = 𝑖10 = = =
𝑖Ԧ1 ∙ 𝑗Ԧ𝑜 cos(90 − 𝜃) sin 𝜃
 So is 𝑗10
𝑖1 ∙ 𝑖𝑜 𝑗1 ∙ 𝑖𝑜
𝑅10 = 𝑖10 𝑗10 =
𝑖1 ∙ 𝑗𝑜 𝑗1 ∙ 𝑗𝑜
 Since the inner product is the cos of the angle in between the unit vectors

𝑖1 ∙ 𝑖𝑜 = cos 𝜃
𝑖𝑜 𝑖1 ∙ 𝑗𝑜 = cos(90 − 𝜃)
𝑖𝑜 𝑗1 ∙ 𝑖𝑜 = cos(90 + 𝜃)
𝑖𝑜 𝑗1 ∙ 𝑗𝑜 = cos 𝜃

cos 𝜃 − sin 𝜃
𝑅1𝑜 =
sin 𝜃 cos 𝜃

Some properties of R Rotational Matrix

 Columns(rows) mutually orthonormal

 𝑅 −1 = 𝑅 𝑇
𝑇 −1
 𝑅𝑎𝑏 = 𝑅𝑏𝑎 = 𝑅𝑏𝑎
 𝑅 = [𝑟𝑖 𝑟𝑗 𝑟𝑘 ] with columns are unit vector projections
 Since all are in the right-handed coordinate system
 𝑟𝑖 × 𝑟𝑗 = 𝑟𝑘
 𝑟𝑗 × 𝑟𝑘 = 𝑟𝑖
 𝑟𝑘 × 𝑟𝑖 = 𝑟𝑗

4
 10/13/2023

Rotations in 3D

 Rotation Σ𝑜 about z by 𝜃 to get Σ1

𝑖1 ∙ 𝑖𝑜 𝑗1 ∙ 𝑖𝑜 𝑘1 ∙ 𝑖𝑜
𝑅10 = 𝑖10 𝑗10 𝑘10 = 𝑖1 ∙ 𝑗𝑜 𝑗1 ∙ 𝑗𝑜 𝑘1 ∙ 𝑗𝑜
𝑖1 ∙ 𝑘𝑜 𝑗1 ∙ 𝑘𝑜 𝑘1 ∙ 𝑘𝑜

𝑅10 = 𝑅𝑧,𝜃 may be shown in terms of principal rot as well

• 𝑅𝑧,0 = 𝐼
• 𝑅𝑧,𝜃 𝑅𝑧,𝜙 =𝑅𝑧,𝜃+𝜙
−1 𝑇
• 𝑅𝑧,𝜃 = 𝑅𝑧,−𝜃 = 𝑅𝑧,𝜃

Example

 Find the rotation matrix 𝑅1𝑜

 𝑖1 ∙ 𝑖𝑜 =

cos 45 = 1/ 2
 𝑖1 ∙ 𝑗𝑜 =
cos 90 = 0
 𝑖1 ∙ 𝑘𝑜 =
cos 45 = 1/ 2
 𝑗1 ∙ 𝑖𝑜 =
cos 45 = 1/ 2
 𝑗1 ∙ 𝑗𝑜 =
cos 90 = 0
 𝑗1 ∙ 𝑘𝑜 =
cos 135 = −1/ 2
 𝑘1 ∙ 𝑖𝑜 =
1 1
cos 90 = 0 0
 𝑘1 ∙ 𝑗𝑜 = 2 2
cos 0 = 1 𝑅01 = 1 1
0 −
 𝑘1 ∙ 𝑘𝑜 = 2 2
cos 90 = 0 0 1 0

10

5
 10/13/2023

Single Rotations about Principal Axes

1 0 0 For small angles

 𝑅𝑥,𝜙 = 0 𝑐𝜙 −𝑠𝜙 1 0 0
0 𝑠𝜙 𝑐𝜙 0 1 −𝜙
0 𝜙 1

𝑐𝜃 0 𝑠𝜃 1 0 𝜃
 𝑅𝑦,𝜃 = 0 1 0 0 1 0
−𝑠𝜃 0 𝑐𝜃 −𝜃 0 1

1 −𝜓 0
𝑐𝜓 −𝑠𝜓 0 𝜓 1 0
 𝑅𝑧,𝜓 = 𝑠𝜓 𝑐𝜓 0 0 0 1
0 0 1

11

Similarity Transformation

 If A is a transformation in Σ𝑜 . It may be described in another frame,

Σ1 , say B, as

−𝟏
𝑩 = 𝑹𝟎𝟏 𝑨𝑹𝟎𝟏

 Example: Let there be two frames Σ𝑜 and Σ1 related by

0 0 1
𝑅10 = 0 −1 0
1 0 0
 How to describe 𝐴 = 𝑅𝑦,𝜃 defined in Σ𝑜 as
𝑐𝜃 0 𝑠𝜃
𝐴 = 𝑅𝑦,𝜃 = 0 1 0
−𝑠𝜃 0 𝑐𝜃
in the frame Σ1 as 𝐵?

12

6
 10/13/2023

Similarity Transformation

 Using the similarity transformation

𝐵 = 𝑅1𝑜 −1
𝐴𝑅1𝑜

𝑇
0 0 1 𝑐𝜃 0 𝑠𝜃 0 0 1
𝐵 = 0 −1 0 0 1 0 0 −1 0
1 0 0 −𝑠𝜃 0 𝑐𝜃 1 0 0

0 0 1 𝑠𝜃 0 𝑐𝜃 𝑐𝜃 0 −𝑠𝜃
𝐵 = 0 −1 0 0 −1 0 = 0 1 0 = 𝑅𝑦,−𝜃
1 0 0 𝑐𝜃 0 −𝑠𝜃 𝑠𝜃 0 𝑐𝜃

13

Consecutive Rotations

 With respect to the current (body) frame

 First, Σ𝑜 is rotated Σ1 is obtained
 Then, Σ1 is rotated to get Σ2
𝑅1𝑜 𝑅21
𝑝𝑜 𝑝1 𝑝2
𝑝𝑜 = 𝑅1𝑜 𝑝1
𝑝1 = 𝑅21 𝑝2
So
𝑝𝑜 = 𝑅1𝑜 𝑅21 𝑝2
Thus
𝑅20 = 𝑅1𝑜 𝑅21
𝑅2𝑜
𝑝𝑜 𝑝2

14

7
 10/13/2023

Fixed-Frame Rotations

 Rotation with respect to the fixed frame

 First, Σ𝑜 is rotated by its axis and Σ1 is obtained
 Then, Σ1 is rotated with respect to fixed frame Σ𝑜 to get Σ2
 Recall, the similarity xform enables us to represent the
second transform 𝐴(Σ0 ) in Σ1 as
𝑅10 −1 𝐴(Σ0 )𝑅10

15

Fixed-Frame Rotations Applied

 First, 𝑅10 = 𝑅𝑦𝑜 ,𝜙

 Then, we want to apply another xform defined in Σ0 fixed-frame
𝐴 Σ0 = 𝑅𝑧0 ,𝜃
to the new current-frame
 Second transform in Σ1 frame (using the similarity xform)
𝑅10 −1 𝐴 Σ0 𝑅10
 Now, we may construct two consecutive rotations as
𝑅20 = 𝑅10 𝑅10 −1 𝐴 Σ0 𝑅10

16

8
 10/13/2023

 So

𝑅20 = 𝐴 Σ0 𝑅10

𝑅20 = 𝑅𝑧0 ,𝜃 𝑅𝑦𝑜,𝜙

𝑛𝑑 𝑠𝑡
𝑅20 = 𝑅2 Σ0 ⋅ 𝑅1 (Σ0 )

 Therefore, rotations in the fixed frame are post multiplied

𝑹𝟎𝟐 = 𝑹𝟐𝟏 𝑹𝟎𝟏

17

18

9
 10/13/2023

Orientation Parameterization

 Rotation by Euler Angles

 Three successive orthogonal rotations about principal axis are
sufficient to represent rotation between frames
 There are 12 possibilities: XYZ, XYX, XZX, XZY,YXY, YXZ, etc.
 Recall two consecutive rotations about the same axis is
equivalent to a single rotation

19

Roll, Pitch, Yaw Angles

Three consecutive rotations on fixed-frame:

1. Yaw 𝑥, 𝜓
2. Pitch 𝑦, 𝜃
3. Roll 𝑧, 𝜙

Recall: Yaw-Pitch-Roll w.r.t. the fixed frame equivalent to

Roll-Pitch-Yaw w.r.t. the current frame

20

10
 10/13/2023

Roll, Pitch, Yaw Angles

 Note that for ZYX Euler angles for small angles Directional Cosine Matrix

(DCM) approximation become

𝑐𝜙 𝑐𝜃 −𝑠𝜙 𝑐𝜓 + 𝑐𝜙 𝑠𝜃 𝑠𝜓 𝑠𝜙 𝑠𝜓 + 𝑐𝜙 𝑠𝜃 𝑐𝜓 1 −𝜙 𝜃
𝑠𝜙 𝑐𝜃 𝑐𝜙 𝑐𝜓 + 𝑠𝜙 𝑠𝜃 𝑠𝜓 −𝑐𝜙 𝑠𝜓 + 𝑠𝜙 𝑠𝜃 𝑐𝜓 = 𝜙 1 −𝜓
−𝑠𝜃 𝑐𝜃 𝑠𝜓 𝑐𝜃 𝑐𝜓 −𝜃 𝜓 1

 sin 𝛼 = 𝛼, cos 𝛼 = 1, and higher order terms negligible

 6 trigonometric function calls and 16 floating point multiplications may be

avoided for small angles when the CPU power is limited

21

Skew-Matrix Operator ∙

 With 𝐚 the skew-symmetric matrix (A) operator defined as

𝑎𝑥 0 −𝑎𝑧 𝑎𝑦
A = 𝐚 = 𝑎𝑦 = 𝑎𝑧 0 −𝑎𝑥
𝑎𝑧 −𝑎𝑦 𝑎𝑥 0

 Remarks:
1) Vector multiplication of 𝑣1 , 𝑣2 ∈ ℝ3
𝑣1 × 𝑣2 = 𝑉1 𝑣2
Becomes a matrix multiplication with
𝑉1 = 𝑣1
2) 𝑣1 × 𝑣2 = −𝑣2 × 𝑣1

22

11
 10/13/2023

Roll, Pitch, Yaw Angles

 Define
𝜓
𝚽= 𝜃
𝜙

 Then, DCM approximation is

1 −𝜙 𝜃
𝜙 1 −𝜓 = 𝐼 + 𝚽
−𝜃 𝜓 1

23

Some Properties of (Skew Symmetric) Matrices

24

12
 10/13/2023

Other Parameterizations of Orientation

𝑟11 𝑟12 𝑟13

𝑅 = 𝑟21 𝑟22 𝑟23
𝑟31 𝑟32 𝑟33
 R may also be parameterized by a single rotation of 𝜃 about
a vector 𝑘
𝑟11 +𝑟22 +𝑟33 −1
 𝜃 = cos −1 2

𝑟32 − 𝑟23
1
 𝑘= 𝑟13 − 𝑟31
2 sin 𝜃
𝑟21 − 𝑟12

25

Rodriguez Formula

 Rodriguez formula gives the rotation matrix if Σ𝑜 rotated by 𝜃

about an arbitrary unit vector 𝑘 (inverse of prev. slide)

𝑅1𝑜 = 𝐼 ⋅ cos 𝜃 + 𝑘 ⋅ sin 𝜃 + 𝑘 ⋅ 𝑘 𝑇 1 − cos 𝜃

26

13
 10/13/2023

Example

This is the total rotation matrix

𝑟11 + 𝑟22 + 𝑟33 − 1 −1

𝜃 = cos −1 = cos = 120𝑜
2 2

1
The same rotation is obtained by 1 3
𝑟32 − 𝑟23
A single rotation about a unit vector 𝑘 1
𝑟13 − 𝑟31 =
1 −
𝑘= 2 2
2 sin 𝜃 𝑟 − 𝑟 3
21 12 1 3
+
2 2

27

Example

 Find the rotation matrix of a rotation of 120𝑜 about unit vector

 Using 𝑅 = 𝐼 ⋅ cos 𝜃 + 𝑘 ⋅ sin 𝜃 + 𝑘 ⋅ 𝑘 𝑇 1 − cos 𝜃

1 1 1 1 1
− +2
3 6 2 3 6 3
𝑇 1 1 1 1 1
 Substituting 𝑘 ⋅ 𝑘 = 6
− 2 3 3
− 2 3
−6 results in
1 1 1 1 1
+ −6 +
6 2 3 3 2 3

 𝑅= as expected from the prev. page

28

14
 10/13/2023

Example in Python

 Rodriguez formula with 𝜃 = 120, 𝑘 = [.577 −.211 .789]

𝑅1𝑜 = 𝐼 ⋅ cos 𝜃 + 𝑘 ⋅ sin 𝜃 + 𝑘 ⋅ 𝑘 𝑇 1 − cos 𝜃
import numpy as np
th=120*np.pi/180
0.00 -0.87 0.50
ct=np.cos(th)
0.50 -0.43 -0.75
st=np.sin(th)
0.87 0.25 0.43
sq3=np.sqrt(3)
I=np.eye(3)
k=np.array([1, 0.5-sq3/2, .5+sq3/2]/sq3).reshape(3,1)
def skew_mat(v):
V=np.array([[0,-v[2],v[1]],
[v[2],0,-v[0]],
[-v[1],v[0],0]],dtype=float)
return V
R=I*ct+skew_mat(k)*st+np.dot(k,k.T)*(1-ct)
for row in R:
print('%6.2f %6.2f %6.2f' %(row[0],row[1],row[2]))

29

Quaternions

 Invented by W.R. Hamilton in 1843

 A quaternion carry 3dof info in 4D complex vector
 In numerical implementations of orientation, they are most
useful
 They may be derived from an angular rotation about a unit
vector in 3D
𝑞 = 𝑠, 𝑣
With 𝑠 ∈ ℝ, v ∈ ℝ3

30

15
 10/13/2023

Quaternions

𝑞 = 𝑞𝑜 , 𝑞1 , 𝑞2 , 𝑞3 = 𝑞𝑜 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3

As in complex numbers, we have
𝑖2 = 𝑗2 = 𝑘2 = 1
𝑖 = 𝑗𝑘 = −𝑘𝑗
𝑗 = 𝑘𝑖 = −𝑖𝑘
𝑘 = 𝑖𝑗 = −𝑗𝑖

31

Quaternion Multiplication

 Let 𝑞 and 𝑞′ be two quaternions, then

𝑞𝑞 ′ = (𝑞𝑜 + 𝑖𝑞1 + 𝑗𝑞2 + 𝑘𝑞3 )(𝑞𝑜′ + 𝑖𝑞1′ + 𝑗𝑞2′ + 𝑘𝑞3′ )

 With a short representation

𝑞𝑞 ′ = 𝑠, 𝑣 𝑠 ′ , 𝑣 ′

𝑠𝑠′ − 𝑣 ⋅ 𝑣 ′ , 𝑠𝑣 ′ + 𝑠 ′ 𝑣 + 𝑣 × 𝑣 ′

If they are rotation, then 𝑞𝑞 ′is equivalent to rotation of 𝑞 ′ followed by

32

16
 10/13/2023

Quaternions as Rotations

 To represent a 𝜃 rotation about unit vector 𝑘 using quaternion

𝜃 𝜃 𝜃 𝜃
𝑞 = cos , 𝑘1 sin , 𝑘2 sin , 𝑘3 sin
2 2 2 2
 So that 𝑞 is also a unit vector
 In a short syntax
𝜃 𝜃
𝑞 = cos , 𝒌 sin
2 2

33

Quaternion to R Matrix

 A rotation may be represented by a quaternion 𝑞

𝑝𝑜 = 𝑅1𝑜 𝑝1 = 𝑞𝑝1 𝑞 −1

 A rotation given by 𝑞 quaternion, the R matrix is

1 − 2𝑞22 − 2𝑞32 2𝑞1 𝑞2 − 2𝑞𝑜 𝑞3 2𝑞1 𝑞3 + 2𝑞1 𝑞2

𝑅1𝑜 = 2𝑞1 𝑞2 + 2𝑞𝑜 𝑞3 1 − 2𝑞12 − 2𝑞32 2𝑞2 𝑞3 − 2𝑞𝑜 𝑞1
2𝑞1 𝑞3 − 2𝑞𝑜 𝑞2 2𝑞2 𝑞3 + 2𝑞𝑜 𝑞1 1 − 2𝑞12 − 2𝑞22

34

17
 10/13/2023

R matrix to Quaternion

 If you get the trace of R matrix written in terms of quaternions

𝑇𝑟{𝑅} = 𝑟11 + 𝑟22 + 𝑟33 = 4𝑞𝑜2 − 1
So, one gets the
1
𝑞𝑜 = 𝑇𝑟 + 1
2
 Use 𝑟11 to get
𝑟11 = 1 − 2 𝑞22 + 𝑞32 = 1 − 2(1 − 𝑞𝑜2 − 𝑞12 )

1
𝑞1 = 2𝑟11 + 1 − 𝑇𝑟
2

35

 Similarly using, 𝑟22 and 𝑟33 we get the rest of the terms

1
𝑞2 = 2𝑟22 + 1 − 𝑇𝑟
2
1
𝑞3 = 2𝑟33 + 1 − 𝑇𝑟
2

36

18
 10/13/2023

Python Rotation Library

 class scipy.spatial.transform.Rotation
 provides an interface to initialize from and represent rotations
with:
 Quaternions
 Rotation Matrices
 Rotation Vectors
 Modified Rodrigues Parameters
 Euler Angles

37

import numpy as np
from scipy.spatial.transform import Rotation as R
r = R.from_quat([0, 0, np.sin(np.pi/4), np.cos(np.pi/4)])
r.as_matrix()
r.as_euler('zyx', degrees=True)

38

19
 10/13/2023

Angular Velocity Vector

 Angular velocity of k frame relative to m frame given in p

frame is

Vector mapping Matrix mapping

39

Least Squares Method

 Measurement of variable 𝑥 vector is 𝑧 with

𝑧 = 𝐻𝑥 + 𝜀
 An optimal estimate of the true 𝑥 may be derived by LSM
2 𝑇
Minimize 𝑧 − 𝐻 𝑥ො = 𝑧 − 𝐻 𝑥ො 𝑧 − 𝐻 𝑥ො
 Explicitly
2
𝑧 − 𝐻 𝑥ො = 𝑧 𝑇 𝑧 − 𝑧 𝑇 𝐻 𝑥ො − 𝑥ො 𝑇 𝐻 𝑇 𝑧 + 𝑥ො 𝑇 𝐻 𝑇 𝐻𝑥ො
 Minimizing w.r.t. 𝑥ො requires the partials w.r.t. 𝑥ො
 Observing the facts (vector gradient as row vector convention)

40

20
 10/13/2023

Least Square Method

2 = 𝑧 𝑇 𝑧 − 𝑧 𝑇 𝐻𝑥ො − 𝑥ො 𝑇 𝐻 𝑇 𝑧 + 𝑥ො 𝑇 𝐻 𝑇 𝐻𝑥ො
 We have 𝑧 − 𝐻 𝑥ො

 Finally the solution is obtained by equating it to zero

2 −𝑧 𝑇 𝐻 + 𝑥ො 𝑇 𝐻 𝑇 𝐻 = 0𝑇 ⇒ −𝐻 𝑇 𝑧 + 𝐻 𝑇 𝐻 𝑥ො = 0
 Hence the estimate through LSM
𝑥ො = 𝐻 𝑇 𝐻 −1
𝐻𝑇 𝑧
 If 𝐻 𝑇 𝐻 is invertible

41

Rotation, Translation, Homogeneous Transform

 As discussed, rotational motion may be represented thru

orthonormal rotation matrix R

 Translation is just a vector translating the origin from before to

after the motion by 𝑡Ԧ = 𝑂𝑜 𝑂1

 Thus, a motion with a translation and rotation may be

[𝑅10 𝑡𝑜 ]

42

21
 10/13/2023

 To make matrix operation friendly form an augmented

homogeneous matrix for total motion
𝑅𝑜 𝑡 𝑜
𝑇1𝑜 = 1′
0 1
 with 0′ n-dimensional row column matrix
 Any point in 2D or 3D space may be mapped from TF1 to TF0
coordinate frames
𝑥 𝑥
𝑜 𝑜
𝑅1 𝑦 𝑅1 3×3 𝑦
or
𝑧
0 0 1 3×3 0 0 0 1 4×4

43

Inverse Motion

 For
𝑅1𝑜 𝑡𝑜
𝑇1𝑜 =
0′ 1
 Inverse is
𝑅1𝑜 𝑇
− 𝑅1𝑜 𝑇 𝑡 𝑜
𝑇1𝑜 −1
= 𝑇01 =
0′ 1
 Proof
 Simply show 𝑇1𝑜 −1 0
𝑇1 = 𝐼4×4

𝑅1𝑜 𝑇
− 𝑅1𝑜 𝑇 𝑡 𝑜 𝑅1𝑜 𝑡𝑜 𝐼3×3 0
= = 𝐼4×4
0′ 1 0′ 1 0′ 1

44

22
 10/13/2023

Example

Camera mounted at a known position measures base and block

End-effector is measured by forward kinematics of robot arm
Compute the block position and orientation w.r.t. camera frame for
motion planning and control

45

 𝑇𝑐𝑒 =?
 𝑇𝑎𝑏 𝑇𝑏𝑐 𝑇𝑐𝑒 = 𝑇𝑎𝑑 𝑇𝑑𝑒
 Base not measurable w.r.t. {a} but 𝑇𝑎𝑏 = 𝑇𝑎𝑑 𝑇𝑑𝑏
 𝑇𝑎𝑑 𝑇𝑑𝑏 𝑇𝑏𝑐 𝑇𝑐𝑒 = 𝑇𝑎𝑑 𝑇𝑑𝑒
−1
 𝑇𝑐𝑒 = 𝑇𝑎𝑑 𝑇𝑑𝑏 𝑇𝑏𝑐 𝑇𝑎𝑑 𝑇𝑑𝑒

46

23
 10/13/2023

47

Derivative of Rotation Matrix

 Let 𝑅(𝜃) ∈ 𝑆𝑂(3). Then, by definition

𝑅 𝜃 𝑅 𝜃 𝑇=𝐼
 Let’s differentiate this equation w.r.t. 𝜃
𝑑𝑅 𝑑𝑅 𝑇
𝑅 𝜃 𝑇 +𝑅 𝜃 =0
𝑑𝜃 𝑑𝜃
 Define
𝑑𝑅
𝑆≝ 𝑅 𝜃 𝑇
𝑑𝜃
 Derivative equation satisfy SSM definition, i.e.
𝑆 + 𝑆𝑇 = 0
 Hence, 𝑆 is a SSM. Multiply 𝑆 by 𝑅 from right
𝑑𝑅
= 𝑆𝑅(𝜃)
𝑑𝜃

48

24
 10/13/2023

Angular Velocity

 Angular velocity about an arbitrary non-stationary axis, we have

𝑅 = 𝑅 𝑡 ∈ 𝑆𝑂(3)
 Differentiating 𝑅 with respect to 𝑡 rather than 𝜃 yields
𝑑𝑅
= 𝑆(𝑡)𝑅(𝑡)
𝑑𝑡
 Recall, 𝑆(𝑡) was an SSM and was also defined as Ω = 𝜔 , implies that
there exists a vector say 𝜔(𝑡) such that
𝑆 𝑡 =𝑆 𝜔 𝑡 =Ω
 This 𝝎(𝒕) happens to be the angular velocity vector of a moving frame
w.r.t. stationary frame and
𝑅ሶ 𝑡 = Ω𝑅(𝑡)

49

Time Derivative of Rotation Matrix

 With the coordinate frames specified

𝑅ሶ 𝑘𝑚 = Ω𝑚 𝑚
𝑚𝑘 𝑅𝑘 (a)
𝑚
 Using the similarity transformation for 𝛺𝑚𝑘 may be related to
the k-frame as
Ω𝑚 𝑚 𝑘 𝑚
𝑚𝑘 = 𝑅𝑘 Ω𝑚𝑘 𝑅𝑘
−1

 Substitute Ω𝑚
𝑚𝑘 in (a)

𝑅ሶ 𝑘𝑚 = 𝑅𝑘𝑚 Ω𝑘𝑚𝑘 (b)

50

25
 10/13/2023

Derivative of Position Vector in Inertial Frame

 Let a position vector measured in the body frame 𝑟 𝑏

 Thus, 𝑟 𝑖 = 𝑅𝑏𝑖 𝑟 𝑏
 Time derivative is

 Using (b)

 Arranging it
(c)
 Coriolis equation

51

Time Derivative of Velocity Vector

 One more derivative of the linear velocity

𝑟ሶ 𝑖 = 𝑅𝑏𝑖 (𝑟ሶ 𝑏 + Ω𝑏𝑖𝑏 𝑟 𝑏 )
 Yields
𝑟ሷ 𝑖 = 𝑅ሶ 𝑏𝑖 (𝑟ሶ 𝑏 + Ω𝑏𝑖𝑏 𝑟 𝑏 ) + 𝑅𝑏𝑖 𝑟ሷ b + 𝑅𝑏𝑖 Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏
 Substitute 𝑅ሶ 𝑏𝑖 from (b)
𝑟ሷ 𝑖 = 𝑅𝑏𝑖 Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + 𝑅𝑏𝑖 Ω𝑏𝑖𝑏 Ω𝑏𝑖𝑏 𝑟 𝑏 + 𝑅𝑏𝑖 𝑟ሷ b + 𝑅𝑏𝑖 Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + 𝑅𝑏𝑖 Ω𝑏𝑖𝑏 𝑟ሶ 𝑏
 Arranging yields linear acceleration measured on b-frame and
inertial acceleration for Newton’s law
𝑟ሷ 𝑖 = 𝑅𝑏𝑖 (𝑟ሷ b + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ω𝑏𝑖𝑏 Ω𝑏𝑖𝑏 𝑟 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 )

52

26
 10/13/2023

𝑟ሷ 𝑖 = 𝑅𝑏𝑖 (𝑟ሷ b + 2Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + Ω𝑏𝑖𝑏 Ω𝑏𝑖𝑏 𝑟 𝑏 )

 where

𝑟ሷ 𝑖 = 𝑅𝑏𝑖 𝑟ሷ b + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + 𝑅𝑏𝑖 Ω𝑏𝑖𝑏 𝑅𝑖𝑏 rሶ 𝑖

𝑟ሷ 𝑖 = 𝑅𝑏𝑖 𝑟ሷ b + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + Ω𝑖𝑖𝑏 𝑣 𝑖
𝑣ሶ 𝑏 = Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 𝑟ሷ 𝑖 = 𝑅𝑏𝑖 𝑣ሶ 𝑏 + Ω𝑖𝑖𝑏 𝑣 𝑖

𝑖
𝑟ሷ 𝑖 = 𝑣ሶ 𝑖 + 𝜔𝑖𝑏 × 𝑣𝑖

53

Derivative of Position Vector in Inertial Frame

 Let a position vector measured in the body frame 𝑟 𝑏

 Thus, 𝑟 𝑖 = 𝑅𝑏𝑖 𝑟 𝑏
 Time derivative is

 Using (b)

 Arranging it
(c)
 Coriolis equation

54

27
 10/13/2023

Time Derivative of Velocity Vector

 One more derivative of the linear velocity (acceleration)

𝑟ሶ 𝑖 = 𝑅𝑏𝑖 (𝑟ሶ 𝑏 + Ω𝑏𝑖𝑏 𝑟 𝑏 )
 Yields
𝑟ሷ 𝑖 = 𝑅ሶ 𝑏𝑖 (𝑟ሶ 𝑏 + Ω𝑏𝑖𝑏 𝑟 𝑏 ) + 𝑅𝑏𝑖 𝑟ሷ b + 𝑅𝑏𝑖 Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏
 Substitute 𝑅ሶ 𝑏𝑖 from (b)
𝑟ሷ 𝑖 = 𝑅𝑏𝑖 Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + 𝑅𝑏𝑖 Ω𝑏𝑖𝑏 Ω𝑏𝑖𝑏 𝑟 𝑏 + 𝑅𝑏𝑖 𝑟ሷ b + 𝑅𝑏𝑖 Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + 𝑅𝑏𝑖 Ω𝑏𝑖𝑏 𝑟ሶ 𝑏
 Arranging yields linear acceleration measured on b-frame and
inertial acceleration for Newton’s law
𝑟ሷ 𝑖 = 𝑅𝑏𝑖 (𝑟ሷ b + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ω𝑏𝑖𝑏 Ω𝑏𝑖𝑏 𝑟 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 )

55

𝑟ሷ 𝑖 = 𝑅𝑏𝑖 (𝑟ሷ b + 2Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + Ω𝑏𝑖𝑏 Ω𝑏𝑖𝑏 𝑟 𝑏 )

 where

𝑣ሶ 𝑏 = Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏
𝑟ሷ 𝑖 = 𝑅𝑏𝑖 𝑟ሷ b + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + (𝑅𝑏𝑖 Ω𝑏𝑖𝑏 𝑅𝑖𝑏 )rሶ 𝑖
𝑟ሷ 𝑖 = 𝑅𝑏𝑖 𝑟ሷ b + Ω𝑏𝑖𝑏 𝑟ሶ 𝑏 + Ωሶ 𝑏𝑖𝑏 𝑟 𝑏 + Ω𝑖𝑖𝑏 𝑣 𝑖
𝑟ሷ 𝑖 = 𝑅𝑏𝑖 𝑣ሶ 𝑏 + Ω𝑖𝑖𝑏 𝑣 𝑖
𝑖
𝑟ሷ 𝑖 = 𝑣ሶ 𝑖 + 𝜔𝑖𝑏 × 𝑣𝑖

56

28
 INTELLIGENT CONTROL OF
ROBOT MANIPULATORS
MKT 4846 Dr. Aydin Yesildirek

Intelligence

 Although it is difficult, the intelligence may roughly be

defined as

the ability to increase the probability of survival or success in

a changing environment

 Requires to have
1. Objectives and goals– to stay alive or achieve a task
2. Sensing mechanism
3. Metric to measure the level of success
4. Autonomous actions

1
Form of Intelligence

 Multidimensional
 Social intelligence
 Fine arts intelligence
 Etc.
 Individual intelligence
 Cooperative intelligence
 Swarm intelligence
 Network intelligence
 Structural intelligence
 Evolution
 Genetics

Human Intelligence

 Human intelligence is gained by

the nervous system that is
composed of neurons connected by
synapses
 Human brain only has about 86
billion neurons
 Complete understanding of its
operation is not known
 Simplistic explanations of learning:
D. Hebb 1949
◼ Axon modification by pre and
post-synaptic activities

2
 Nervous System

 Simplified nervous system building block is shown

 Current understanding of learning and the nervous system reveals
 All nucleus propagates its inputs the same squashing manner
 Learning activity strengthens the axons
 Different artificial neural network structures mimicking this operation
proposed by many

Artificial Intelligence (AI)

 Most intiuitive approach to build an AI system is to mimic the

natural intelligence in
 Structures
 Learning processes
 For instance
 Artificial neural networks (ANN)
 Knowledge based systems
◼ Expert systems
◼ Fuzzy logic systems
 Genetics and evolution
 Swarm / distributed systems
 Etc.

3
Learning Process

 Simplest form of intelligence may be obtained by

 Given a metric for a performance defining the success
 Cognition to measure the environment and self

 Describe the driving mechanism and structure to be

successful

 System ID and adaptive control systems may be

considered as a low level of IS

System ID

 For example, the system of interest

𝑥ሶ = −𝑎𝑥 + 𝑏𝑢, 𝑥 0 = 𝑥𝑜
How to learn the unknown parameters of 𝑎 and 𝑏 using the measurements
of
𝑦=𝑥
 Define a performance index, e.g.,
2
𝐽 = 𝑎 − 𝑎ො 2
+ b − 𝑏෠

 Unfortunately can’t be used since it isn’t measureble

 Indirect but measurable performance index may be
𝐽 = 𝜖12 = 𝑦 − 𝑦ො 2

 Least square error offers a minimum solution based on measurements

4
Learning Process – Min Search

 Mimimum search techniques

 Search for min 𝐽 = 𝜖12 = 𝑦 − 𝑦ො 2
for parameter vector
𝑝
𝑝 = 𝑎ො 𝑏෠ 𝑇
 Gradient descent methods searches for the local minima by
learning rules of
𝑝𝑘+1 = 𝑝𝑘 − 𝜂𝛻𝑝 𝐽(𝑥, 𝑝)
◼𝜂 > 0 a small step size (better be 𝜂𝑘 for efficiency)
◼ 𝛻𝑝 𝐽 the gradient of 𝐽 along 𝑝
◼ Very slow learning process
◼ Solution is local
−𝜂𝛻𝐽

𝑥∗ 𝑥𝑘+1 𝑥𝑘

Learning – Lyapunov Technique

 Define a candidate signal (LFC) using an error space vector, 𝑒, including all
parametric and dynamic errors, such that
𝑉 𝑡, 𝑒 > 0, ∀𝑡 ≥ 0, ∀ 𝑒 ≠ 0
𝑉 𝑡, 𝑒 = 0, ∀𝑡 ≥ 0, 𝑒 = 0
e.g., energy functions in error space
 Study the time derivative
ሶ 𝑒)
𝑉(𝑡,
 If you can find an input signal and learning algorithms such that
𝑉ሶ < 0, ∀𝑡 ≥ 0, ∀ 𝑒 ≠ 0
 The positive signal with negative derivative will always decrease till zero
lim 𝑉 𝑡, 𝑒 = 0 ⇒ lim 𝑒 𝑡 = 0
𝑡→∞ 𝑡→∞

Very simple reasoning

5
Intelligent Structure

 Define the estimation model as

𝑥ොሶ = −𝑎ො 𝑥ො + 𝑏𝑢,
෠ 𝑥ො 0 = 𝑥ො𝑜

𝑦ො = 𝑥ො

 Note that for any desired 𝑎𝑚 > 0 adding zero

𝑎𝑚 − 𝑎𝑚 𝑥 = 0 or 𝑎𝑚 − 𝑎𝑚 𝑥ො = 0

 One gets the system and estimation model as

𝑥ሶ = −𝑎𝑚 𝑥 + 𝑎𝑚 − 𝑎 𝑦 + 𝑏𝑢
𝑥ොሶ = −𝑎𝑚 𝑥ො + 𝑎𝑚 − 𝑎ො 𝑦ො + 𝑏𝑢
෠

Linear-in-the-Paramater Representation

 System output may be expressed as (linear-in-the-parameters)

𝑦 = 𝜃 𝑇 𝜙 𝑢, 𝑦
Where the parameter and the regression vectors

1
𝑢
𝑏 𝑠 + 𝑎𝑚
𝜃= 𝜙 𝑢, 𝑦 =
𝑎𝑚 − 𝑎 1
𝑦
𝑠 + 𝑎𝑚
 Similarly the estimation model output
𝑦ො = 𝜃መ 𝑇 𝜙 𝑢, 𝑦
With the parameter estimates

1
𝑢
𝑏෠ 𝑠 + 𝑎𝑚
𝜃መ = 𝜙 𝑢, 𝑦ො =
𝑎𝑚 − 𝑎ො 1
𝑦ො
𝑠 + 𝑎𝑚

6
Error Systems Dynamics

 Define the output measurement error as

𝜖1 = 𝑦 − 𝑦ො

 Error dynamics may be written as

෨
𝜖1ሶ = −𝑎𝜖1 + 𝑎෤ 𝑥ො − 𝑏𝑢

 𝑎෤ ≜ 𝑎ො − 𝑎 and 𝑏෨ ≜ 𝑏෠ − 𝑏 Unknown 𝑎 may have any value, get rid of it by

𝜖1ሶ = −𝑎𝑚 𝜖1 + 𝑎𝑦 ෨
෤ − 𝑏𝑢

 Observe that the error dynamics rewritten in another form that

- The transient behavior controlled by us (good term, 𝑎𝑚 instead of unknown 𝑎)
- All terms are either measurable signals/errors or parametric signals/errors

Lyapunov Based Learning Algorithm

 Define an LFC on error space

1 2 1 2 1 2
𝑉(𝑡, 𝑒) = 𝜖 + 𝑎෤ + 𝑏෨ > 0, ∀𝑡 ≥ 0
2 1 𝛾 𝛾
With 𝑒 = [𝜖, 𝑎, ෨
෤ 𝑏]
 Express the time derivative along the dynamics in friendly format
𝑉ሶ = −𝑎𝑚 𝜖12 + 𝑎𝑥𝜖 ෨ 1 + 𝑎𝑓
෤ 1 − 𝑏𝑢𝜖 ෨ 𝑏
෤ 𝑎 + 𝑏𝑓
𝑓𝑎 𝑓𝑏
𝑉ሶ = −𝑎𝑚 𝜖12 + 𝑎෤ + 𝑥𝜖1 + 𝑏෨ − 𝑢𝜖1
𝛾 𝛾
The best can do
ෝሶ = 𝒇𝒂 = −𝜸𝒙𝝐𝟏
𝒂
෡ሶ = 𝒇𝒃 = 𝜸𝒖𝝐𝟏
𝒃
So that
𝑉ሶ = −𝑎𝑚 𝜖12 ≤ 0

Hence, 𝑉 ∈ 𝐿∞ implies 𝜖1 , 𝑎, ෤ 𝑏෨ ∈ 𝐿∞ (bounded), ultimately stable.

In addition, since 𝑉ሷ = −2𝑎𝑚 𝜖1𝜖1ሶ < ∞ invoking the Barbalat’s lemma gives us more, 𝜖1 → 0
as 𝑡 → ∞.

7
  The learning rules 𝑎ොሶ = −𝛾𝑥𝜖1 and 𝑏෠ሶ = 𝛾𝑢𝜖1 are still in the
form of multiplication of
 Excitation input signal to the parameter
 An error signal of interest

 This learning algorithm suits to control, bringing the output

tracking error to zero
 Parametric errors do not blow out but not necessarily go to
zero
 May not learn the correct parameters
 It provides an unsupervised online learning mechanism

𝑎ොሶ = 𝑓1 = −𝛾𝑥𝜖1
𝑏෠ሶ = 𝑓2 = 𝛾𝑢𝜖1

8
 Parametric convergence possible with a good selection of
input

9
 Remarks on Lypaunov Based Techniques

 For a given input, if the actual parameters to be learnt

are not known (unsupervised learning)
 Using an auxiliary error signal may help
 Persistent excitation gains importance

 In a control system, main reason is to shape up the overall

system response to commands that is achieved, not to
learn some unknown parameters

Artificial Neural Network Structures

 Start with a single neuron

 Develop a simplistic network of neurons

10
Multilayer Perceptron Structure

 Inputs: xi’s
 Outputs: yj’s
 Neurons have the same
activation functions of
 Squashing functions:
 Sigmoidal
 Radial Basis function
 Hyper tangent fnc
 Wavelets
 Weights indicate interneuron
strength and modified during
learning

Activation Functions 𝜎(⋅)

 Squashing functions: R → [𝑎, 𝑏]

 Sigmoid (Logistic) function: 𝑎, 𝑏 = [0, 1]
1
 𝜎 𝑥 =
1+𝑒 −𝑥

 Hyperbolic tangent function: 𝑎, 𝑏 = [−1, 1]

1−𝑒 −𝑥 𝑒 𝑥 −𝑒 −𝑥
 𝜎 𝑥 = or 𝜎 𝑥 =
1+𝑒 −𝑥 𝑒 𝑥 +𝑒 −𝑥
𝜋 𝜋
 Arctan(x) function: 𝑎, 𝑏 = [− , ]
2 2
 𝜎 𝑥 = atan 𝑥
 Gaussian function: 𝑎, 𝑏 = (0, 1]
2
 𝜎 𝑥 = 𝑒 −𝑥
 Sinc function: 𝑎, 𝑏 = [−0.271, 1]
sin 𝑥
 𝜎 𝑥 = for 𝑥 ≠ 0 and 𝜎 0 = 1
𝑥

11
 Non-squashing activations
 Linear 𝜎 𝑥 = 𝑥
 Rectified Linear Unit (ReLU) 𝜎 𝑥 = max{0, 𝑥}
−𝑥
 Softplus 𝜎 𝑥 = ln(1 + 𝑒 )
 Maxout Linear 𝜎 𝑥 = max 𝑎𝑖 𝑥 + 𝑏𝑖
𝑖∈{1,…,𝑘}

Recurrent Neural Network

Feedforward NN

𝒛−𝟏 𝒛−𝟏 𝒛−𝟏 𝒛−𝟏 𝒛−𝟏 𝒛−𝟏

x

 Dynamic structure is achieved by delayed I/O feedback

 It stores knowledge at its equilibrium (memory function)

12
RNN Details

Single Neuron Model

 For a multilayer feedforward
NN
 The 𝑖 th neuron at the layer 𝑙
 Its output 𝒛𝒍𝒊
 Inputs coming from 𝑙 − 1th layer
outputs, 𝒛𝒍−𝟏
𝒋 , 𝑗 = 1, … , 𝑁𝑙−1
 𝑁𝑙−1 𝐰eights to the neuron 𝒘𝒍𝒊𝒋
accumulates weighted inputs

𝑁𝑙−1

𝛾𝑖𝑙 = ෍ 𝑤𝑖𝑗𝑙 𝑧𝑗𝑙−1

𝑗=1
 The neuron output

𝑧𝑖𝑙 = 𝜎(𝛾𝑖𝑙 )

13
 Biasing

 Since common activation functions squashes the input to a finite band, e.g. −1, 1 , a
DC bias (constant) needed to extend the output range
 To keep the neuron model the same we only augment the input by adding a one
 Augmentation for bias in matrix form

𝑙−1
𝒛𝑙−1 = 𝝈(𝜸 )
𝟏

𝑁𝑙−1 +𝟏

𝛾𝑖𝑙 = ෍ 𝑤𝑖𝑗 𝑧𝑗 ⇒ 𝜸𝑙−1 = 𝑊 𝑙 𝐳 l−1

𝑙 𝑙−1

𝑗=1

 ANN Output, with some abuse of notation,

𝑦 = 𝑊 𝐿 𝜎(𝑊 𝐿−1 𝑧 𝐿−1 )

Input-Output Mapping
28

𝑙−1

 Axon strengths represented by weighted𝑎 inputs

𝑤
𝐰
1

𝑙
𝑙
𝑖1
𝑙
𝑖
𝑎2𝑙−1 𝑤𝑖2
𝑙=1 𝑙=2 𝑙=3 𝑎𝑖𝑙
𝑧𝑖𝑙
𝜎(𝑧𝑖𝑙 )
𝑙
𝑤𝑖𝑗
𝑙
𝑤𝑖𝑁𝑙−1

𝑙
𝑤𝑖0
𝑎𝑎𝑗𝑙−1
𝑙−1
𝑜
𝑙−1
𝑎𝑁 𝑙−1

1
𝑧32 𝜎(𝑧 2 ) 𝜎 𝑧11
3
𝜎(z1 ) = 𝜎 𝑧21
⋮
𝜎 𝑧𝑁1 1
2
𝑤31 𝜃32
2
𝑁1 𝑁1 𝑤31
2
𝑧32 = ෍ 𝑤3𝑖 2
𝜎(𝑧𝑖1 ) + 𝜃32 = ෍ 𝑤3𝑖 𝜎(𝑧𝑖1 ) = 𝐰32𝑇 𝜎(z1 ) 𝐰32 = 𝑤322

𝑖=1 𝑖=0 ⋮
2
𝑤3𝑁 1

14
Three Layer MLP

 For the sake of simplicity input and output neurons are

linear, 𝜎 𝑥 = 𝑥, let the hidden layer be sigmoid

 For notational simplicity we let 1st layer weight matrix V

and 2nd layer weight matrix W and drop the layer index

𝑦 = 𝑊 𝑇 𝜎(𝑉 𝑇 𝐱)

𝐲 = 𝑊 𝑇 𝛔(𝑉 𝑇 𝐱)

 Let 𝑁1 be # of inputs, 𝑁2 # of hidden layer neurons, 𝑁3 # outputs, than the augmented vectors

𝑥1 𝜎1
𝑦1
⋮ ⋮
𝑥𝑖 𝜎𝑗 ⋮
𝐱= ⋮ , 𝛔= ⋮ , 𝑦
𝐲= 𝑘
⋮
𝑥𝑁1 𝜎𝑁2 𝑦𝑁3
1 1

𝑁1 +1

𝜎𝑗 = 𝜎 ෍ 𝑣𝑗𝑖 𝑥𝑖 𝑗 = 1,2, … , 𝑁2
𝑖=1

𝑁2 +1

𝑦𝑘 = ෍ 𝑤𝑘𝑗 𝜎𝑗 𝑘 = 1,2, … , 𝑁3
𝑗=1

15
Universal Approximation Theorem

 Stone-weistrass theorem applied to MLP shows the approximation

capability of the ANN in a compact set
 Let 𝐼𝑛 represent n-dimensional unit vector hypercube containing any
𝑥𝑖 ∈ 0, 1 , for 𝑖 = 1,2, … , 𝑛 and 𝐶(𝐼𝑛 ) be continuous functions in 𝐼𝑛 .
MLP with sigmoid activation
𝑦 = 𝑊 𝑇 𝜎(𝑉 𝑇 𝑥)
Are dense in 𝐶(𝐼𝑛 ).
 This implies that given any 𝑓 ∈ 𝐶(𝐼𝑛 ) and 𝜀 > 0 there are 𝑊, 𝑉, 𝑁2
such that 𝑓 − 𝑦(𝑥) < 𝜀 for all 𝑥 ∈ 𝐼𝑛
 Any continuous function 𝑓 may be approximated by MLP at any
accuracy within a compact set e.g. 𝐼𝑛 Hornik et al.

 The approximation error depends on neuron number 𝑁2 that is not known in advanced
 For practical implementations pick sufficiently large 𝑁2 if 𝜖 doesn’t satisfy 𝑁2 is increased.
 For a given 𝑁2 , let the ideal MLP be
𝑦 ∗ = 𝑊 ∗ 𝑇 𝜎(𝑉 ∗ 𝑇 𝑥)
with acceptable MLP construction error
𝑦 − 𝑦 ∗ < 𝜖(𝑁2 )

 The approximate model

෡ 𝑇 𝜎(𝑉෠ 𝑇 𝑥)
𝑦ො = 𝑊

 ෠ 𝑊
We now focus on learning the weight estimates 𝑉, ෡ minimizing the reconstruction error
𝑦 ∗ − 𝑦ො

16
Backpropagation Algorithm

 Let the measured I/O samples of the function be

(𝑥𝑘 , 𝑓𝑘 ) for 𝑘 = 1,2, … , 𝑃
and corresponding MLP I/O pairs be
(𝑥𝑘 , 𝑦𝑘 )
 The learning process requires to find the unknown weights minimizing the output error

1 1
𝐸𝑘 = 𝑒𝑘2 = 𝑓𝑘 − 𝑦𝑘 2
2 2
recursively in time (least squared error)
 Gradient descent algorithm

𝜕𝐸𝑘
𝑊𝑘 = 𝑊𝑘−1 − 𝜂 ቤ
𝜕𝑊 𝑊=𝑊
𝑘−1
𝜂 learning rate
 Will get stuck in local minima 

Backpropagation Algorithm

 The gradient descent algorithm applied to MLP may be written in generic form as

𝜕𝐸𝑘 𝜕𝐸𝑘 𝜕𝑧𝑖𝑙

𝑙 =
𝜕𝑤𝑖𝑗 𝜕𝑧𝑖𝑙 𝜕𝑤𝑖𝑗
𝑙
𝑙
For 𝑤𝑖𝑗 at layer 𝑙, 𝑗 th weight coming out of neuron 𝑖
1
 For the sigmoid 𝜎 𝛾 =
1+𝑒 −𝛾

𝜕𝜎 e−𝛾 1 1
= = 1−
𝜕𝛾 1 + e−𝛾 2 1 + 𝑒 −𝛾 1 + 𝑒 −𝛾

𝜕𝜎
= 𝜎(1 − 𝜎)
𝜕𝛾

17
 𝑁𝑙−1

𝑧𝑖𝑙 =𝜎 𝑙 𝑙−1
෍ 𝑤𝑖𝑗 𝑧𝑗
𝑗=0
𝜕𝑧𝑖𝑙
 𝑙 = 𝑧𝑖𝑙 1 − 𝑧𝑖𝑙 𝑧𝑗𝑙−1
𝜕𝑤𝑖𝑗
 Define error vectors of
𝛿 𝐿 = 𝑒𝑘
𝑁𝑙+1

𝛿𝑖𝑙 = ෍ 𝛿𝑗𝑙+1 𝑤𝑗𝑖𝑙+1 𝑧𝑖𝑙 1 − 𝑧𝑖𝑙 𝑙 = 𝐿 − 1, … , 2

𝑗=1

 Backpropagating these errors used in the weight updates by

𝜕𝐸𝑘
𝑙 = 𝛿𝑖𝑙 𝑧𝑗𝑙−1 𝑙 = 𝐿, 𝐿 − 1, … , 2
𝜕𝑤𝑖𝑗

Deep Neural Networks

 Having more than three layers or many nodes in each

layer NOT necessarily improve the network performance

 Input selection to the neural network effects greatly to the

learning and performance of the system

 Raw input may be conditioned or preprocessed for a

better performance

 Distinct features play key roles in both classification and

regression problems

18
Lyapunov Theorem

 To observe stability requirement energy in the error space may

be observed
 Energy is always positive
 If the error space energy is not increasing for all time then those signals
remain bounded
 If the energy in the error space is decreasing then error signals must go
to zero

 Define a lyapunov function candidate 𝑉 𝑒 𝑡 >0

 Check its time derivative wheather it is negative semi definite

𝑉ሶ 𝑡 ≤ 0 ∀𝑡 ≥ 0
→ 𝑒 𝑡 ∈ 𝐿∞

19
Rigid Robot Dynamics

Autonomous Robots MKT 6110

Tracking Problem

Autonomous Robots MKT 6110

20
 Observe that the filtered tracking error
𝑟 = 𝑒ሶ + Λ𝑒

 Represents a system with input r output e as

𝑒ሶ = −Λ𝑒 + 𝑟
 For Λ > 0, PD, if 𝑟 → 0 then 𝑒, 𝑒ሶ → 0
 If 𝑟 ∈ 𝐿∞ then 𝑒 ∈ 𝐿∞ implies 𝑒ሶ ∈ 𝐿∞

Error System Dynamics

 With

 Where

 For a well-behaved 𝑞𝑑 (𝑡)

Autonomous Robots MKT 6110

21
NN Controller

Substituted, results in the error system dynamics

Autonomous Robots MKT 6110

 Taylor series expansion

 For sigmoid and other squashing functions

Autonomous Robots MKT 6110

22
NN Controller for Robot Manipulators

Autonomous Robots MKT 6110

Stability Analysis

Autonomous Robots MKT 6110

23
Autonomous Robots MKT 6110

Autonomous Robots MKT 6110

24
Autonomous Robots MKT 6110

Summary

Autonomous Robots MKT 6110

25
Autonomous Robots MKT 6110

Example

Autonomous Robots MKT 6110

26
 Adaptive controller with unmodelled dynamics

Autonomous Robots MKT 6110

 Neural network controller

Autonomous Robots MKT 6110

27
 12/20/2022

ROBOT CONTROL

Feedback Control

 Feedback control system

𝑟(𝑡) 𝑒(𝑡) 𝑢(𝑡) 𝑦(𝑡)

Controller System

 Robot control

𝑢(𝑡) 𝜏(𝑡) 𝑦(𝑡)

𝑟(𝑡) 𝑒(𝑡) actua
Controller Robot
tor

 Control signal may be

 voltages applied to joint actuators
 Torques/forces applied at the joints

1
 12/20/2022

 The control objective, in general, is to

 The output to follow the given reference signal 𝑟(𝑡)
 In robot control the output may be end-effector’s position, velocity, force,
moment etc.
 Position may be a continuous reference signal or a set of way points
 Types of actuation of joints
 PM DC motor with gear reducer
◼ Motor models are well studied
◼ Gears help to decouple the system but bring backlash, compliance and
friction
 Direct drive robots
◼ Reduces the gear induced problems, friction, backlash, compliance
◼ Harder to model

 Each link will be controlled as SISO system

 Forces/torques coming from other links will be considered as disturbance

 Control objectives are augmented as

 Reference tracking
 Disturbance rejection

2
 12/20/2022

Robot Dynamic Models

 Universal robot dynamic model was given

𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏
 Each element of 𝜏, say 𝜏𝑘 is force/torque input at
prismatic/revolute joint 𝑘 for link 𝑘
 Generalized forces may be produced by
 Electric
 Hydraulic
 Pneumatic
actuators having their own dynamics
 Friction is a reality and not included in this model

DC Motor Actuator Dynamics

Motor model
𝜏𝑚 = 𝐾𝑚 𝑖𝑎
𝑉𝑏 = 𝐾𝑏 𝜔𝑚

3
 12/20/2022

 In practice 𝐿/𝑅 is negligible and a reasonable approx. model for 𝜃𝑚

How about 𝜏𝑚 ?

Set-Point Tracking of DC Motor Actuator

 Constant 𝜃 𝑑 is the control reference for

𝐽𝜃ሷ + 𝐵𝜃ሶ + 𝜏𝑑 = 𝑈

𝜏𝑑 (𝑡)
𝑑 𝑒(𝑡) Compans 𝑢(𝑡) 𝜃(𝑡)
𝑟 𝑡 =𝜃
1 1
ator
𝐽𝑠 + 𝐵 𝑠

 Objective: make 𝜃(𝑡)

 Track desired reference
𝑒 𝑡 = 𝜃 𝑑 − 𝜃 𝑡 → 0 with desired transient as 𝑡 → ∞ when 𝜏𝑑 = 0
 Reject constant (step type) disturbances,
If 𝜏𝑑 𝑡 = 𝐷 then 𝜃 𝑡 → 0 as 𝑡 → ∞ when 𝑟(𝑡) = 0

4
 12/20/2022

Transient Control

 Find negative feedback controller so that the output tracks the reference
with a given combinations of desired
 %Overshoot (%OS)
 Settling time (Ts)
 Peak time (Tp)
 Rise time (Tr)
 These time domain requirements may be transformed to frequency domain
%𝑂𝑆 = 100 × 𝑒 −𝜁𝜋/ 1−𝜁 2

4
𝑇𝑠 ≈
𝜁𝜔𝑛
 The desired closed loop system’s damping ratio and natural frequency may
be achieved by placing the dominant poles at
𝑝1,2 = −𝜁𝜔𝑛 ± 𝜔𝑛 1 − 𝜁 2 ≝ 𝜎𝑑 ± 𝑗𝜔𝑑

Example

 If we want to joint angle 𝜃 follow the constant command with

 At most 4% overshoot
 Settle before 1 sec
 Where the dominant poles of the closed loop control system to be placed?
− ln 0.04
𝜁= = 0.7156
ln2 0.04 + 𝜋 2
4 4
𝜔𝑛 ≥ = = 5.56
𝑇𝑠 𝜁 0.7156
 Dominant poles must be at
𝑝1,2 = −𝜁𝜔𝑛 ± 𝜔𝑛 1 − 𝜁 2 = −4 ± 𝑗3.9
 The characteristic equation
Δ𝑑 𝑠 = 𝑠 + 4 + 𝑗3.9 𝑠 + 4 − 𝑗3.9 = 𝑠 2 + 8𝑠 + 31.24

10

5
 12/20/2022

Example

 For the joint with 𝐽 = 1, 𝐵 = 2 the open-loop reference response to set-

point 𝜃 𝑑 = 1
𝜃ሷ + 2𝜃ሶ + 𝜏𝑑 = 𝑈

>> J=1;B=2;
>> G=tf(1,[J B 0]);
>> step(G)

11

 Let the controller be

𝑈 = 𝑘𝑝 + 𝑘𝐷 𝑠 𝐸
 When 𝜏𝑑 =0

𝑟(𝑡) 𝑒(𝑡) 𝑢(𝑡) 𝑦(𝑡)

𝑘𝑝 + 𝑘𝐷 𝑠 1
𝐽𝑠 2 + 𝐵𝑠

 The closed loop system

𝐶𝐺 𝑘𝑝 + 𝑘𝐷 𝑠 𝑘𝑝 + 𝑘𝐷 𝑠
𝑇𝑐𝑙 = = =
1 + 𝐶𝐺 𝐽𝑠 2 + 𝐵 + 𝑘𝐷 𝑠 + 𝑘𝑃 𝑠 2 + 2 + 𝑘𝐷 𝑠 + 𝑘𝑃
Δ𝑐𝑙 = 𝑠 2 + 8𝑠 + 31.24 = 𝑠 2 + 2 + 𝑘𝐷 𝑠 + 𝑘𝑃
𝑘𝑃 = 31.24 and 𝑘𝐷 = 6

12

6
 12/20/2022

 𝐶 𝑠 = 𝑘𝑝 + 𝑘𝐷 𝑠

>> kp=31.24;kd=6;
>> C=tf([kp kd],1);
>> Tcl=G*C/(1+G*C);
>> step(Tcl)

13

Disturbance effect

𝜏𝑑 (𝑡)
𝑟 𝑡 =0 𝑒(𝑡) 𝑢(𝑡) 𝜃(𝑡)
𝑘𝑝 + 𝑘𝐷 𝑠 1 1
𝑠+2 𝑠

𝜃 𝑠 𝐺 1
= = 2
𝜏𝑑 (𝑠) 1 + 𝐶𝐺 𝑠 + 2 + 𝑘𝐷 𝑠 + 𝑘𝑃

>> H=tf(1,[J B+kd kp]);

>> step(H)
>> title('Step Disturbance Response')

14

7
 12/20/2022

Matlab’s Control Designer GUI

>>sisotool(G) or
>> controlSystemDesigner(G)

15

16

8
 12/20/2022

 Remove Bode diagrams and add design requirements both in

root-locus and I/O response

17

 Step response
Design requirements

18

9
 12/20/2022

 Tuned parameters to some specs shown

Ts needs to be improved, for say

19

 Tuned kp was 2.7538

 Add a zero for PD transient improvement

20

10
 12/20/2022

 Rework with the gain

Before the transient improvement

21

PD Controller

 Compensator
𝐶 𝑠 = 7 𝑠 + 3 = 21 + 7𝑠
 Is the ideal PD controller with gains
𝑘𝑃 = 21, 𝑘𝐷 = 7

22

11
 12/20/2022

Disturbance Analysis for the PD Controller

23

Steady State Response Improvement

 We have a static error at the steady state disturbance response

 This may be improved by a lag controller by N-folds, or
 May be eliminated by an additional integral control, e.g. PI

𝑘𝐼
𝐶 = 𝑘 𝑝 + 𝑘𝐷 𝑠 +
𝑠
2
𝐶𝐺 𝑘𝐷 𝑠 + 𝑘𝑃 𝑠 + 𝑘𝐼
𝑇𝑐𝑙 = = 3
1 + 𝐶𝐺 𝐽𝑠 + 𝐵 + 𝑘𝐷 𝑠 2 + 𝑘𝑝 𝑠 + 𝑘𝐼
 Using the previous PD controller base and adding a PI as
𝑠 + 0.01 0.03𝐾
𝐶 =𝐾 𝑠+3 = 3.01𝐾 + + 𝐾𝑠
𝑠 𝑠
 The transient is preserved and static errors to be improved

24

12
 12/20/2022

PID Controller Gaining Disturbance Rejection

𝐾 𝑠 + 3 𝑠 + 0.01 0.03𝐾
𝐶 𝑠 = = 3.01𝐾 + + 𝐾𝑠
𝑠 𝑠

25

Effects of Saturation and Noise in PID

 In Simulink use tune the PID block

26

13
 12/20/2022

 With precalculated gains, the simulation show step

response

 The controller effort needed

 What if the actuator can only supply +-5?

27

 With saturation

 The suppressed tracking during the transition worsen when the step
reference is changed, reaction slows till excess control signal is trimmed

28

14
 12/20/2022

Drive Train Dynamics

 Harmonic drive gear mechanisms offer

 Low backlash
 High torque transmission
 Excellent gear ratios are possible
 Compact, light weight
 Hence very popular in robot joints

https://www.engineeringclicks.com/harmonic-drive/

29

https://commons.wikimedia.org/wiki/File:Harmonic_drive_animation.gif

30

15
 12/20/2022

Modeling Harmonic Drive

𝜃𝑙 𝑘/𝐽𝑚 𝐽𝑙
=
𝑇𝑚 𝑠 4 + 𝐵1 + 𝐵2 𝑠 3 + 𝑘 1 + 1 + 𝐵1 𝐵2 𝑠 2 + 𝑘 𝐵 + 𝐵 𝑠
𝐽𝑚 𝐽𝑙 𝐽𝑚 𝐽𝑙 𝐽𝑚 𝐽𝑙 𝐽𝑚 𝐽𝑙 1 2

31

PD Control

𝐾𝑝
𝐶 𝑠 = 𝐾𝑑 𝑠 +
𝐾𝑑
 Feedback may come either shafts 𝜃𝑚 or 𝜃𝑙
 When 𝐵1 ~𝐵2 ~0
𝐽𝑚 𝐽𝑙 𝑠 4 + 𝑘 𝐽𝑚 + 𝐽𝑙 𝑠 2
 We have open-loop system poles

1 1
0,0, ±𝑗 𝑘 +
𝐽𝑚 𝐽𝑙

 For typical 𝑘 ≫ 𝐵𝑖
 Two complex conjugate poles; Imaginary part large, real part small
 One pole at the origin and
 Remaining one is a real pole

32

16
 12/20/2022

 Compensator
𝐾𝑝
𝐶 𝑠 = 𝐾𝑑 𝑠 +
𝐾𝑑
 Flexible joint

33

State Space Control

34

17
 12/20/2022

35

36

18
 12/20/2022

Computed Torque control

 The nonlinear rigid robot dynamics

𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏

 With the computed torque input

𝜏 = 𝐷 𝑞 𝑎 + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞

 The closed loop system become

𝐷 𝑞 𝑞ሷ = 𝐷(𝑞)𝑎

 Or

𝑞ሷ = 𝑎

 That is a linear system

37

Reference Joint Space Tracking

 Let the desired differentiable joint space vector be

𝑞𝑑 𝑡 , 𝑞ሶ 𝑑 , 𝑡 𝑞ሷ 𝑑 𝑑
 The reference tracking error
𝑒 = 𝑞𝑑 − 𝑞
 Now the CT auxiliary control signal be made
𝑎 = 𝑞ሷ 𝑑 + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒
The closed loop error system dynamics becomes
𝑒ሷ + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒 = 0

38

19
 12/20/2022

 System
𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏
 Controller
𝜏 = 𝐷 𝑞 [𝑞ሷ 𝑑 + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒] + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞
 For a transient response with maximum rise time without an
overshoot
𝜁 = 1, 𝜔𝑛
Desired poles
𝑠 2 + 2𝜁𝜔𝑛 𝑠 + 𝜔𝑛2 = 𝑠 2 + 2𝜔𝑛 𝑠 + 𝜔𝑛2 = 0
Implies
𝐾𝑝 = 𝜔𝑛2 , 𝐾𝑑 = 2𝜔𝑛

39

Reference Tracking in Cartesian Space

 Given a feasible desired pose of the end effector

𝑇(𝑡) = 𝑅𝑑 𝑡 𝑝𝑑 𝑡
 The inverse kinematics yield (when exists) the joint state
𝑞𝑑 𝑡 = 𝐼𝐾[𝑇 𝑡 ]
 Then apply the joint space controller

40

20
 12/20/2022

Friction

 Nonlinear and discontinuous phenomenon

 There are different modelling approaches

 In general
 ሶ
For static analysis (b) 𝜏𝑓 = 𝐵𝑠 𝑠𝑖𝑔𝑛(𝜃)
 For dynamic analysis (a) 𝜏𝑓 = 𝐵𝑑 𝜃ሶ
 For detailed analysis Stribeck model (f) may be appropriate

41

 Robot dynamics with friction

ሶ =𝜏
𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 + 𝐹(𝜃)

42

21
 12/20/2022

Force Control

 When the robot arm has contact with its environment the control problem
will shift from tracking motion trajectories to the force/torque control of the
end effector
 Tasks like
 Grinding
 Deburring
 Assembly
 Window cleaning

43

Force Sensor

 Force/torque sensor at the end effector will be needed to measure the

reacting forces/torques from the environment that may be placed at the
wrist joint at each axis, called 6-axis wrist sensor

44

22
 12/20/2022

45

23
 19.11.2023

EXAMPLES
Ch. 2-3

 C-space:{𝜃1 , 𝑥1 , 𝑦1 , 𝜃2 , 𝑥2 , 𝑦2 } with four constraints and 2DoF

e.g. 𝑙1 cos(𝜃1 ) + 𝑙2 cos(𝜃1 + 𝜃2 ) − 𝑥2 = 0
 Workspace
𝑓𝑜𝑟 𝜃1 ∈ 0,2𝜋
𝑓𝑜𝑟 𝜃2 ∈ 0,2𝜋
𝑊= 𝑥, 𝑦 𝑥 = 𝑥𝑡𝑖𝑝 , 𝑦 = 𝑦𝑡𝑖𝑝 }

1
 19.11.2023

 Determine whether the velocity constraints are holonomic or

nonholonomic?

 Put the constraints into Pfaffian form

𝐴 𝑞 𝑞ሶ = 0
If it is integrable then holonomic, i.e. ∃ 𝑎 𝑔 𝑞 ∋

න 𝐴 𝑞 𝑞ሶ 𝑑𝑡 = 𝑔(𝑞)

 Or
𝑑𝑞
න𝐴 𝑞 𝑑𝑡 = න 𝐴 𝑞 𝑑𝑞 = 𝑔(𝑞)
𝑑𝑡

 Or
𝜕𝑔 𝑞
𝐴 𝑞 =
𝜕𝑞

2
 19.11.2023

 For this example

− cos 𝑞2 0 sin(𝑞1 + 𝑞2 ) −cos(𝑞1 + 𝑞2 )
𝐴 𝑞 =
0 0 sin 𝑞1 − cos 𝑞1
𝜕𝑔1
 = − cos 𝑞2 a solution may be
𝜕𝑞1
𝑔1 = −𝑞1 cos 𝑞2 + ℎ(𝑞2 , 𝑞3 , 𝑞4 )
𝜕𝑔1
 = 0 apply to this implicit solution
𝜕𝑞2
ℎ 𝑞2 , 𝑞3 , 𝑞4 = 𝑞1 cos 𝑞2 + 𝑓(𝑞3 , 𝑞4 )
 However, this contradicts
ℎ 𝑞2 , 𝑞3 , 𝑞4 ≠ 𝑓(𝑞1 )
 Thus, not integrable, hence nonholonomic constraints

 In general, we would continue

𝜕𝑔𝑖 (𝑞)
= 𝑎𝑖𝑗 (𝑞)
𝜕𝑞𝑗
 Should have been searched for a solution
 For 𝑖 = 1, 2, 𝑗 = 1, 2, 3, 4 in our example

3
 19.11.2023

Rotations in 3D

 Let there be an unknown transformation R mapping points

1 3 −1 1 0 −2
2 → 0.707 0 → 0.707 1 → 0.707
3 −2.121 1 0.707 −2 −0.707
Find the transformation R

4
 19.11.2023

A=np.array([[1,2,3],
[-1,0,1],
[0,1,-2]])
B=np.array([[3,0.707,-2.121],
[1,0.707,0.707],
[-2,0.707,-0.707]])
[email protected](A.T)
print(R)

[[ 0. 0. 1. ]
[-0.707 0.707 0. ]
[-0.707 -0.707 0. ]]

 Camera is used to measure the robot end-effector and objct in the

env
 Joint space is measurable so is the cartesian space xform 𝑇𝑏𝑐
 To define the motion robot base so that end effector is placed to the
block object to pick it up, find the homogeneous xform 𝑇𝑎𝑏

5
 19.11.2023

 𝑇𝑎𝑏 = 𝑇𝑎𝑑 𝑇𝑑𝑏

 𝑇𝑐𝑒 ?
 {a-e} Reachable over {a-b-c-e}
and {a-d-e} frames
𝑇𝑎𝑏 𝑇𝑏𝑐 𝑇𝑐𝑒 = 𝑇𝑎𝑑 𝑇𝑑𝑒
𝑇𝑐𝑒 = 𝑇𝑎𝑏 𝑇𝑏𝑐 −1 𝑇𝑎𝑑 𝑇𝑑𝑒

import numpy as np

Tdb=np.array([[0,0,-1,250],[0,-1,0,-150],[-1,0,0,200],[0,0,0,1]])
Tde=np.array([[0,0,-1,300],[0,-1,0,100],[-1,0,0,120],[0,0,0,1]])
Tad=np.array([[0,0,-1,400],[0,-1,0,50],[-1,0,0,300],[0,0,0,1]])
a=1/np.sqrt(2)
Tbc=np.array([[0,-a,-a,30],[0,a,-a,-40],[1,0,0,25],[0,0,0,1]])

[[ 1 0 0 200]
Tab=Tad@Tdb [ 0 1 0 200]
print(Tab) [ 0 0 1 50]
[ 0 0 0 1]]
Tce=np.linalg.inv(Tab@Tbc)@Tad@Tde
printFmttd(Tce)

6
 19.11.2023

Parameterization of Orientation

0 −1 0
𝑅 = 0 0 −1
1 0 0
 R may also be parameterized by a single rotation of 𝜃 about
a vector 𝑘
𝑟11 +𝑟22 +𝑟33 −1 1
 𝜃 = cos −1 2
= cos−1 − 2 = 120

𝑟32 − 𝑟23 1
1 1
 𝑘= 𝑟13 − 𝑟31 = −1
2 sin 𝜃 3
𝑟21 − 𝑟12 1

Example in Python

 Rodriguez formula with 𝜃 = 120, 𝑘 = [.577 −.211 .789]

𝑅1𝑜 = 𝐼 ⋅ cos 𝜃 + 𝑘 ⋅ sin 𝜃 + 𝑘 ⋅ 𝑘 𝑇 1 − cos 𝜃
import numpy as np
th=120*np.pi/180
0.00 -0.87 0.50
ct=np.cos(th)
0.50 -0.43 -0.75
st=np.sin(th)
0.87 0.25 0.43
sq3=np.sqrt(3)
I=np.eye(3)
k=np.array([1, 0.5-sq3/2, .5+sq3/2]/sq3).reshape(3,1)
def skew_mat(v):
V=np.array([[0,-v[2],v[1]],
[v[2],0,-v[0]],
[-v[1],v[0],0]],dtype=float)
return V
R=ct*I+st*skew_mat(k)+(1-ct)*k.dot(k.T)#Rodriguez formula
for row in R:
print('%6.2f %6.2f %6.2f' %(row[0],row[1],row[2]))

7
 17.11.2023

Free Moving Coordinate Frames

ZYZ Euler angles, 𝑅 = 𝑅𝑧,𝜓 𝑅𝑦,𝜃 𝑅𝑧,𝜙

𝑟11 𝑟12 𝑟13 𝑐𝜓 −𝑠𝜓 0 𝑐𝜃 0 𝑠𝜃 𝑐𝜙 −𝑠𝜙 0

 𝑅 = 𝑟21 𝑟22 𝑟23 = 𝑠𝜓 𝑐𝜓 0 0 1 0 𝑠𝜙 𝑐𝜙 0
𝑟31 𝑟32 𝑟33 0 0 1 −𝑠𝜃 0 𝑐𝜃 0 0 1
 Given an R find the ZYZ Euler angles?

𝑐𝜓 −𝑠𝜓 0 𝑐𝜃𝑐𝜙 −𝑐𝜃𝑠𝜙 𝑠𝜃

𝑅 = 𝑠𝜓 𝑐𝜓 0 𝑠𝜙 𝑐𝜙 0
0 0 1 −𝑠𝜃𝑐𝜙 𝑠𝜃𝑠𝜙 𝑐𝜃

𝑐𝜃𝑐𝜙𝑐𝜓 − 𝑠𝜙𝑠𝜓 −𝑐𝜃𝑠𝜙𝑐𝜓 − 𝑐𝜙𝑠𝜓 𝑠𝜃𝑐𝜓

= 𝑐𝜃𝑐𝜙𝑠𝜓 + 𝑠𝜙𝑐𝜓 −𝑐𝜃𝑠𝜙𝑠𝜓 + 𝑐𝜙𝑐𝜓 𝑠𝜃𝑠𝜓
−𝑠𝜃𝑐𝜙 𝑠𝜃𝑠𝜙 𝑐𝜃

1
 17.11.2023

VELOCITY KINEMATICS
Spong’d Text Ch. 4

2
 17.11.2023

Skew Symmetric Matrices

 Skew symmetric matrix 𝑆 ∈ ℝ𝑛×𝑛 is defined as

𝑆 + 𝑆𝑇 = 𝑂

 Some Properties

1. Linearity
𝑆 𝑐1 𝒙 + 𝑐2 𝒚 = 𝑐1 𝑆 𝒙 + 𝑐2 𝑆(𝒚)

2. Cross product
𝑆 𝒙 𝒚=𝒙×𝒚

3. If 𝑅 ∈ 𝑆𝑂(3)
𝑅 𝒙 × 𝒚 = 𝑅𝒙 × 𝑅𝒚

If R is a rotation matrix, cross multiplication of two vectors’ rotation is the same

as rotated vectors cross multiplication

4. Similarity transformation
𝑅𝑆 𝒙 𝑅𝑇 = 𝑆(𝑅𝒙)

 A transform in one frame can be transformed into another frame

 Let the vector 𝜔𝑏 be angular velocity vector measured in body frame

{b} and represented by the SSM matrix, 𝑆 𝜔𝑏 = 𝛺𝑏 = 𝜔𝑏 .

 The same transform can be expressed in stationary frame {s} where

𝑅 = 𝑅𝑏𝑠

 The vector given in {b} and observed in {s}

𝑆(𝜔 𝑠 ) = 𝑅𝑆 𝜔𝑏 𝑅𝑇 = 𝑆 𝑅𝜔𝑏 = 𝜔 𝑠

3
 17.11.2023

Derivative of Rotation Matrix – Revisited

 Let 𝑅 𝜃 ∈ 𝑆𝑂 3 differantiation of
𝑇
𝑅 𝜃 𝑅𝜃 =𝐼

𝑑𝑅 𝑇
𝑑𝑅 𝑇
𝑅 𝜃 +𝑅 𝜃 =0 (∗)
𝑑𝜃 𝑑𝜃

 Let

𝑑𝑅 𝑑𝑅
 𝑆≝ 𝑅 𝜃 𝑇 = 𝑆𝑅(𝜃)
𝑑𝜃 𝑑𝜃
 (*) becomes

𝑆 + 𝑆𝑇 = 0

Examples

 Let 𝑅 = 𝑅𝑥,𝜙 basic rotation about x axis

1 0 0
𝑅 = 𝑅𝑥,𝜙 = 0 𝑐𝜙 −𝑠𝜙
0 𝑠𝜙 𝑐𝜙

Then

𝑑𝑅 0 0 0 1 0 0
𝑇
𝑆= 𝑅 𝜙 = 0 −𝑠𝜙 −𝑐𝜙 0 𝑐𝜙 𝑠𝜙
𝑑𝜙 0 𝑐𝜙 −𝑠𝜙 0 −𝑠𝜙 𝑐𝜙

0 0 0 1
𝑆 = 0 0 −1 = 𝑆 0 = 𝑆(𝒊)
0 1 0 0
𝑆(𝒊) that is the axis of rotation’s SSM multiplying 𝑅 in the derivative of 𝑅

𝑑𝑅𝑥,𝜙
= 𝑆 𝑖 𝑅𝑥,𝜙
𝑑𝜙

4
 17.11.2023

 Basic rotation derivatives are

𝑑𝑅𝑥,𝜙
= 𝑆 𝒊 𝑅𝑥,𝜙
𝑑𝜙

𝑑𝑅𝑦,𝜃
= 𝑆 𝒋 𝑅𝑦,𝜃
𝑑𝜃

𝑑𝑅𝑧,𝜓
= 𝑆 𝒌 𝑅𝑧,𝜓
𝑑𝜓

 Generally, for the derivative of a rotation 𝜃 about a vector 𝒏 is

𝑑𝑅𝑛,𝜃
= 𝑆 𝒏 𝑅𝑛,𝜃
𝑑𝜃

Angular Velocity

 Angular velocity about an arbitrary non-stationary axis, we have

𝑅 = 𝑅 𝑡 ∈ 𝑆𝑂(3)

 Differentiating 𝑅 with respect to 𝑡 rather than 𝜃 yields

𝑑𝑅
= 𝑆(𝑡)𝑅(𝑡)
𝑑𝑡
 Recall, 𝑆(𝑡) was an SSM and was also defined as Ω = 𝜔 , implies that
there exists a vector say 𝜔(𝑡) such that
𝑆 𝑡 = 𝑆(𝜔 𝑡 )

 This 𝝎(𝒕) happens to be the angular velocity vector of a moving frame

w.r.t. stationary frame and
𝑅(𝑡) = 𝑆(𝜔(𝑡))𝑅(𝑡)

5
 17.11.2023

 Notation used in Spong’s text

𝑅1𝑜 = 𝑆 𝜔0,1 𝑅1𝑜

 Frame {1} is rotating w.r.t. frame {0} at angular velocity 𝜔0,1

𝑎
 This vector 𝜔0,1 may be expressed (observed) in any frame {a} by 𝜔0,1

 What is
0
𝜔1,2

Example

 Let 𝑅 𝑡 = 𝑅𝑥,𝜃(𝑡) moving frame is rotating about x-axis by 𝜃(𝑡). What is

the time derivative 𝑅?

 Applying the chain rule

𝑑𝑅 𝑑𝜃
𝑅=
𝑑𝜃 𝑑𝑡

𝑑𝑅 𝑑𝜃
 We had = 𝑆 𝒊 𝑅(𝑡) and scalar =𝜃
𝑑𝜃 𝑑𝑡

𝑅 = 𝑆 𝒊 𝑅 𝑡 𝜃 = 𝑆 𝜃𝒊 𝑅 𝑡 = 𝑆 𝜔𝑥 𝑅 𝑡

6
 17.11.2023

Angular Velocities of Rotating Frames

 Assume there are multiple frames rotating independently and a stationary

frame {0}
 Define the time dependent rotation matrices by
 𝑅10 𝑡 and 𝑅21 𝑡
 These also give us 𝑅20 𝑡 = 𝑅10 𝑡 𝑅21 𝑡
 To find the angular velocities need to start taking the time derivatives
𝑅20 = 𝑅10 𝑅21 + 𝑅10 𝑅21 (*)

 Also using the previous result between {0} and {2}

𝑅20 = 𝑆 𝜔 𝑅20

 Here 𝜔 is ang. vel of {2} w.r.t. {0} written in {0} or

0
𝜔 = 𝜔0,2

 The first term in (*) is

𝑅10 𝑅21 = 𝑆 𝜔0,1
0
𝑅10 𝑅21 = 𝑆 𝜔0,1
0
𝑅20

 Similarly, express the derivative in the second term of (*)

𝑅10 𝑅21 = 𝑅10 𝑆 𝜔1,2
1
𝑅21
𝑇
 Multiply 𝑅21 by the identity matrix 𝑅10 𝑅10 = 𝐼
𝑇
𝑅10 𝑅21 = 𝑅10 𝑆 𝜔1,2
1
𝑅10 𝑅10 𝑅21

 Use the similarity property 4

𝑇
𝑅10 𝑆 𝜔1,2
1
𝑅10 = 𝑆 𝑅10 𝜔1,2
1

Putting all together in (*),

𝑅20 = 𝑆 𝜔0,1
0
𝑅20 + 𝑆 𝑅10 𝜔1,2
1
𝑅20

7
 17.11.2023

𝑅20 = 𝑆 𝜔0,1
0
+ 𝑆 𝑅10 𝜔1,2
1
𝑅20

 Now apply linearity property 𝑆 𝑎 + 𝑆 𝑏 = 𝑆(𝑎 + 𝑏)

𝑅20 = 𝑆 𝜔0,1
0
+ 𝑅10 𝜔1,2
1
𝑅20

 Since,

𝑅20 = 𝑆 𝜔0,2
0
𝑅20

 We have the addition of angular velocity vectors

0 0
𝜔0,2 = 𝜔0,1 + 𝑅10 𝜔1,2
1 0
= 𝜔0,1 0
+ 𝜔1,2 (1)

 Therefore, the addition of angular velocities in different frames may be

added and expressed in another frame as in (1)

Velocity in the Kinematic Chain

 Assume that we have n moving frames and a reference {0} frame and

𝑅𝑛0 = 𝑅10 𝑅21 ⋯ 𝑅𝑛𝑛−1

 The results in (1) can be extended as

0
𝑅𝑛0 = 𝑆 𝜔0,𝑛 𝑅𝑛0

 With
0 0
𝜔0,𝑛 = 𝜔0,1 + 𝑅10 𝜔1,2
1 0
+ ⋯ + 𝑅𝑛−1 𝑛−1
𝜔𝑛−1,𝑛 (2)

0 0 0 0 (3)
𝜔0,𝑛 = 𝜔0,1 + 𝜔1,2 + ⋯ + 𝜔𝑛−1,𝑛

8
 17.11.2023

Linear Velocity on Moving Frames

 Assume that we have a point p attached to a moving frame

 Assume {1} is only rotating about a fixed axis
𝑝0 = 𝑅10 (𝑡)𝑝1

 Let’s find the linear velocity of 𝑝 observed in {0} frame

𝑝0 (𝑡) = 𝑅10 𝑡 𝑝1 + 𝑜0,1

0

0
 Assume 𝑜0,1 is constant

 Its time derivative

𝑝0 = 𝑅10 𝑝1 + 𝑅10 𝑝1

 Since 𝑝 is rigidly attached to frame {1} 𝑝1 = 0 and 𝑅10 = 𝑆 𝜔0 𝑅10

𝑝0 = 𝑆 𝜔0 𝑅10 𝑝1
𝑝0 = 𝑆 𝜔0 𝑝0
 We get the linear velocity due to rotation only by property
𝑝0 = 𝜔0 × 𝑝0
 The direction of linear velocity is perpendicular to 𝜔0 and 𝑝0

9
 17.11.2023

 Now assume that the frame translation is also in motion

𝑝0 (𝑡) = 𝑅10 𝑡 𝑝1 + 𝑜0,1
0
(𝑡)

 Time derivative

𝑝0 = 𝑅10 𝑝1 + 𝑜0,1
0

 Using the previous results

0
𝑝0 = 𝑆 𝜔0 𝑝0 + 𝑜0,1
0
𝑝0 = 𝜔0 × 𝑝0 + 𝑣0,1

n-Link Manipulator

 The n-Link Manipulator homogeneous matrix was

0 𝑜𝑛0 𝑞
𝑇𝑛0 𝑞 = 𝑅𝑛 𝑞
0 1
With 𝑞 = 𝑞1 , 𝑞2 , … , 𝑞𝑛 𝑇

 When the robot moves, the end-effector position and orientation will
change in time with some velocities

 We’ll try to find the relationship of those velocities with the joint space
velocities 𝑞(𝑡) using Jacobians

10
 17.11.2023

Jacobians

 The angular velocity of the end-effector is

𝑆 𝜔𝑛0 = 𝑅𝑛0 𝑅𝑛0 𝑇

 The linear velocity

𝑣𝑛0 = 𝑜𝑛0

 The Jacobian is defined as

𝑣 0 𝐽𝑣
= 𝑞
𝜔 𝑛 𝐽𝜔
 As

𝐽𝑣
𝐽=
𝐽𝜔 6×𝑛

 Let’s have a closer look at those matrices

 Let the 𝑖 𝑡ℎ joint be a revolute with 𝜔𝑖−1,𝑖 = 𝜔𝑖 angular vel.

 Any link’s axis of rotation is on the 𝑧 axis by the DH convention

 Relative angular velocity with respect to i-1 th link

0
𝜔𝑖𝑖−1 = 𝑞𝑖 𝑧𝑖−1
𝑖−1
= 𝑞𝑖 0 = 𝑞𝑖 𝑘𝑖−1
1
 If the link is prismatic then
𝜔𝑖𝑖−1 = 0

11
 17.11.2023

Angular Velocity Kinematics

 The end effector angular velocity (2)

0 0
𝜔0,𝑛 = 𝜔0,1 + 𝑅10 𝜔1,2
1 0
+ ⋯ + 𝑅𝑛−1 𝑛−1
𝜔𝑛−1,𝑛
𝑇
 DH convention: All rotations about z axes, 𝑘 = 0,0,1
0
𝜔0,𝑛 = 𝜌1 𝑞1 𝑘 + 𝜌2 𝑞2 𝑅10 𝑘 + ⋯ + 𝜌𝑛 𝑞𝑛 𝑅𝑛−1
0
𝑘
𝑧00 𝑧10 0
𝑧𝑛−1
1 𝑟𝑒𝑣𝑜𝑙𝑢𝑡𝑒
 With 𝜌𝑖 =
0 𝑝𝑟𝑖𝑠𝑚𝑎𝑡𝑖𝑐

 In a compact form, the Jacobian

𝑛
0 0
𝜔0,𝑛 = 𝜌𝑖 𝑞𝑖 𝑧𝑖−1 = 𝐽𝜔 𝒒
𝑖=1

𝐽𝐽𝜔 = [[𝜌
𝜔 = 𝜌11 𝑧𝑧00 ⋯ 𝜌
⋯ 𝜌𝑛 𝑧𝑧𝑛−1 ]
𝑛 𝑛−1 ]

Linear Velocity Jacobian

 By chain rule, it is straight forward to express the linear velocity as

𝑛
𝜕𝑜𝑛0
𝑜𝑛0 = 𝑞 = 𝐽𝑣 𝒒
𝜕𝑞𝑖 𝑖
𝑖=1

With

𝜕𝑜𝑛0 𝜕𝑜𝑛0
𝐽𝑣 = ⋯
𝜕𝑞1 𝜕𝑞𝑛

 That is the 𝑖 𝑡ℎ column

𝜕𝑜𝑛0
𝐽𝑣𝑖 =
𝜕𝑞𝑖

12
 17.11.2023

𝜌𝑖 = 1 Prismatic Joint Case

𝑜0 𝑜𝑖−1,𝑖 𝑜𝑖 𝑜𝑖,𝑛

𝑜0,𝑖−1 𝑜𝑛
𝑜𝑖−1

 Note that vector sum may be expressed as

𝑜0,𝑛 = 𝑜0,𝑖−1 + 𝑜𝑖−1,𝑖 + 𝑜𝑖,𝑛

In any coordinate frame

 Then, in {0}
0 0 𝑖−1
0
𝑜0,𝑛 = 𝑜0,𝑖−1 + 𝑅𝑖−1 𝑜𝑖−1,𝑖 + 𝑅𝑖0 𝑜𝑖,𝑛
𝑖

 Note that in DH convention

𝑐𝜃𝑖 −𝑠𝜃𝑖 𝑐𝛼𝑖 𝑠𝜃𝑖 𝑠𝛼𝑖 𝑎𝑖 𝑐𝜃𝑖

𝑠𝜃𝑖 𝑐𝜃𝑖 𝑐𝛼𝑖 −𝑐𝜃𝑖 𝑠𝛼𝑖 𝑎𝑖 𝑠𝜃𝑖
𝑇𝑖𝑖−1 𝜃𝑖 = 𝐴𝑖 =
0 𝑠𝛼𝑖 𝑐𝛼𝑖 𝑑𝑖
0 0 0 1

𝑖−1
𝑜𝑖−1,𝑖

13
 17.11.2023

Linear Velocity: Prismatic Joint Case

 So

𝑎𝑖 𝑐𝜃𝑖
𝑖−1
𝑜𝑖−1,𝑖 = 𝑎𝑖 𝑐𝜃𝑖
𝑑𝑖

 And the linear velocity due to ith prismatic joint is

0 0
𝜕𝑜0,𝑛 𝑑𝑞𝑖 𝜕𝑜0,𝑛 𝜕
𝑣𝑖 = = 𝑑 = 𝑑𝑖 𝑜0 0
+ 𝑅𝑖−1 𝑖−1
𝑜𝑖−1,𝑖 + 𝑅𝑖0 𝑜𝑖,𝑛
𝑖
𝜕𝑞𝑖 𝑑𝑡 𝜕𝑑𝑖 𝑖 𝜕𝑑𝑖 0,𝑖−1

𝜕 𝜕
= 𝑑𝑖 𝑅0 𝑜 𝑖−1 = 𝑑𝑖 𝑅𝑖−1
0
𝑜 𝑖−1
𝜕𝑑𝑖 𝑖−1 𝑖−1,𝑖 𝜕𝑑𝑖 𝑖−1,𝑖

 Only the 3rd row is a function of 𝑑𝑖

0
0
𝑣𝑖 = 𝑑𝑖 𝑅𝑖−1 0 = 𝑑𝑖 𝒛0𝑖−1
1

Prismatic Joint’s Linear Velocity

𝑑𝑖
 In Jacobian form

𝐽𝑣𝑖 𝑞 = [0,0, … , 𝒛0𝑖−1 , … , 0]𝑞

14
 17.11.2023

Linear Velocity Due to Revolute Joints

 Here
𝑞𝑖 = 𝜃𝑖

 Doesn’t effect frames {0}-{i-1} and only will move the frames {i}-{n}

𝑜𝑖−1,𝑛
{𝑛}
{𝑖 − 1}

𝑜0,𝑖−1 𝑧 , 𝜃
𝑖−1 𝑖
𝑜0,𝑛

{0}
 Distance of the end effector to the origin of {𝑖 − 1}
0 0 0
𝑜𝑖−1,𝑛 = 𝑜0,𝑛 − 𝑜0,𝑖−1

 Rotation 𝜔𝑖 about 𝑧𝑖−1 generates linear velocity at the end effector by

𝑣𝑖0 = 𝜔𝑖0 × 𝑜0,𝑛

0 0
− 𝑜0,𝑖−1

 Since the angular velocity vector of link 𝐿𝑖 is

𝜔𝑖0 = 𝜃𝑖 𝒛0𝑖−1

 Implies, the linear velocity of the end effector

𝑣𝑖0 = 𝜃𝑖 𝒛0𝑖−1 × (𝑜0,𝑛

0 0
− 𝑜0,𝑖−1 )

 The jacobian
𝜃𝑖

𝐽𝑣𝑖 𝑞 = [0,0, … , 𝒛0𝑖−1 × (𝑜0,𝑛

0 0
− 𝑜0,𝑖−1 ), … , 0]𝑞

15
 17.11.2023

Final Jacobian Expressions

 As a result, the velocity Jacobian matrix is

𝐽𝑣 = 𝐽𝑣1 , … , 𝐽𝑣𝑛
3×𝑛

 With the column vectors

𝒛𝒛0𝑖−1
0 × (𝑜 0 0 − 𝑜 0 0 )
𝑖−1 × (𝑜 𝑜0,𝑖−1 ) revolute
0,𝑛 − 0,𝑖−1
0,𝑛 if joint 𝑖joint 𝑖
revolute
𝐽𝐽𝑣𝑣𝑖 =
=
𝑖 𝒛𝒛0𝑖−1
0 prismatic joint 𝑖
if prismatic
𝑖−1

 Angular velocity Jacobian matrix is

𝐽𝜔 = 𝐽𝜔1 , … , 𝐽𝜔𝑛
3×𝑛

 With the column vectors

𝒛0𝑖−1
𝑖−1 if joint i revolute
revolute joint 𝑖
𝐽𝐽𝜔𝜔𝑖𝑖 =
0 prismatic joint 𝑖
if prismatic

 As a one big matrix

𝐽 = 𝐽1 , … , 𝐽𝑛 6×𝑛

 For a revolute joint i

𝒛0𝑖−1 × 𝑜0,𝑛
0 0
− 𝑜0,𝑖−1
𝐽𝑖 =
𝒛0𝑖−1
6×1

 Or prismatic joint i

0
𝐽𝑖 = 𝒛𝑖−1
0 6×1

16
 17.11.2023

Example

 For RR Robot find the linear and angular velocities of the end effector using
Jacobians

 Two revolute joints the Jacobian is

𝒛0𝑖−1 × 𝑜0,2
0 0
− 𝑜0,𝑖−1
𝐽𝑖 = 𝑖 = 1,2
𝒛0𝑖−1 6×1

With
𝒛00 = 𝒛10 = 0, 0, 1 𝑇

0 𝑇
𝑜0,0 = 0,0,0
0 𝑇
𝑜0,1 = 𝑥1 , 𝑦1 , 0
0 𝑇
𝑜0,2 = 𝑥2 , 𝑦2 , 0

with
𝑥1 = 𝑎1 𝑐𝜃1 , 𝑦1 = 𝑎1 𝑠𝜃1
𝑥2 = 𝑎1 𝑐𝜃1 + 𝑎2 𝑐 𝜃1 + 𝜃2
𝑦2 = 𝑎1 𝑠𝜃1 + 𝑎2 𝑠 𝜃1 + 𝜃2

17
 17.11.2023

𝒛00 × 𝑜0,2
0 0
− 𝑜0,0 𝒛10 × 𝑜0,2
0 0
− 𝑜0,1
𝐽1 = 𝐽2 =
𝒛00 6×1
𝒛10 6×1

With

0 −1 0 𝑥2 −𝑦2 −𝑎1 𝑠1 − 𝑎2 𝑠12

𝒛00 × 𝑜0,2
0 0
− 𝑜0,0 = 1 0 0 𝑦2 = 𝑥2 = 𝑎1 𝑐1 + 𝑎2 𝑐12
0 0 0 0 0 0

0 −1 0 𝑥2 − 𝑥1 −𝑎2 𝑠12
𝒛10 × 𝑜0,2
0 0
− 𝑜0,1 = 1 0 0 𝑦2 − 𝑦1 = 𝑎2 𝑐12
0 0 0 0 0
 The jacobian

−𝑎1 𝑠1 − 𝑎2 𝑠12 −𝑎2 𝑠12

𝑎1 𝑐1 + 𝑎2 𝑐12 𝑎2 𝑐12
0 0
𝐽= 0 0
0 0
1 1

Example

 Instead of the end effector find Jacobian at an arbitrary point on a link

18
 17.11.2023

Analytical Jacobian

 Let’s represent the orientation of the end effector with minimum variables,
e.g. three Euler angles.

 The 6-dof pose of the end effector w.r.t. the base

𝑑 𝑞
𝑋=
𝛼 𝑞 6×1

 With 𝑑 𝑞 position and 𝛼 = 𝜙, 𝜃, 𝜓 𝑇 pose at the end of the effector w.r.t

base. The velocity vector of the tool frame related to the joint velocities

𝑋 = 𝑑 = 𝐽𝑎 (𝑞)𝑞
𝛼
is called the analytical jacobian

 The ZYZ Euler angle transformation

𝑅 = 𝑅𝑧,𝜓 𝑅𝑦,𝜃 𝑅𝑧,𝜙

𝑐𝜙 𝑠𝜙 0 𝑐𝜃 0 −𝑠𝜃 𝑐𝜓 𝑠𝜓 0
𝑅𝑇 = −𝑠𝜙 𝑐𝜙 0 0 1 0 −𝑠𝜓 𝑐𝜓 0
0 0 1 𝑠𝜃 0 𝑐𝜃 0 0 1
 The angular velocities can be expressed by
𝑅=𝑆 𝜔 𝑅

 As

 Proof?

19
 17.11.2023

 Euler angle rates in matrix form

𝜔 =.

 The velocity kinematics

 The geometric and analytical Jacobians

𝐼 0
𝐽 𝑞 𝑞= 𝐽𝑎 (𝑞)𝑞
0 𝐵 𝛼
 Inverse

𝐼 0
𝐽𝑎 𝑞 = −1 𝐽 𝑞
0 𝐵 𝛼
 In this structure the invertibility of 𝐵(𝛼) defines the singularities of the
velocity kinematics

20
 17.11.2023

Singularity

 Identifying manipulator singularities is important, since

 Singularities represent configurations from which certain directions of motion may be

unattainable

 At singularities, bounded end-effector velocities may correspond to unbounded joint

velocities

 At singularities, bounded end-effector forces and torques may correspond to

unbounded joint torques.

 Singularities usually (but not always) correspond to points on the boundary of the
manipulator workspace, that is, to points of maximum reach of the manipulator.

 Singularities correspond to points in the manipulator workspace that may be

unreachable

 Near singularities there will not exist a unique solution to the inverse kinematics
problem

Inverse Velocity

 The Jacobian relationship

𝜉 = 𝐽𝑞

 The inverse of non square matrix exists?

𝑞 = 𝐽−1 𝜉?

 For 𝐽 ∈ ℝ𝑚×𝑛 with 𝑚 < 𝑛. If rank 𝐽 = 𝑚 then 𝐽𝐽𝑇 −1 exists

 Then
𝐽𝐽𝑇 𝐽𝐽𝑇 −1 =𝐼
𝐽 𝐽𝑇 𝐽𝐽𝑇 −1 =𝐼
𝐽𝐽+ = 𝐼

 𝐽+ = 𝐽𝑇 𝐽𝐽𝑇 −1 right pseudo inverse of 𝐽

21
 17.11.2023

 Then, the inverse velocity kinematics solution may be given as

𝑞 = 𝐽+𝜉 + 𝐼 − 𝐽+𝐽 𝑏

 For arbitrary 𝑏 ∈ ℝ𝑛

 Can be proved by multiplying by 𝐽 from the left

𝐽𝑞 = 𝐽𝐽𝑇 𝐽𝐽𝑇 −1 𝜉 + 𝐽 𝐼 − 𝐽+𝐽 𝑏
𝐽𝑞 = 𝜉 + 𝐽 𝐼 − 𝐽𝑇 𝐽𝐽𝑇 −1 𝐽 𝑏
𝐽𝑞 = 𝜉 + 𝐽 − 𝐽𝐽𝑇 𝐽𝐽𝑇 −1 𝐽 𝑏
𝐽𝑞 = 𝜉

22
 1/5/2024

ROBOT ENGINEERING FINAL EXAM

REVIEW

 Final exam will be open notes, books

 No phones, networked computing device
 Comprehensive coverage
 Exam date is announced and duration is 75 min

1
 1/5/2024

Mechanisms
Grübler’s Formula

 The # DoF of a mechanism with links and joints can be calculated as

 Let
• 𝑁 = # of links +1 (for ground)
• 𝐽 = # joints
• 𝑓𝑖 = # freedom of joint I
• 𝑐𝑖 is the constraints by joint i with 𝑓𝑖 + 𝑐𝑖 = 𝑚
• 𝑚 = DoF of rigid body (3 for 2D 6 for 3D)
 Then
𝑱
• 𝑫𝒐𝑭 = 𝒎 𝑵 − 𝟏 − σ𝒊=𝟏 𝒄𝒊
𝑱
• = 𝒎 𝑵 − 𝟏 − σ𝒊=𝟏(𝒎 − 𝒇𝒊 )
𝑱
• = 𝒎 𝑵 − 𝟏 − 𝑱 + σ𝒊=𝟏 𝒇𝒊

DoF

2
 1/5/2024

Constraints

 k independent holonomic constraints on 𝜃1 , 𝜃2 , … , 𝜃𝑛 ∈ ℝ𝑛

=0

 reduce an n-dim C-space to n−k dof

 Constraints in Pfaffian’s form
𝐴 𝜃 𝜃ሶ = 0
May or may not be holonomic
If integrable, i.e., there exists function of constraints

ሶ = න 𝐴 𝜃 𝑑𝜃 = 𝑔(𝜃)
න 𝐴 𝜃 𝜃𝑑𝑡

 If 𝐴 𝑞 𝑞ሶ = 0 is integrable then there exists a 𝑔 𝑞 such that

𝜕𝑔
= 𝐴(𝑞)
𝜕𝑞
Example:
𝑞ሶ 1
1 + cos 𝑞1 cos 𝑞2 1 + cos 𝑞1 cos 𝑞2 𝑞ሶ 2 = 0
𝑞ሶ 3
 Search for a possible solution of

𝜕𝑔
= 1 + cos 𝑞1 → 𝑔 𝑞 = 𝑞1 + sin 𝑞1 + ℎ(𝑞2 , 𝑞3 )
𝜕𝑞1
𝜕𝑔
= cos 𝑞2 → 𝑔 𝑞 = 𝑞1 + sin 𝑞1 + sin 𝑞2 + ℎ(𝑞3 )
𝜕𝑞2
. 𝜕𝑔
= 1 + cos 𝑞1 cos 𝑞2 → ℎ 𝑞3 = 1 + cos 𝑞1 cos 𝑞2 𝑞3 ? ? ?
𝜕𝑞3

3
 1/5/2024

Coordinate Frames

 A point in one frame may be

expressed in another frame after
◼ Orientation alignment
◼ Translational shift
 Orientation alignment assumes the
 origins coincide
 Unit vector in the frame is
expressed in the other frame

𝑖1 ∙ 𝑖𝑜 𝑗1 ∙ 𝑖𝑜 𝑘1 ∙ 𝑖𝑜
𝑅10 = 𝑖10 𝑗10 𝑘10 3×3 = 𝑖1 ∙ 𝑗𝑜 𝑗1 ∙ 𝑗𝑜 𝑘1 ∙ 𝑗𝑜 ∈ 𝑆𝑂(3)
𝑖1 ∙ 𝑘𝑜 𝑗1 ∙ 𝑘𝑜 𝑘1 ∙ 𝑘𝑜

Example

cos 𝜃
 𝑖10 =
sin 𝜃

− sin 𝜃
 𝑗10 =
cos 𝜃

cos 𝜃 − sin 𝜃
 𝑅10 =
sin 𝜃 cos 𝜃

cos 𝜃 −sin 𝜃 0
 𝑅10 = sin 𝜃 cos 𝜃 0 = 𝑅𝑜𝑡 𝑧, 𝜃 = 𝑅𝑧,𝜃
0 0 1

4
 1/5/2024

Similarity Transformation

 A transformation in {0}, 𝐴0 , may be expressed in {1}, 𝐴1 , as

𝐴1 = 𝑅01 𝐴0 𝑅10
 Or
𝐴1 = 𝑅10 −1 0 0
𝐴 𝑅1
 Angular velocity in {b} frame, 𝜔𝑏 , may be expressed in {i}, 𝜔𝑖
 𝑆 𝑏 = 𝜔𝑏
−1 𝑏
𝑆 𝑖 = 𝑅𝑖𝑏 𝑆 𝑅𝑖𝑏
With 𝑆 𝑖 = 𝜔𝑖

 A rigid body is rotating has 𝜔𝑏 = 110 𝑇

its transform
0 1 0
𝑖
𝑅𝑏 = −1 0 0
0 0 1
𝑖
what is 𝜔 ?
 𝑅𝑏𝑖 𝑆 𝜔𝑏 𝑅𝑖𝑏 = 𝑆( 1 − 1 0 ′ )
 May be written as well
𝑅𝑆 𝒙 𝑅 𝑇 = 𝑆(𝑅𝒙)

10

5
 1/5/2024

Consecutive Rotations

 If rotations 𝑅1 , 𝑅2 , … , 𝑅𝑛 have carried out

 About current (body) frame (pre multiplied)

𝑅1 𝑅2 … 𝑅𝑛−1 𝑅𝑛

 About fixed (inertial) frame (post multiplied)

𝑅𝑛 𝑅𝑛−1 … 𝑅2 𝑅1

11

 Three consecutive rotations on fixed-frame:

1. Yaw 𝑥, 𝜓
2. Pitch 𝑦, 𝜃
3. Roll 𝑧, 𝜙

12

6
 1/5/2024

 𝑅 ∈ 𝑆𝑂(3) may be represented by

 3x3 Matrix form
 Three consecutive rotations
 Single rotation about a unit vector
 Quaternion
 Single rotation of
𝑟11 + 𝑟22 + 𝑟33 − 1
𝜃 = cos −1
2
About
𝑟32 − 𝑟23
1
𝑘= 𝑟13 − 𝑟31
2 sin 𝜃 𝑟 − 𝑟
21 12

13

 Given single rotation 𝜃 and vector 𝑘

𝑅 = 𝐼 ⋅ cos 𝜃 + 𝑘 ⋅ sin 𝜃 + 𝑘 ⋅ 𝑘 𝑇 1 − cos 𝜃

14

7
 1/5/2024

Derivative of R

 Let 𝑅(𝜃) ∈ 𝑆𝑂(3). Then, by definition

𝑅 𝜃 𝑅 𝜃 𝑇 =𝐼
 Let’s differentiate this equation w.r.t. 𝜃
𝑑𝑅 𝑇
𝑑𝑅𝑇
𝑅 𝜃 +𝑅 𝜃 =0
𝑑𝜃 𝑑𝜃
 Define
𝑑𝑅 𝑇
𝑆≝ 𝑅 𝜃
𝑑𝜃
 Derivative equation satisfy SSM definition, i.e.
𝑆 + 𝑆𝑇 = 0
 Multiply 𝑆 by 𝑅 from right
𝑑𝑅
= 𝑆𝑅(𝜃)
𝑑𝜃

15

 Observed that
𝑑𝑅𝑥,𝜙
= 𝑆 𝑖 𝑅𝑥,𝜙
𝑑𝜙
𝑑𝑅𝑦,𝜃
= 𝑆 𝑗 𝑅𝑦,𝜃
𝑑𝜃
𝑑𝑅𝑧,𝜓
= 𝑆 𝑘 𝑅𝑧,𝜓
𝑑𝜓
 And
𝑑𝑅(𝑡)
= 𝑆 𝜔 𝑅(𝑡)
𝑑𝑡

16

8
 1/5/2024

Homogenous Transformation

 When the origins don’t coincide

𝑅10 𝑡10 𝑡10

 [𝑅10 𝑡10 ]~𝑇10 =
0𝑇 1
 Inverse of 𝑇10
𝑅10 𝑇 − 𝑅10 𝑇 𝑡10
𝑇10 −1
=
0𝑇 1

𝑅10 𝑡10 𝑅10 𝑇 − 𝑅10 𝑇 𝑡10 𝐼 0

𝑇10 𝑇10 −1
= = 3×3 = 𝐼4×4
0𝑇 1 0𝑇 1 0𝑇 1

17

𝑇𝑒𝑎 = 𝑇𝑑𝑎 𝑇𝑒𝑑 = 𝑇𝑏𝑎 𝑇𝑐𝑏 𝑇𝑒𝑐

𝑇𝑐𝑏 = (𝑇𝑏𝑎 )−1 𝑇𝑑𝑎 𝑇𝑒𝑑 𝑇𝑒𝑐 −1

18

9
 1/5/2024

DH Coordinate Assignment (Spong’s Approach)

 Rule 1: 𝑧𝑖−1 axis is the 𝑖 𝑡ℎ joint axis of actuation, on the parent frame
 Rule 2: 𝑥𝑖 axis on the shortest line segment between 𝑧𝑖−1 and 𝑧𝑖 , on the
child frame
 Note: Selection 𝑥𝑖 must be orthogonal to both 𝑧𝑖−1 and 𝑧𝑖
 Steps to assign the coordinate frames for link 𝑖 with respect to {𝑖 − 1}
 If 𝑧𝑖−1 and 𝑧𝑖 are parallel, 𝑥𝑖 can be anywhere, pick the most
convenient on 𝑖 𝑡ℎ joint
 If 𝑧𝑖−1 and 𝑧𝑖 orthogonal, 𝑥𝑖 normal (either ways) to the surface of the
plane of 𝑧𝑖−1 and 𝑧𝑖 , and 𝑜𝑖 is at the intersection
 𝑥𝑜 is free but for the convenience picked parallel to 𝑥1
 If 𝑧𝑛 of the end effector is free, pick it parallel to 𝑧𝑛−1

19

 Once DH params are derived Link 𝑖’s coordinate transformation

𝐴𝑖 = 𝑅𝑜𝑡 𝑧𝑖−1 , 𝜃𝑖 𝑇𝑟𝑎𝑛𝑠 𝑧𝑖−1 , 𝑑𝑖 𝑇𝑟𝑎𝑛𝑠 𝑥𝑖 , 𝑎𝑖 𝑅𝑜𝑡(𝑧𝑖 , 𝛼𝑖 )

𝑐𝜃𝑖 −𝑠𝜃𝑖 𝑐𝛼𝑖 𝑠𝜃𝑖 𝑠𝛼𝑖 𝑎𝑖 𝑐𝜃𝑖

𝑠𝜃𝑖 𝑐𝜃𝑖 𝑐𝛼𝑖 −𝑐𝜃𝑖 𝑠𝛼𝑖 𝑎𝑖 𝑠𝜃𝑖
𝐴𝑖 (𝑞𝑖 ) = = 𝑇𝑖𝑖−1 (𝑞𝑖 )
0 𝑠𝛼𝑖 𝑐𝛼𝑖 𝑑𝑖
0 0 0 1

 Can be used iteratively for 𝑖 = 1,2, … , 𝑛 from base to end effector

𝜃 revolute
 𝑞𝑖 = ቊ 𝑖
𝑑𝑖 prismatic

20

10
 1/5/2024

21

Coordinate Frames (only z and x)

22

11
 1/5/2024

 Angular velocity
0 0
𝜔0,2 = 𝜔0,1 + 𝑅10 𝜔1,2
1 0
= 𝜔0,1 0
+ 𝜔1,2

Example: Let
0 𝑇
𝜔0,1 = 110
1 𝑇
𝜔1,2 = 011
If
0.7 −.7 0
𝑅10 = 0.7 0.7 0
0 0 1
0
What is 𝜔0,2 ?

23

Velocity Jacobians

 Cartesian and joint space velocities of the end effetor

𝜉ሶ = 𝐽𝑞ሶ
 As a one big n-column matrix
𝐽 = 𝐽1 , … , 𝐽𝑛 6×𝑛

 With a revolute joint the ith column

𝒛0𝑖−1 × 𝑜0,𝑛
0 0
− 𝑜0,𝑖−1 𝒛𝑖−1 × 𝑜𝑛 − 𝑜𝑖−1
𝐽𝑖 = → in short:
𝒛0𝑖−1 𝒛𝑖−1
6×1

 All in the base frame. For the prismatic joint the ith column
𝒛
𝐽𝑖 = 𝑖−1
0 6×1

24

12
 1/5/2024

 Two revolute joints the Jacobian is

𝒛0𝑖−1 × 𝑜0,2
0 0
− 𝑜0,𝑖−1
𝐽𝑖 = 𝑖 = 1,2
𝒛0𝑖−1 6×1

With
𝒛00 = 𝒛10 = 0, 0, 1 𝑇

0 𝑇
𝑜0,0 = 0,0,0
0 𝑇
𝑜0,1 = 𝑥1 , 𝑦1 , 0
0 𝑇
𝑜0,2 = 𝑥2 , 𝑦2 , 0
And
𝑥1 = 𝑎1 𝑐𝜃1 , 𝑦1 = 𝑎1 𝑠𝜃1
𝑥2 = 𝑎1 𝑐𝜃1 + 𝑎2 𝑐 𝜃1 + 𝜃2
𝑦2 = 𝑎1 𝑠𝜃1 + 𝑎2 𝑠 𝜃1 + 𝜃2

25

𝒛00 × 𝑜0,2
0 0
− 𝑜0,0 𝒛10 × 𝑜0,2
0 0
− 𝑜0,1
𝐽1 = 𝐽2 =
𝒛00 6×1
𝒛10 6×1
With
0 −1 0 𝑥2 −𝑎1 𝑠1 − 𝑎2 𝑠12
𝒛00 × 𝑜0,2
0 0
− 𝑜0,0 = 1 0 0 𝑦2 = 𝑎1 𝑐1 + 𝑎2 𝑐12
0 0 0 0 0
0 −1 0 𝑥2 −𝑎2 𝑠12
𝒛10 × 𝑜0,2
0 0
− 𝑜0,1 = 1 0 0 𝑦2 = 𝑎2 𝑐12
0 0 0 0 0
 The jacobian
−𝑎1 𝑠1 − 𝑎2 𝑠12 −𝑎2 𝑠12
𝑎1 𝑐1 + 𝑎2 𝑐12 𝑎2 𝑐12
0 0
𝐽= 0 0
0 0
1 1

26

13
 1/5/2024

Motion Planning Methods

 Complete Methods
 Exact topology of 𝐶𝑓𝑟𝑒𝑒 is available and used, costly though

27

Motion Planning Methods

 Grid methods
 Discretize C-space into grids/voxels.
 Works well with low dimensional problems, search and memory footprint
grows exponentially with the complexity of map
 Sampling methods
 Random or deterministic sampling of C-space, fast but optimal
 Potential fields
 Attract to the goal and repel from the obstacles via defined fields
 Local minima is a concern
 Nonlinear optimization
 In nonconvex space local minima is a concern, high computation need
 Smoothing is desired for jerky planners

28

14
 1/5/2024

Roadmap on Visibility Graphs

stop
start

29

Grid Methods

 A* may be used on a grid map as planner that is the shortest

if exists
 If multiple queries required for a given goal Wavefront
planner may be invoked, e.g. 4-connected grid map

30

15
 1/5/2024

A* Algorithm

 One of the most common graph search algoritm

 Define for every node
 past_cost: cost path from the start to the current node.
 heuristic_cost: optimistic cost path from the current node to the goal.
 Total_cost: cost path from the start node to the goal node.
 Parent: On the path from start to goal the preceding node
 Manage the search by the arrays
◼ OPEN: sorted list containing the nodes generated but have not been examined yet
◼ CLOSED: which contains the nodes with all (unclosed) neighbors are examined

31

Heuristic costs are bird-fly distance

5 Say, 1-6=20, {2,3,4}-6=10, 5-6=5
2
15 5
6
7
1 12 3
15
6 Start with OPEN: 1(20)
20
4
12

1 2 3 4 5 6
past_cost 0 ∞ ∞ ∞ ∞ ∞
heuristic_cost 20 10 10 10 5 0
Total_cost 20 ∞ ∞ ∞ ∞ ∞
Parent - - - - - -

32

16
 1/5/2024

RRT

33

Rapid Exploring Random Tree

𝑥𝑠𝑎𝑚𝑝𝑙𝑒

𝑥𝑛𝑒𝑤
𝑥𝑛𝑒𝑎𝑟𝑒𝑠𝑡

34

17
 1/5/2024

PRM – Probabilistic Random Map

 A roadmap is build randomly with k nearest neighbors first,

 Start and goal added with connections closest to the map
 Possibly disconnected graph is searched for a solution, A*?,
that is likely to be non-optimal, if exists

35

 Path
 Showing a way from a point A to point B in C-space
 No notion of time and velocity
 Trajectory
 Following a path with time and velocity constraints
 Motion defined by the trajectory must be able to satisfy
dynamic equations modeling the system with a feasible input
 A trajectory 𝑥 ∗ (𝑡) driven by a feasible 𝑢∗ (𝑡) satisfy
𝑥ሶ ∗ = 𝑓(𝑥 ∗ , 𝑢∗ )
 Limits of the actuators must be taken into account for
trajectory planning

36

18
 1/5/2024

Smooth Trajectories

 For general
𝑇
2
𝑥 ∗ 𝑡 = argmin න 𝑥 𝑛 𝑑𝑡
𝑥(𝑡) 0

 The derivative order 𝑛

 𝑛 = 1, Minimum velocity, (the shortest path)
 𝑛 = 2, Minimum acceleration
 𝑛 = 3, Minimum jerk
 𝑛 = 4, Minimum snap
𝑛 2
 𝐿= 𝑥 to satisfy Lagrange equation
𝜕𝐿 𝑑 𝜕𝐿 𝑑 2 𝜕𝐿 𝑑3 𝜕𝐿 𝑑𝑛 𝜕𝐿
− + 2 − 3 +⋯ =0
𝜕𝑥 𝑑𝑡 𝜕𝑥ሶ 𝑑𝑡 𝜕𝑥ሷ 𝑑𝑡 𝜕𝑥 3 𝑑𝑡 𝑛 𝜕𝑥 𝑛

37

Example

 Design a trajectory with minimum velocity and the following boundaries:

Time(sec) 0 2 10 50 100

Position(m) 0 4 30 100 1000

 For segment 2: 𝑥2 𝑡 = 𝑐1 𝑡 + 𝑐𝑜

1 2 𝑐𝑜 4
1 10 𝑐1 = 30
𝑐𝑜 1 10 −2 4 1 −20

𝑐1 = 8 −1 1 30
=
8 26

13 5
 𝑥2 𝑡 = 𝑡 − , 𝑡 ∈ [2,10]
4 2

38

19
 1/5/2024

Minimum Acceleration with Boundaries

39

Velocity Profile

 Splines require to specify arrival times, not always appropriate and

curvature and robot kinematics are not considered in general
 Instead using a path planner and speed profile offer more suitable
motion plan in some apps
 Some trajectories
 Trapezoidal
 S-curve (smoothed trapezoidal)

40

20
 1/5/2024

Single Joint Control

 Single joint controller must take the disturbance into account

 Control objectives are augmented as
 Reference tracking
 Disturbance rejection

41

DC Motor Actuator Dynamics

Motor model
𝜏𝑚 = 𝐾𝑚 𝑖𝑎
𝑉𝑏 = 𝐾𝑏 𝜔𝑚

42

21
 1/5/2024

 In practice 𝐿/𝑅 is negligible. A reasonable approx. model for 𝜃𝑚

43

Transient Control

 Find negative feedback controller so that the output tracks the reference
with a given combinations of desired
 %Overshoot (%OS)
 Settling time (Ts)
 Peak time (Tp)
 Rise time (Tr)
 These time domain requirements may be transformed to frequency domain
%𝑂𝑆 = 100 × 𝑒 −𝜁𝜋/ 1−𝜁 2

4
𝑇𝑠 ≈
𝜁𝜔𝑛
 The desired closed loop system’s damping ratio and natural frequency may
be achieved by placing the dominant poles at
𝑝1,2 = −𝜁𝜔𝑛 ± 𝜔𝑛 1 − 𝜁 2 ≝ 𝜎𝑑 ± 𝑗𝜔𝑑

44

22
 1/5/2024

Example

 If we want to joint angle 𝜃 follow the constant command with

 At most 4% overshoot
 Settle before 1 sec
 Where the dominant poles of the closed loop control system to be placed?
− ln 0.04
𝜁= = 0.7156
ln2 0.04 + 𝜋 2
4 4
𝜔𝑛 ≥ = = 5.56
𝑇𝑠 𝜁 0.7156
 Dominant poles must be at
𝑝1,2 = −𝜁𝜔𝑛 ± 𝜔𝑛 1 − 𝜁 2 = −4 ± 𝑗3.9
 The characteristic equation
Δ𝑑 𝑠 = 𝑠 + 4 + 𝑗3.9 𝑠 + 4 − 𝑗3.9 = 𝑠 2 + 8𝑠 + 31.24

45

Modeling Harmonic Drive

𝜃𝑙 𝑘/𝐽𝑚 𝐽𝑙
=
𝑇𝑚 𝑠 4 + 𝐵1 + 𝐵2 𝑠 3 + 𝑘 1 + 1 + 𝐵1 𝐵2 𝑠 2 + 𝑘 𝐵 + 𝐵 𝑠
𝐽𝑚 𝐽𝑙 𝐽𝑚 𝐽𝑙 𝐽𝑚 𝐽𝑙 𝐽𝑚 𝐽𝑙 1 2

46

23
 1/5/2024

 Robot dynamic model was given

𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏
 Joint space reference tracking
 Find 𝜏 = 𝜏𝑐𝑜𝑛𝑡𝑟𝑜𝑙 such that 𝑞, 𝑞ሶ follow 𝑞𝑑 , 𝑞ሶ 𝑑
 Cartesian space reference tracking
 Find 𝜏 = 𝜏𝑐𝑜𝑛𝑡𝑟𝑜𝑙 such that 𝑥, 𝑥ሶ follow 𝑥𝑑 , 𝑥ሶ 𝑑
 Wrapping the joint space controller
◼ Inverse kinematics 𝑥𝑑 , 𝑥ሶ 𝑑 → 𝑞𝑑 , 𝑞ሶ 𝑑 at reference and
◼ Forward kinematics 𝑞, 𝑞ሶ → 𝑥, 𝑥ሶ at output
𝑞𝑑 , 𝑞ሶ 𝑑 𝜏𝑐𝑜𝑛𝑡𝑟𝑜𝑙 𝑥, 𝑥ሶ
𝑥𝑑 , 𝑥ሶ 𝑑 𝑞, 𝑞ሶ
Inverse Forward
Controller Robot
Kinematics Kinematics

47

Computed Torque control

 The nonlinear rigid robot dynamics

𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏

 With the computed torque input

𝜏 = 𝐷 𝑞 𝑎 + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞

 The closed loop system become

𝐷 𝑞 𝑞ሷ = 𝐷(𝑞)𝑎

 Or

𝑞ሷ = 𝑎

 That is a linear system

48

24
 1/5/2024

Reference Joint Space Tracking

 Let the desired differentiable joint space vector be

𝑞𝑑 𝑡 , 𝑞ሶ 𝑑 , 𝑡 𝑞ሷ 𝑑 𝑑
 The reference tracking error
𝑒 = 𝑞𝑑 − 𝑞
 Now the CT auxiliary control signal be made
𝑎 = 𝑞ሷ 𝑑 + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒
The closed loop error system dynamics becomes
𝑒ሷ + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒 = 0

49

 System
𝐷 𝑞 𝑞ሷ + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞 = 𝜏
 Controller
𝜏 = 𝐷 𝑞 [𝑞ሷ 𝑑 + 𝐾𝑑 𝑒ሶ + 𝐾𝑝 𝑒] + 𝐶 𝑞, 𝑞ሶ 𝑞ሶ + 𝐺 𝑞
 For a transient response with maximum rise time without an
overshoot
𝜁 = 1, 𝜔𝑛
Desired poles
𝑠 2 + 2𝜁𝜔𝑛 𝑠 + 𝜔𝑛2 = 𝑠 2 + 2𝜔𝑛 𝑠 + 𝜔𝑛2 = 0
Implies
𝐾𝑝 = 𝜔𝑛2 , 𝐾𝑑 = 2𝜔𝑛

50

25
 INTRO TO ROS

What is ROS?
A software framework for programming

robots

Prototypes originated from Stanford robotics

lab, officially created and developed by

Willow Garage starting in 2007

Currently maintained by Open-Source

Robotics Foundation

Consists of infrastructure, tools, capabilities,

and ecosystem

Dr. Y. 1
 Advantages and Disadvantages of ROS

Advantages Disadvantages of ROS

 Provides lots of infrastructure, tools, and

 Approaching maturity, but still changing
capabilities for robot programming
 Security and scalability are not first-class
 Easy to try other people's work and share
concerns
your own
 OSes other than Ubuntu Linux are not well
 Large community
supported
 Free, open source, BSD license
Not great for mission-critical tasks
Great for open-source and researchers

❑ Disadvantageous addressed in the ROS2,

Improvements in ROS2
❑ Windows, macOS, now, have limited support

ROS Distros

Check the web site for current recommendations

Dr. Y. 2
 Installation

 There is OCW at TU-Delft for ROS1 good source for getting

started
 ROS1 Install instructions may be followed at
http://wiki.ros.org/ROS/Introduction
 The recommended versions are
 ROS Noetic on Ubuntu 20.04 (LTS 2024)

 ROS2 Humble on Ubuntu 22.04 (LTS May 2027)

Connected Graph Model

 ROS graphs’ nodes and edges

 processes are called nodes and

 the flow of info passed through the edges, named as topics

 Nodes can
 pass messages to one another through topics,

 make service calls to other nodes,

 provide a service for other nodes, or

 set or retrieve shared data from a common database called

the parameter server.

Dr. Y. 3
 Decentralized architecture

 The main process called the ROS Master, must be started first, that
 registers nodes,
 sets up peer-to-peer node-to-node communication for topics,
 controls parameter server updates
 Messages and service calls do not pass through the master

Master

top
Node1 Node2
ics

Basic Terminology

 Nodes be, e.g., “wheel odom”, “path planner”, “controller”

 Topics are named buses with data, e.g. “/odom”, “/cmd_vel”
 A node may advertise services for other nodes
 Any node may subscribe to topics

Topic 1 Topic 2
/odom /cmd_vel
Node 1 Node 2 Node 3
x: 0.1 Forward: 0.2
Wheel y: -1 Path Turn: 0 Motor
odometer z: 0 planner controller

Dr. Y. 4
 Services and Actions

 Services allow nodes to send a request and receive a

response
 Service calls are blocking, caller node waits for the response
 Actions are special non-blocking services

Parameter server

 A database shared between nodes for the class of low

overhead data
 does not change frequently

 accessed infrequently

 For instance
 distance between two fixed points in the environment,

 the weight of the robot,

 The gravity constant

 Map of environment

10

Dr. Y. 5
 Some ROS Key Terminologies

Messages: ROS data type used when subscribing or publishing to a topic.

Master: Name service for ROS (i.e. helps nodes find each other)

rosout: ROS equivalent of stdout/stderr

roscore: Master + rosout + parameter server (parameter server will be introduced later)

11

Some Tools

 rviz
 a three-dimensional visualization tool for robots, the environments, and
sensor data
 rosbag
 a command line tool used to record and playback ROS message data

 catkin
 the ROS build system based on CMake, and is similarly cross-platform,
open-source, and language-independent.
 roslaunch
 a tool used to launch multiple ROS nodes from multiple packages, as
well as setting parameters.
 rosbridge
 provides a JSON API to ROS functionalities for non-ROS programs

12

Dr. Y. 6
  rosrun is another command to launch single node from a single package if
roscore is running
 Whereas roslaunch can launch multiple nodes from multiple packages
 Additionally, roslaunch will automatically start roscore (if not running
already)
 roslaunch uses launch files, which can automatically start other programs
by including other launch files, set some parameters etc.

13

ROS Catkin Package

System ROS environment is set by

$ source /opt/ros/noetic/setup.bash
Typically added into ~/.bashrc file not to type it in all terminal instances

 Catkin environment to be created into a fresh directory

$ mkdir -p ~/catkin_ws/src
$ cd ~/catkin_ws/
$ catkin_make
To make newly built nodes be available, must run after each build
$ source devel/setup.bash

14

Dr. Y. 7
 Packages in a catkin Workspace

15

ROS File System

Packages:
The lowest level of ROS software
organization that can contain any of
libraries, tools, executables, etc.

Pack. Manifest:
A manifest is a description of a package. Its
most important role is to define
dependencies between packages, author,
license, dependencies, compilation flags,
and so on.

Metapacks/Stacks:
Stacks are collections of packages that form
a higher-level library. E.g., ROS Navigation
Stack

Services:
request/reply interaction between
processes

16

Dr. Y. 8
 Now you can create a package in the empty src directory using catkin_create_pkg

$ cd ~/catkin_ws/src

$ catkin_create_pkg my_pkg std_msgs rospy roscpp

# syntax: catkin_create_pkg <package_name> [dep1] [dep2]

aydin@ayG:~/catkin_ws/src$ tree

├── CMakeLists.txt -> /opt/ros/noetic/share/catkin/cmake/toplevel.cmake

└── my_pkg

├── CMakeLists.txt

├── include

│ └── my_pkg

├── package.xml

└── src

17

Config the
Package.xml file package.xml

Define package owner etc. in the xml file

18

Dr. Y. 9
 Building the execs

19

roscore

Run roscore to start the ROS master

20

Dr. Y. 10
 ROS Command-line

21

22

Dr. Y. 11
 rosmsg/rossrv displays Message/Service (msg/srv) data
structure definitions.
Commands:
rosmsg show Display the fields in the msg.
rosmsg users Search for code using the msg.
rosmsg md5 Display the msg md5 sum.
rosmsg package List all the messages in a package.
rosnode packages List all the packages with messages.
Examples:
Display the Pose msg:
$ rosmsg show Pose
List the messages in nav msgs:
$ rosmsg package nav msgs
List the files using sensor msgs/CameraInfo:
$ rosmsg users sensor msgs/CameraInfo

23

Wiki Tutorials

 In a new terminal, run:

$ roscore
 In a new terminal, run:

$ rosrun turtlesim turtlesim_node

 The turtlesim window will open:

24

Dr. Y. 12
 Example

rostopic type [topic]

$ rostopic type /turtle1/cmd_vel
geometry_msgs/Twist

$ rosmsg show geometry_msgs/Twist

geometry_msgs/Vector3 linear

float64 x

float64 y

float64 z

geometry_msgs/Vector3 angular

float64 x

float64 y

float64 z

25

Message Types

 ROS uses a simplified messages description language for

describing the data values (aka messages) that ROS nodes
publish.
 This description makes it easy for ROS tools to automatically
generate source code for the message type in several target
languages.
 Message descriptions are stored in .msg files in the msg/
subdirectory of a ROS package.
 Allows nodes written in C++ and Python to communicate with
each other

26

Dr. Y. 13
 Publishing to a Topic thru cmd line

Publishing to a topic from shell

rostopic pub [topic] [msg_type] [args]

$ rostopic pub -1 /turtle1/cmd_vel geometry_msgs/Twist -- '[2.0, 0.0, 0.0]'

'[0.0, 0.0, 1.8]’

In -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]’

-- tells that following two vectors are not options

First string is the linear velocity in YAML syntax

Second string is the angular velocity

-1 tells to publish once and exit

27

Repetitive Message Publishing -r #n

$ rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, -1.8]'

28

Dr. Y. 14
 Graph Visualization

 Assume that we have roscore running

 In another terminal typing
$ rosrun rqt_graph rqt_graph
 Illustrates the nodes/topics graph

29

PLOT Visualization

Visualize the Simulation Plots

 $ rosrun rqt_plot rqt_plot

30

Dr. Y. 15
 Publishing and subscribing

● Any node can publish a message to any topic

● Any node can subscribe to any topic
● Multiple nodes can publish to the same topic
● Multiple nodes can subscribe to the same topic
● A node can publish to multiple topics
● A node can subscribe to multiple topics

31

Template ROS Node Code

 #include "ros/ros.h“
 Name of node

 @50Hz

32

Dr. Y. 16
 Launch Files

 Typically, located under “launch” directory as “.launch” file

 $ roslaunch <package_name> <launch_file>
 A launch file
<launch>
<node name="map_server" pkg="map_server" type="mapserver" />
<node name="stageros" pkg="stage" type="stageros" args="$(find
navigation_stage)/stage config/worlds/willow-pr2-5cm.world" >
<param name="base_watchdog_timeout" value="0.2"/>
</node>
<include file="$(find navigation_stage)/move_base_config/amcl_node.xml"/>
</launch>

33

Transforms (TF2 package)

 Coordinate frames are represented as a graph with

 the transforms as edges
 the coordinate frames as nodes
 Launch a demo
$ roslaunch turtle_tf turtle_tf_demo.launch
 To visualize the frames on the running ROS
$ rosrun tf view_frames
$ evince frames.pdf

34

Dr. Y. 17
  The frames.pdf file

35

 Real-time coordinate transforms visualization

$ rosrun rqt_tf_tree rqt_tf_tree

36

Dr. Y. 18
  Find the transform between two frames
rosrun tf tf_echo [reference_frame] [target_frame]
 For instance
$ rosrun tf tf_echo turtle1 turtle2
At time 1568050753.324
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.344, 0.939]
in RPY (radian) [0.000, -0.000, 0.703]
in RPY (degree) [0.000, -0.000, 40.275]

37

 Any node can broadcast or listen transforms defining the

coordinate frames
 Frames are defined w.r.t. another (base) frame
 Frames are handled differently if
 Stationary /tf_static

 Moving /tf

 Stationary frames are defined thru static_transform_publisher

 rosrun tf2_ros static_transform_publisher x y z yaw pitch roll
parent_frame child_frame
 Moving frames may be defined thru robot_ state_publisher

38

Dr. Y. 19
  In the URDF file each joint can be defined as fixed or movable
(e.g. continuous spinning, limited rotation, linear sliding).
 robot_state_publisher needs /joint_states to publish moving
frames

39

$ rosrun robot_state_publisher robot_state_publisher --

ros-args -p robot_description:="$(xacro

path/to/my/file.urdf.xacro)“

$ rosrun joint_state_publisher_gui joint_state_publisher_gui

$ rosrun tf2_tools view_frames.py

40

Dr. Y. 20
 Rviz -- 3D Visualization

$ rviz

41

Robot Description

 Robot structure info including

 Joints type and parent/child frames

 Geometry of links

 Physical parameters mass, inertia etc.

 Needed to be defined in a, preferably, human readable form

 There are many syntaxes, a popular one is URDF written in
XML

42

Dr. Y. 21
 Unified Robot Description Format (URDF)

 Derived from Simulation Description Format

 May have additional fields like
 Dynamics (/inertial)
 Mass and inertia parameters interpreted in
Newton-Euler models
 Visual appearance (/visual)
 Geometry
 Color
 Texture etc.
 Collision (/collision)
 Defines the collision zone in geometry

43

Model

 Not all mechanisms are supported

44

Dr. Y. 22
  Simple URDF 1 DoF
 XML syntax is used to define
fields
 Minimal info for the
Transformations (coordinate
frames) shown
 Kinematically consistent open-
chain robots are supported

45

Links

 May have elements

 <inertia> …. </inertia>
 <visual> …. </visual>
 <collision> …. </ collision >
 Each with attribute
 <origin …..> defining the frame

46

Dr. Y. 23
 Joints

<joint name="my_joint" type="floating">

<origin xyz="0 0 1" rpy="0 0 3.1416"/>
<parent link="link1"/>
<child link="link2"/>
…
</joint>

47

Joint Types

 revolute — rotational with limited range

 continuous — rotational without constraints

 prismatic — limited range sliding joint

 fixed — None moving

 floating — this joint allows 3D motion for all 6 degrees of freedom.

 planar — this joint allows 2D motion in a plane perpendicular to the

axis

48

Dr. Y. 24
  Industrial robots URDF (XACRO) files may be generated using

STL files from 3D CAD models

 Many assembly CAD programs, such as SolidWorks, support

direct exporting the design to URDF

49

Dr. Y. 25
 12/13/2023

PATH&TRAJECTORY PLANNING

Some Definitions

Configuration: 𝑞 ∈ 𝐶
C-space: 𝐶 = 𝐶𝑓𝑟𝑒𝑒 ∪ 𝐶𝑜𝑏𝑠
State: 𝑥 = 𝑞 (if the input is velocity)
𝑇
𝑥= 𝑞 𝑣 ∈ 𝑋 if the input is force
 Equations of motion of a robot
𝑥ሶ = 𝑓 𝑥, 𝑢 𝑢∈𝑈
 with a solution
𝑡
𝑥 𝑡 = 𝑥𝑜 + න 𝑓 𝑥, 𝑢 𝑑𝜏 , ∀𝑡 ∈ [𝑡𝑜 , 𝑇]
𝑡𝑜

1
 12/13/2023

Motion Planning

 Problem Statement:
 Given
 an initial state 𝑥𝑜 = 𝑥𝑠𝑡𝑎𝑟𝑡 and
 a desired state 𝑥𝑇 = 𝑥𝑔𝑜𝑎𝑙
 Find 𝑇 and 𝑢: 𝑡𝑜 , 𝑇 → 𝑈 such that
 𝑥 𝑡 ȁ𝑡=𝑇 = 𝑥𝑔𝑜𝑎𝑙 and
 𝑞 𝑡 ∈ 𝐶𝑓𝑟𝑒𝑒 for all 𝑡 ∈ [0, 𝑇].

Motion Planner

 Path planning is a geometric problem

 Find 𝑞 𝑠 , 𝑠 ∈ [0,1] with 𝑞 0 = 𝑞𝑠𝑡𝑎𝑟𝑡 and 𝑞 1 = 𝑞𝑔𝑜𝑎𝑙 .
 It is assumed, path can be time scaled to create a feasible
trajectory with robot dynamics, travel time, input constraints
 If there are fewer control inputs than the DoF, motion may not
be realized even if it is in 𝐶𝑓𝑟𝑒𝑒
 Responsiveness of the planner to account for fast changing
environment, online vs. offline?
 Optimal?
 Exact vs. approximate?

2
 12/13/2023

Motion Planning Methods

 Complete Methods
 Exact topology of 𝐶𝑓𝑟𝑒𝑒 is available and used, costly though

Motion Planning Methods

 Complete Methods
 Exact topology of 𝐶𝑓𝑟𝑒𝑒 is available and used, costly though

3
 12/13/2023

Motion Planning Methods

 Grid methods
 Discretize C-space into grids/voxels.
 Works well with low dimensional problems, search and memory footprint
grows exponentially with the complexity of map
 Sampling methods
 Random or deterministic sampling of C-space, fast but optimal
 Potential fields
 Attract to the goal and repel from the obstacles via defined fields
 Local minima is a concern
 Nonlinear optimization
 In nonconvex space local minima is a concern, high computation need
 Smoothing is desired for jerky planners

Obstacle Space

 Circular robot to point mass in 𝐶 space

4
 12/13/2023

Collision Detection

 Given a ball of B in 𝐶-Obstacle and a configuration 𝑞, let

𝑑(𝑞, 𝐵) be the distance between the robot and the obstacle,
s.t.
𝑑 𝑞, 𝐵 > 0 ⇒ 𝑁𝑜 𝑐𝑜𝑛𝑡𝑎𝑐𝑡
𝑑 𝑞, 𝐵 = 0 ⇒ 𝐶𝑜𝑛𝑡𝑎𝑐𝑡
𝑑 𝑞, 𝐵 < 0 ⇒ 𝐶𝑜𝑙𝑙𝑖𝑠𝑖𝑜𝑛
 Example
 Robot defined by radius 𝑅𝑖 balls centered at 𝑟𝑖 (𝑞)
 Obstacles defined by radius 𝑅𝑗 balls centered at 𝑏𝑗
𝑑 𝑞, 𝐵 = min ȁȁ𝑟𝑖 𝑞 − 𝑏𝑗 ȁȁ − 𝑅𝑖 − 𝑅𝑗
𝑖,𝑗

Graphs and Trees

 Many planner represent 𝐶-space by graphs or trees to search

for a collision free path
 Graphs are composed of Nodes and Edges

leafs

 (a) Directed (b) Undirected (c) tree

10

5
 12/13/2023

Complete Path Planners

 Applicable when the free C-space is available

 High Dim 𝐶𝑓𝑟𝑒𝑒 is represented by a 1D roadmap 𝑅 with
 Reachability: from every 𝑞 ∈ 𝐶𝑓𝑟𝑒𝑒 a 𝑞 ′ ∈ 𝑅 can be found
 Connectivity: For each connected component of 𝐶𝑓𝑟𝑒𝑒 there is one
connected component of 𝑅
 Typically, hard to find 𝑅 some problems are suitable using the visibility
graphs where
 Nodes are the vertices of inflated 𝐶𝑜𝑏𝑠
 Edges weighted by the Euclidean distances between nodes that can see
each other without an occlusion of an obstacle
 A* like graph search algos are applicable on such a graph

11

Roadmap on Visibility Graphs

stop
start

12

6
 12/13/2023

A* Algorithm

 One of the most common graph search algoritm

 Define for every node
 past_cost: cost path from the start to the current node.
 heuristic_cost: optimistic cost path from the current node to
the goal.
 Total_cost: cost path from the start node to the goal node.
 Parent: On the path from start to goal the preceding node
 Manage the search by the arrays
◼ OPEN: sorted list containing the nodes generated but have not been examined yet
◼ CLOSED: which contains the nodes with all (unclosed) neighbors are examine

13

A* Algorithm – Development

 YouTube like platform house some coding solutions

 https://youtu.be/JtiK0DOeI4A for a python implementation
 Web links like The Insider's Guide to A* Algorithm in Python - Python Pool
 Graphs in Python - Theory and Implementation - A* Search Algorithm
(stackabuse.com)
 AI generated codes using LLMs e.g. ChatGPT with a prompt such as
 I need help understanding how to write a python program that will
achieve A* algorithm.
 I need help understanding the basics of python code and how to apply it
to develop A* algorithm.
 Can you review my python program and give me some feedback on
how to improve it?

14

7
 12/13/2023

Heuristic costs are bird-fly distance

5 Say, 1-6=20, {2,3,4}-6=10, 5-6=5
2
15 5
6
7
1 12 3
15
6 Start with OPEN: 1(20)
20
4
12

1 2 3 4 5 6
past_cost 0 ∞ ∞ ∞ ∞ ∞
heuristic_cost 20 10 10 10 5 0
Total_cost 20 ∞ ∞ ∞ ∞ ∞
Parent - - - - - -

15

Node 1 Search

5
2
15 5
6
7
1 12 3
15
6
20
4 OPEN: 1(20), 2(25)
12

1 2 3 4 5 6
past_cost 0 15
∞ ∞ ∞ ∞ ∞
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25
∞ ∞ ∞ ∞ ∞
Parent - 1- - - - -

16

8
 12/13/2023

5
2
15 5
6
7
1 12 3
15
6 Open list sorted by the cost
20
4 OPEN: 1(20), 3(22), 2(25)
12

1 2 3 4 5 6
past_cost 0 15 12 ∞ ∞ ∞
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 ∞ ∞ ∞
Parent - 1 1 - - -

17

5
2
15 5
6
7
1 12 3
15
6
20
4 OPEN: 1(20), 3(22), 2(25), 4(30)
12
Now node 1 closed

1 2 3 4 5 6
past_cost 0 15 12 20 ∞ ∞
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 ∞ ∞
Parent - 1 1 1 - -

18

9
 12/13/2023

5
2
15 5
6
7
1 12 3
15 The next search for shortest node
6
20
4 OPEN: 3(22), 2(25), 4(30)
12
CLOSED: 1(20)

1 2 3 4 5 6
past_cost 0 15 12 20 ∞ ∞
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 ∞ ∞
Parent - 1 1 1 - -

19

5
2
15 5
6
7
1 12 3
15
6
20
4 OPEN: 3(22), 5(23), 2(25), 4(30)
12
CLOSED: 1(20)

1 2 3 4 5 6
past_cost 0 15 12 20 18
∞ ∞
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 23
∞ ∞
Parent - 1 1 1 3- -

20

10
 12/13/2023

5
2
15 5
6
7
1 12 3
15
6
20
4 OPEN: 3(22), 5(23), 2(25), 6(27),4(30)
12
CLOSED: 1(20)

1 2 3 4 5 6
past_cost 0 15 12 20 18 27
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 23 27
Parent - 1 1 1 3 3

21

5
2
15 5
6
7
1 12 3
15
6
20
4 OPEN: 5(23), 2(25), 6(27),4(30)
12
CLOSED: 1(20), 3(22)

1 2 3 4 5 6
past_cost 0 15 12 20 18 27
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 23 27
Parent - 1 1 1 3 3

22

11
 12/13/2023

5
2
15 5
6
7
1 12 3
15
6
20
4 OPEN: 5(23), 2(25), 6(27),4(30)
12
CLOSED: 1(20), 3(22)

1 2 3 4 5 6
past_cost 0 15 12 20 18 27
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 23 27
Parent - 1 1 1 3 3

23

5
2
15 5
6
7
1 12 3
15
6
20
4 OPEN: 5(23), 6(25), 2(25), 4(30)
12
CLOSED: 1(20), 3(22)

1 2 3 4 5 6
past_cost 0 15 12 20 18 27 25
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 23 27 25
Parent - 1 1 1 3 35

24

12
 12/13/2023

5
2
15 5
6
7
1 12 3
15
6
20
4 OPEN: 6(25), 2(25), 4(30)
12
CLOSED: 1(20), 3(22), 5(23)

1 2 3 4 5 6
past_cost 0 15 12 20 18 25
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 23 25
Parent - 1 1 1 3 5

25

Parent(6)=5
Parent(5)=3
5
2 Parent(3)=1
15 5
6
7 Path:1 → 3 → 5 → 6
1 12 3 Cost: 25
15
6
20
4 OPEN: 6(25), 2(25), 4(30)
12
CLOSED: 1(20), 3(22), 5(23)

1 2 3 4 5 6
past_cost 0 15 12 20 18 25
heuristic_cost 20 10 10 10 5 0
Total_cost 20 25 22 30 23 25
Parent - 1 1 1 3 5

26

13
 12/13/2023

Notes

 If heuristic estimate is larger than the actual cost you may not
get an optimal path
 May pick Euclidean distance for those heuristic estimates
 Widely used in autonomous mobile robot applications

27

A* Graph Search

28

14
 12/13/2023

Grid Methods

 A* may be used on a grid map as planner that is the shortest

if exists
 If multiple queries required for a given goal Wavefront
planner may be invoked, e.g. 4-connected grid map

29

Multi-Resolution Grid

 Uniform sampling of C space bring excessive search time and map memory
footprint that are not desired complex maps
 Multi resolution grid technique can reduce the complexity
 Each dimension is split in half to turn a cell into 22𝑛 cells
 If there is an obstacle in a cell, keep splitting till the maximum resolution
else don’t spit
 2D grids divided 2𝑛 = 4 cells at each run is called quadtree
 3D grid cells (voxels) divided 2𝑛 = 8 cells called an octree

30

15
 12/13/2023

Multi-Resolution Grid

 10 cell searched in multi-resolution tree vs

 64 cells at the single resolution tree

31

Other Search Algos

 Dijkstra’s algorithm:
 If the heuristic cost-to-go is always estimated as zero then
A∗ always explores from the OPEN node that has been
reached with minimum past cost. Naturally slower than A*
 Breadth-first search:
 If each edge has the same cost, Dijkstra’s algorithm reduces
to breadth-first search.
 All nodes one edge away from the start node are
considered first, then all nodes two edges away, etc. The
first solution found is, therefore, a minimum-cost path.

32

16
 12/13/2023

A Wheeled Mobile Robot Planner

33

Illustration

(a) (b)
(a) Each action has the same cost
(b) Reverse motion is penalized in the cost

34

17
 12/13/2023

Sampling Methods

 High dimensional grid searches are computationally expensive

 Random or deterministic sampler may offer efficient but
optimal solutions
 Common ones
 RRT: Rapidly exploring Random Trees
 PRM: Probabilistic Road Maps

35

36

18
 12/13/2023

RRT

 Line 3. Sampler: The most obvious sampler is uniform distro

over C-space, 𝑥𝑠𝑎𝑚𝑝 , Deterministic Van der Corput sampling
may be selected as well
 Line 4. Nearest Node in T to 𝑥𝑠𝑎𝑚𝑝 is 𝑥𝑛𝑒𝑎𝑟𝑒𝑠𝑡 . How to define
nearest when one state is position the other is orientation?
 Line 5: Grow T by 𝑥𝑛𝑒𝑤 at a small distance 𝑑 from 𝑥𝑛𝑒𝑎𝑟𝑒𝑠𝑡 in
the direction of 𝑥𝑠𝑎𝑚𝑝 on a straight line
 Local planner maybe
◼ Straight line
◼ Curve+Straight

 If the motion from 𝑥𝑛𝑒𝑎𝑟𝑒𝑠𝑡 is collision free add 𝑥𝑛𝑒𝑤 to the

tree T

37

Rapid Exploring Random Tree

𝑥𝑠𝑎𝑚𝑝𝑙𝑒

𝑥𝑛𝑒𝑤
𝑥𝑛𝑒𝑎𝑟𝑒𝑠𝑡

38

19
 12/13/2023

 Illustration

Random Walk vs RRT

39

 Once goal is reached, we stop at RRT but if continued we may

get a shorter path in RRT*

40

20
 12/13/2023

PRM – Probabilistic Random Map

 A roadmap is build randomly with k nearest neighbors first,

 Start and goal added with connections closest to the map
 Possibly disconnected graph is searched for a solution, e.g. A*,
that is likely to be non-optimal, if exists

41

42

21
 12/13/2023

43

44

22
 12/13/2023

 Unconnected islands on the map is a possibility to watch

45

Nonlinear Optimization

 Motion planning problem was defined as

Find 𝑢 𝑡 ,𝑞 𝑡 ,𝑇
Minimizing 𝐽(𝑢, 𝑞, 𝑇)
Subject to 𝑥ሶ = 𝑓(𝑥, 𝑢) ∀𝑡 ∈ [0, 𝑇]
𝑢 𝑡 ∈𝑈 ∀𝑡 ∈ [0, 𝑇]
𝑞 𝑡 ∈ 𝐶𝑓𝑟𝑒𝑒 ∀𝑡 ∈ [0, 𝑇]
𝑥 0 = 𝑥𝑠𝑡𝑎𝑟𝑡
𝑥 𝑇 = 𝑥𝑔𝑜𝑎𝑙

 That is a nonlinear optimization problem

 Local minima is a big concern

46

23
 12/13/2023

 Path
 Showing a way from a point A to point B in C-space
 No notion of time and velocity
 Trajectory
 Following a path with time and velocity constraints
 Motion defined by the trajectory must be able to satisfy
dynamic equations modeling the system with a feasible input
 A trajectory 𝑥 ∗ (𝑡) driven by a feasible 𝑢∗ (𝑡) satisfy
𝑥ሶ ∗ = 𝑓(𝑥 ∗ , 𝑢∗ )
 Limits of the actuators must be taken into account for
trajectory planning

47

 There are infinite possible paths from Start to Goal reachable

at finite time

48

24
 12/13/2023

Path

 Why a smooth path?

 Reduce vibrations and high frequency motion likely unmodeled
 Reducing the peaks in velocity, acceleration, jerk minimizes
actuator power, size and cost
 What is smooth?
 Differentiable, all derivatives are continuous and bounded
 𝑠 𝑡 = sin 𝜔𝑡 𝑡 ∈ [0, 𝑇]
 𝑠 𝑡 = σ𝑛𝑖=0 𝑐𝑖 𝑡 𝑖 𝑡 ∈ [0, 𝑇]
 For instance, to achieve smoothnes in position, velocity, acceleration
at the two boundary conditions, we get 6 constraint equations
 So, 𝑛 = 5 quintic polynomials may be used for such a trajectory to
meet the 6 boundary conditions

49

Some Calculus of Variations

𝑇
∗
𝑥 𝑡 = argmin න 𝐿 𝑥,ሶ 𝑥, 𝑡 𝑑𝑡
𝑥(𝑡) 0

 Functionals
𝑇
 Shortest distance path is 𝑥 ∗ 𝑡 = argmin ‫׬‬0 𝑥ሶ 2 𝑑𝑡
𝑥(𝑡)
𝑇
 Least energy: argmin ‫׬‬0 [𝐾 𝑥,ሶ 𝑥, 𝑡 − 𝑃(𝑥, 𝑡)]𝑑𝑡
𝑥(𝑡)

50

25
 12/13/2023

𝑇
∗
𝑥 𝑡 = argmin න 𝐿 𝑥,ሶ 𝑥, 𝑡 𝑑𝑡
𝑥(𝑡) 0

Lagrange Equations
Necessary conditions satisfied by the ‘optimal’ function
𝑑 𝜕𝐿 𝜕𝐿
− =0
𝑑𝑡 𝜕 𝑥ሶ 𝜕𝑥

51

Smooth Trajectories

 For general
𝑇
2
𝑥 ∗ 𝑡 = argmin න 𝑥 𝑛
𝑑𝑡
𝑥(𝑡) 0

 The order 𝑛
 𝑛 = 1, Minimum velocity, (the shortest path)
 𝑛 = 2, Minimum acceleration
 𝑛 = 3, Minimum jerk
 𝑛 = 4, Minimum snap
𝑛
 𝑥 is referred as the input

52

26
 12/13/2023

Smooth Trajectories

𝑇
∗ 𝑛 2
𝑥 𝑡 = argmin න 𝑥 𝑑𝑡
𝑥(𝑡) 0

 Minimum velocity’ happens to be the shortest path

 𝑥 0 = 𝑥𝑜 , 𝑥 𝑇 = 𝑥𝑇 , 𝑖𝑛𝑝𝑢𝑡 𝑢 = 𝑥ሶ 𝑡 → 𝐿 = 𝑥ሶ 2
𝑑 𝜕𝐿 𝜕𝐿
 Lagrange equations 𝑑𝑡 − 𝜕𝑥 = 0
𝜕𝑥ሶ

 2𝑥ሷ = 0 → 𝑥 𝑡 = 𝑐1 𝑡 + 𝑐𝑜
 The unknown coefficients found thru the boundary conditions

53

First Order Trajectory

 Note that the optimal solution family

𝑥 𝑡 = 𝑐1 𝑡 + 𝑐𝑜
 Straight line (shortest path)
 Constant velocity
 At the boundaries
 𝑥𝑜 = 𝑐𝑜
 𝑥𝑇 = 𝑐1 𝑇 + 𝑐𝑜 → 𝑐1 = (𝑥𝑇 −𝑥𝑜 )/𝑇
𝑥𝑇 − 𝑥𝑜
𝑥 𝑡 = 𝑡 + 𝑥𝑜
𝑇
 Example: Let 𝑥𝑜 = 4, 𝑥𝑇 = 100, 𝑇 = 12 ⇒ 𝑥 𝑡 = 8𝑡 + 4

54

27
 12/13/2023

Minimum Path

 𝑑𝑠 = 𝑑𝑥 2 + 𝑑𝑡 2
𝑑𝑥 2
 = +1
𝑑𝑡

 = 𝑥ሶ 2 + 1
 Length of curve
𝑇
 ‫׬ = 𝑠𝑑 ׬‬0 𝑥ሶ 2 + 1

 𝐿 = 𝑥ሶ 2 + 1

55

56

28
 12/13/2023

 Integrate it to get
𝑥ሶ
=𝛼
𝑥ሶ 2 + 1
 Or

𝛼2
𝑥ሶ = ≝ 𝑐1
1 − 𝛼2
 Integrate this
𝑥 = 𝑐1 𝑡 + 𝑐𝑜
 The same solution as the minimum velocity functional

57

 Straight lines provide continues path to follow

 However, the velocities may be discontinuous at the boundary
 Maybe difficult or impossible to find a trajectory for the robot
to realize such a motion
 So, higher order paths may be needed for a smooth motion

58

29
 12/13/2023

Minimum Acceleration n=2

𝑇
𝑥 𝑡 = argmin න 𝑥ሷ 2 𝑑𝑡
∗
𝑥(𝑡) 0

 The functional, 𝐿 = 𝑥ሷ 2
 Euler-Lagrange equations
𝜕𝐿 𝑑 𝜕𝐿 𝑑2 𝜕𝐿
− + 2 =0
𝜕𝑥 𝑑𝑡 𝜕 𝑥ሶ 𝑑𝑡 𝜕𝑥ሷ

𝑑2
2𝑥ሷ = 2𝑥 (4) = 0
𝑑𝑡 2
Cubic splines obtained after four integrations
𝑥 = 𝑐3 𝑡 3 + 𝑐2 𝑡 2 + 𝑐1 𝑡 + 𝑐0

59

Cubic splines
𝑥 = 𝑐3 𝑡 3 + 𝑐2 𝑡 2 + 𝑐1 𝑡 + 𝑐0
 Requires four independent equations to solve four unknowns
 Desired positions: 𝑥𝑜 = 𝑥 0 , 𝑥𝑇 = 𝑥(𝑇) yield 2 eqns.
 𝑡 = 0 → 𝑥𝑜 = 𝑐0
 𝑡 = 𝑇 → 𝑥𝑇 = 𝑐3 𝑇 3 + 𝑐2 𝑇 2 + 𝑐1 𝑇 + 𝑐0
 So the desired velocities 𝑣𝑜 = 𝑣 0 , 𝑣𝑇 = 𝑣(𝑇) needed to be
specified as well for a unique solution
 𝑡 = 0 → 𝑣𝑜 = 𝑐1
 𝑡 = 𝑇 → 𝑣𝑇 = 3𝑐3 𝑇 2 + 2𝑐2 𝑇 + 𝑐1

60

30
 12/13/2023

Minimum Jerk Trajectory (n=3)

𝑇
∗ 3 2
𝑥 𝑡 = argmin න 𝑥 𝑑𝑡
𝑥(𝑡) 0
3
 𝐿=𝑥
 Euler-Lagrange equations
𝜕𝐿 𝑑 𝜕𝐿 𝑑2 𝜕𝐿 𝑑3 𝜕𝐿
− + 2 − 3 =0
𝜕𝑥 𝑑𝑡 𝜕 𝑥ሶ 𝑑𝑡 𝜕 𝑥ሷ 𝑑𝑡 𝜕𝑥 (3)

𝑑3
2𝑥 (3) = 2𝑥 (6) = 0
𝑑𝑡 3

𝑥 = 𝑐5 𝑡 5 + 𝑐4 𝑡 4 + 𝑐3 𝑡 3 + 𝑐2 𝑡 2 + 𝑐1 𝑡 + 𝑐0

62

 Six unknown requires six independent equations

 They are found through the boundary conditions
 Initial position and the final position -> two equations
 Initial and final velocities needed for two more equations
 Initial and final accelerations get us other two equations

63

31
 12/13/2023

𝑥 = 𝑐5 𝑡 5 + 𝑐4 𝑡 4 + 𝑐3 𝑡 3 + 𝑐2 𝑡 2 + 𝑐1 𝑡 + 𝑐0
 Boundary conditions

time Position Velocity Accelerations

t=0 a 0 0
t=T b 0 0

 Equations:

64

 𝐴𝑞 = 𝑏
10 0 0 0 0 𝑐0 𝑥𝑜
01 0 0 0 0 𝑐1 𝑥ሶ 𝑜
00 2 0 0 0 𝑐2 𝑥ሷ 𝑜
𝑐3 = 𝑥𝑇
1 𝑇 𝑇2 𝑇3 𝑇4 𝑇5
01 2𝑇 3𝑇 2 4𝑇 3 5𝑇 4 𝑐4 𝑥ሶ 𝑇
00 2 6𝑇 12𝑇 2 20𝑇 3 𝑐5 𝑥ሷ 𝑇
 Solved for polynomial coefficients
𝑞 = 𝐴−1 𝑏

65

32
 12/13/2023

Smooth 3rd order (Minimum Jerk)

 𝑥ሶ 𝑚𝑎𝑥 = 0.0189 and 𝑥ሶ 𝑎𝑣𝑒 = 0.01 not very efficient use of

motors

66

Trapezoidal Speed Profile

 Commonly used for motor trajectories

 Trapezoidal
 S-curve

Segment 4 Constant velocity

Segments 2,6 Constant acceleration
Segments 1,3,5,7 Constant jerk

67

33
 12/13/2023

Performance Comparison

 To reach at the same destination

 30% less max speed needed in trapezoidal
 96% max. acceleration needed in trapezoid
 Motor efficiency better utilized in trapezoidal

68

Trajectory Travelling Waypoints

69

34
 12/13/2023

𝑥𝑖 − 𝑥𝑖−1 𝑡𝑖 𝑥𝑖−1
𝑥𝑖 𝑡 = 𝑡+ , 𝑖 = 1, … , 𝑚
𝑡𝑖 − 𝑡𝑖−1 𝑡𝑖 − 𝑡𝑖−1

70

Minimum Acceleration for 2nd order Systems

71

35
 12/13/2023

𝑡1 𝑡𝑚
𝑥 ∗ 𝑡 = argmin න 𝑥ሷ 2 𝑑𝑡 + ⋯ + න 𝑥ሷ 2 𝑑𝑡
𝑥(𝑡) 𝑡𝑜 𝑡𝑚−1

 For the second order (n=2) system with m via points we have
4𝑚 unknown parameters in our desired trajectory

72

 Each of the 𝑚 segments has two boundary positions, ⇒ # of equations

2𝑚
 For a smooth motion, the intermediate 𝑚 − 1 nodes must satisfy both
 Velocity
 Acceleration
Continuity constraints ⇒ # of equations
2(𝑚 − 1)

73

36
 12/13/2023

The number of equations

 4𝑚 − 2 equations are at

 But need two more independent equations to solve 4𝑚 params

74

The Solution of Cubic Splines

 2𝑚 + 2 𝑚 − 1 + 2

75

37
 12/13/2023

Splines for nth Order Systems

77

38
 10/5/2023

Robot Engineering
Fall 2023
Assoc. Prof. Aydin Yesildirek

What is a Robot?
• An electromechanical device with multiple degrees-of-
freedom (DOF) that is programmable to accomplish a
variety of (motion) tasks.
✓ Industrial robots: multi-functional manipulator
designed to move materials, parts, tools, or
special devices in an industrial environment with
a fixed reference
✓ Mobile robots: an electromechanical system in
motion without a fixed reference
✓ Humanoid (biped) robots

1
 10/5/2023

Robots in Industry

350 B.C

Robot History The Greek mathematician, Archytas builds a mechanical

bird named "the Pigeon" that is propelled by steam.

200 B.C.

The Greek inventor and physicist Ctesibus of Alexandria

designs water clocks that have movable figures on them.

2
 10/5/2023

12th Century
Al-Jazari’s
Mechanisms

1495 - Leonardo Da Vinci’s designs

Had some mechanisms

with some joints and
body parts.

3
 10/5/2023

Leonardo’s Robot Knight

• a mechanical device that
looks like an armored
knight.
• Designed to make the
knight move as if there
were a real person
inside.

Hezârfen
Ahmed
Çelebi 17th
Century

4
 10/5/2023

1738

Jacques de Vaucanson begins

building automata. The first one was
the flute player that could play
twelve songs.

1770

Swiss inventor of the modern wristwatch

Pierre Jaquet-Droz made automata for
European royalty. He created three dolls,
one can write, one plays music, and the
other one draws pictures.

• Nikola Tesla builds and

demonstrates a remote
controlled robot boat.

1898

10

5
 10/5/2023

1921
Czech writer Karel Capek introduced the word "Robot" in his play
"R.U.R" (Rossuum's Universal Robots). "Robot" in Czech comes
from the word "robota", meaning "compulsory labor“.

1940
Issac Asimov produces a series of short stories about robots
starting with "A Strange Playfellow" (later renamed "Robbie")
for Super Science Stories magazine. The story is about a robot
and its affection for a child that it is bound to protect. Over the
next 10 years he produces more stories about robots that are
eventually recompiled into the volume "I, Robot" in 1950. Issac
Asimov's most important contribution to the history of the
robot is the creation of his “Three Laws of Robotics”.

11

1. A robot may not injure a human being, or, through

inaction, allow a human being to come to harm.

2. A robot must obey the orders given it by human beings

except where such orders would conflict with the First
Law.
Three Laws of
Robotics 3. A robot must protect its own existence as long as such
protection does not conflict with the First or Second Law.

Asimov later adds a "zeroth law" to the list:

0. Law: A robot may not injure humanity, or, through
inaction, allow humanity to come to harm.

12

6
 10/5/2023

1961
Heinrich Ernst develops the MH-1, a
computer operated mechanical hand at
MIT.
1961
Unimate, the company of Joseph
Engleberger and George Devoe, built the
first industrial robot, the PUMA
(Programmable Universal Manipulator
Arm).

13

1966
The Stanford Research
Institute creates Shakey the
first mobile robot to know
and react to its own actions.

14

7
 10/5/2023

1969
Victor Scheinman creates the
Stanford Arm. The arm's
design becomes a standard
and is still influencing the
design of robot arms today.

15

1981
Takeo Kanade builds the direct drive arm. It
is the first to have motors installed
directly into the joints of the arm. This
change makes it faster and much more
accurate than previous robotic arms.
1989
A walking robot named Genghis is unveiled
by the Mobile Robots Group at MIT.

16

8
 10/5/2023

1993
Dante an 8-legged walking robot
developed at CMU descends
into Antarctica. Its mission is to
collect data from a harsh
environment similar to what we
might find on another planet.

17

1996 2000
Honda debuts the P3. Honda, then, improves P3 to ASIMO

18

9
 10/5/2023

1997
NASA’s Pathfinder Mission
lands on Mars

19

1999
SONY releases the AIBO
robotic pet.

20

10
 10/5/2023

2016

• Boston Dynamics developed several viral robots

✓ Atlas
✓ Spot, commercially available now

21

Tesla Robotics

• Robotics + AI
• Robots building robots
• Robots everywhere

22

11
 10/5/2023

Drive System

• Drive mechanisms may be

✓ Hydraulic
✓ Electric
✓ Pneumatic

23

Sophia – Social Robot

• A Hong Kong based AI company

24

12
 10/5/2023

• Provide fast movements

• Preferred for moving heavy parts

Hydraulic drives • Preferred to be used in explosive environments

• Occupy large space area

• There is a danger of oil leak to the shop floor

25

• Slower movement compared to the hydraulic robots

• Good for small and medium size robots
• Better positioning accuracy and repeatability
Electric drives
• Stepper motor drive: open loop control
• DC motor drive: closed loop control
• Cleaner environment

26

13
 10/5/2023

• Preferred for smaller robots

• Less expensive than electric or hydraulic robots
Pneumatic drives • Suitable for relatively less degrees of freedom design
• Suitable for simple pick and place application
• Relatively cheaper

27

Some definitions
• The configuration of a robot is a complete specification of the position of every point of the
robot.
• The n-dimensional space containing all possible configurations of the robot is called the
configuration space (C-space).
• The configuration of a robot is represented by a point in its C-space.
• The task space is the space in which a task is most naturally represented.
• Robot’s end-effector reachable space within C-space is named as workspace.
• The minimum number of real-valued coordinates needed to represent the configuration is the
number of degrees of freedom (dof) of the robot.

28

14
 10/5/2023

A slice of a position-only workspace for a typical

6R robot (here, the Mecademic Meca500)

29

DoF – Coin Example

• Consider a point A on the coin on 2D

✓ The freedom of point = 2
✓ DoF=2 is reached with a single point

• Let’s add more points see the change of DoF

• Two points on the coin A and B on an x-y plane implies
✓ Two coordinates of the points 𝑥𝐴 , 𝑦𝐴 , (𝑥𝐵 , 𝑦𝐵 ) the freedom of points is four
✓ And a constraint
𝑑𝐴𝐵 = 𝑥𝐴 − 𝑥𝐵 2 + 𝑦𝐴 − 𝑦𝐵 2 = 𝑐𝑜𝑛𝑠𝑡𝑎𝑛𝑡
Limits the (𝑥𝐵 , 𝑦𝐵 ) on a circle instead of plane, the circle centered at A with radius 𝑑𝐴𝐵

30

15
 10/5/2023

• B may be parameterized, for instance, by a single

parameter 𝜃𝐴𝐵

• The DoF=(2)+(2-1)=3 that is

• 𝑥𝐴 , 𝑦𝐴 , 𝜃𝐴𝐵 generalized coordinates

31

DoF – Coin Example

• Additional point C adds to this

✓ Two new coordinates of the point C 𝑥𝐶 , 𝑦𝐶 and
✓ On the rigid coin implies two new constraints
𝑑𝐴𝐶 = 𝑥𝐴 − 𝑥𝐶 2 + 𝑦𝐴 − 𝑦𝐶 2 = 𝑐𝑜𝑛𝑠𝑡.
𝑑𝐵𝐶 = 𝑥𝐵 − 𝑥𝐶 2 + 𝑦𝐵 − 𝑦𝐶 2 = 𝑐𝑜𝑛𝑠𝑡.
✓ Constraints define C at the two intersections of the two circles;
⁻ one centered at A with radius 𝑑𝐴𝐶
⁻ the other centered at B with radius 𝑑𝐵𝐶
✓ There is a unique solution for 𝑥𝐴 < 𝑥𝐶 (or 𝑥𝐴 > 𝑥𝐶 or 𝑦𝐴 < 𝑦𝐶 or 𝑦𝐴 > 𝑦𝐶 )
✓ DoF=(2)+(2-1)+(2-2)=3 unchanged by additional points
✓ Any more points will not increase DoF in the config space

32

16
 10/5/2023

33

Coin Example in 3D

• Point A (𝑥𝐴 , 𝑦𝐴 , 𝑧𝐴 ) has DoF=3

• Add another point B (𝑥𝐵 , 𝑦𝐵 , 𝑧𝐵 ) on the rigid coin
✓ with a constraint 𝑑𝐴𝐵 − 𝑥𝐴 − 𝑥𝐵 2 + 𝑦𝐴 − 𝑦𝐵 2 + 𝑧𝐴 − 𝑧𝐵 2 =0
✓ the point B can only be on the surface of the sphere(centered at A, radius 𝑑𝐴𝐵 )
✓ So, DoF=(3)+(3-1)=5

• With three points A, B and C

✓ Given A and B and the new point on the rigid coin may only be on the intersections of the two spheres (on a
circle)
⁻ Radius of 𝑑𝐴𝐶 at the center A
⁻ Radius of 𝑑𝐵𝐶 at the center B
✓ DoF=(3)+(3-1)+(3-2)=6

• Additional points X come with 3 freedom of points minus 3 constraints, constants 𝑑𝐴𝑋 , 𝑑𝐵𝑋 , 𝑑𝐶𝑋

34

17
 10/5/2023

• Rigid body motion in 2D space has 3 DoF

✓ E.g., C-space: 𝑥, 𝑦, 𝜃
Rigid Body Motion
• Rigid body motion in 3D space has 6 DoF
✓ E.g., C-space: 𝑥, 𝑦, 𝑧, 𝜙, 𝜃, 𝜓

35

DoF

• degrees of freedom =
✓ (sum of freedoms of the points) − (number of independent constraints)
✓ (number of variables) − (number of independent equations).

• DoFs: 𝑎 1 𝑏 2 𝑐 3
• C-spaces: 𝑎 𝜃 𝑏 𝑥, 𝑦 𝑐 (𝑥, 𝑦, 𝜃)

36

18
 10/5/2023

Robots

• Chain of rigid bodies (links) connected via joints

3 2𝐷
• Typical joints 𝑓𝑖 + 𝑐𝑖 = 𝑚 = ቊ
6 3𝐷
∀𝑖

Constraints
DoF Constraints
Type C On 3D
f C On 2D

R 1 3-1=2 6-1=5
P 1 3-1=2 6-1=5
H 1 N/A 6-1=5
C 2 N/A 6-2=4
U 2 N/A 6-2=4
S 3 N/A 6-3=3

37

• The # DoF of a mechanism with links and joints can be calculated as

• Let
• 𝑁 = # of links +1 (for ground)
• 𝐽 = # joints
• 𝑓𝑖 = # freedom of joint 𝑖
• 𝑐𝑖 is the constraints by joint 𝑖 with 𝑓𝑖 + 𝑐𝑖 = 𝑚
• 𝑚 = DoF of rigid body (either 3 for 2D or 6 for 3D)

Grübler’s • Then

Formula • 𝐷𝑜𝐹 = 𝑚 𝑁 − 1 − σ𝑖=1 𝑐𝑖

𝐽
𝐽

• = 𝑚 𝑁 − 1 − σ𝑖=1(𝑚 − 𝑓𝑖 )
𝐽
• = 𝑚 𝑁 − 1 − 𝐽 + σ𝑖=1 𝑓𝑖

38

19
 10/5/2023

Examples

✓𝑁 = 3 + 1 = 4
✓𝐽 = 4
✓ 𝑓𝑖 = 1
✓𝑚 = 3

𝐽 4

𝐷𝑜𝐹 = 𝑚 𝑁 − 1 − 𝐽 − ෍ 𝑓𝑖 = 3 4 − 1 − 4 + ෍ 1 = 1
𝑖=1 𝑖=1

39

✓ 𝑁 = 5 + 1 =6
✓𝐽 = 7
✓ 𝑓𝑖 = 1
✓𝑚 = 3

𝐽 7

𝐷𝑜𝐹 = 𝑚 𝑁 − 1 − 𝐽 − ෍ 𝑓𝑖 = 3 6 − 1 − 7 + ෍ 1 = 1
𝑖=1 𝑖=1

40

20
 10/5/2023

✓𝑁 = 5 + 1 = 6
✓𝐽 = 7
✓ 𝑓𝑖 = 1
✓𝑚 = 3

𝐽 7

𝐷𝑜𝐹 = 𝑚 𝑁 − 1 − 𝐽 − ෍ 𝑓𝑖 = 3 6 − 1 − 7 + ෍ 1 = 1
𝑖=1 𝑖=1

41

Example: Overlapping Joints

✓ 𝑁 = 5 + 2 + 1 =8
✓𝐽 = 8 R + 1 P = 9
✓ 𝑓𝑖 = 1
✓𝑚 = 3

𝐽 9

𝐷𝑜𝐹 = 𝑚 𝑁 − 1 − 𝐽 − ෍ 𝑓𝑖 = 3 8 − 1 − 9 + ෍ 1 = 3
𝑖=1 𝑖=1

42

21
 10/5/2023

Example: Redundant Joints

✓𝑁 = 4 + 1 = 5
✓𝐽 = 6
✓ 𝑓𝑖 = 1
✓𝑚 = 3

𝐽 6

𝐷𝑜𝐹 = 𝑚 𝑁 − 1 − 𝐽 − ෍ 𝑓𝑖 = 3 5 − 1 − 6 + ෍ 1 = 0
𝑖=1 𝑖=1
Is it rigid?

• One of the legs is redundant, dependent constraints messing up the formula.

Without one leg, 𝑁 = 4, 𝐽 = 4, 𝐷𝑜𝐹 = 1 is obtained

43

Definitions and Facts

• k independent holonomic constraints on 𝜃1 , 𝜃2 , … , 𝜃𝑛 ∈ ℝ𝑛

• =0

• reduce an n-dim C-space to n−k dof.

• Pfaffian constraints are constraints on velocity:
𝐴 𝜃 𝜃ሶ = 0

44

22
 10/5/2023

• If velocity constraints
𝐴 𝑞 𝑞ሶ = 0
can be integrated to equivalent configuration constraints, they are holonomic.
• If not, they are nonholonomic: they reduce the dimension of the feasible
velocities, but not the dimension of the C-space.

• Determining if constraints are holonomic or nonholonomic is not always trivial

45

Integrable Constraints?
• If 𝐴 𝑞 𝑞ሶ = 0 is integrable then there exists a 𝐺 𝑞 such that
𝜕𝐺
= 𝐴(𝑞)
𝜕𝑞
Example: Check if
1 + cos 𝑞2 𝑞ሶ 1 + 𝑞ሶ 2 cos 𝑞1 + 2 + cos 𝑞1 cos 𝑞2 𝑞ሶ 3 = 0
Is a holonomic constraint or not.
Let’s rewrite the constraint in Pfaffian’s form as

𝑞ሶ 1
𝐴 𝑞 𝑞ሶ = 1 + cos 𝑞2 cos 𝑞1 2 + cos 𝑞1 cos 𝑞2 𝑞ሶ 2 = 0
𝑞ሶ 3
𝜕𝑔
there is a 𝑔(𝑞) such that = 𝐴(𝑞) therefore A would be integrable.
𝜕𝑞

46

23
 10/5/2023

• Pfaffian Form

𝑞ሶ 1
• 1 + cos 𝑞2 cos 𝑞1 2 + cos 𝑞1 cos 𝑞2 𝑞ሶ 2 = 𝐴 𝑞 𝑞ሶ = 0
𝑞ሶ 3
• The first condition
𝜕𝑔
= 𝑎1 = 1 + cos 𝑞2
𝜕𝑞1
• Implies
𝑔 𝑞 = 1 + cos 𝑞2 𝑞1 + ℎ1 (𝑞2 , 𝑞3 )

47

• Similarly, the second term suggests

𝜕𝑔
= 𝑎2 = cos 𝑞1
𝜕𝑞2
• But the partial𝑔 𝑞 = 1 + cos 𝑞2 𝑞1 + ℎ1 𝑞2 , 𝑞3 from the 1st step w.r.t. 𝑞2 ,

𝜕𝑔 𝜕ℎ1 𝑞2 , 𝑞3
= − sin 𝑞2 𝑞1 +
𝜕𝑞2 𝜕𝑞2
• Can’t be equal to 𝑎2 = cos 𝑞1 as suggested in the constraint for any ℎ1 𝑞2 , 𝑞3 .
• Therefore, not integrable hence Nonholonomic

48

24
 10/5/2023

disk rolling upright on a plane

𝜕𝑔
= 1 → 𝑔 = 𝑞1 + ℎ1 (𝑞2 , 𝑞3 , 𝑞4 ) (1)
𝜕𝑞1
𝜕𝑔
= 0 → 𝑔 = 𝑞1 + ℎ1 (𝑞3 , 𝑞4 ) (2)
𝜕𝑞2
𝜕𝑔
= 0 → 𝑔 = 𝑞1 + ℎ1 (𝑞4 ) (3)
𝜕𝑞3
𝜕𝑔
= −𝑟 cos 𝑞3 → 𝑔 = 𝑞1 − 𝑟𝑞4 cos 𝑞3 (4)
𝜕𝑞4

Solution shaped after (4) contradicts (3) since 𝑟𝑞4 sin 𝑞3 ≠ 0

49

3R planar robot has its endpoint pinned by a revolute joint, making a four-
bar linkage.

“loop-closure” equations’ derivatives define three holonomic velocity constraints

50

25
 17.11.2023

INVERSE KINEMATICS
Ch. 3

Inverse Kinematic

 Given a desired pose of the end effector,

 Find a set of joint space variables would position the tool at
the desired pose

1
 17.11.2023

Inverse Kinematics

 Find all solutions satisfy end effector at base as

 Use12 nonlinear equations to solve for 6 joint variables, e.g.

 If 𝑑2 = 0.154 and 𝑑6 = 0.263 then one of the solutions is

 Hard to solve it, but forward kinematic equation may be used

to verify the solution
 Numeric solutions may be necessary to invoke
 Analytic solutions requires some simplification approaches

2
 17.11.2023

Multiple Solutions

 In general, the nonlinear equations are complexy to solve and

having multiple solutions doesn’t ease the problem

Kinematic Decoupling

 When designing a manipulator with inverse kinematic problem

in mind, we may make some assumptions helping to reduce the
complexity
 For a 6-DoF robot with a spherical wrist at the end, a solution
may be derived by decoupling
 Translation: First 3DoF to place the wrist center at the
desired position
 Rotation: The last 3 DoF reserved for the wrist orientation
alignment

3
 17.11.2023

 Assume the last three axis intersect at a point 𝑜𝑐

𝑅60 𝑞1 , … , 𝑞6 = 𝑅
𝑜60 𝑞1 , … , 𝑞6 = 𝑜
 With 𝑅 and 𝑜 desired orientation and translation from given 𝐻

Complexity Reduction – Target Translation Wrist Center

 Can be solved to find 𝑅30 then

 Then

 Used to solve the remaing joint variables

4
 17.11.2023

Translation Step

 If there is an offset d, the left arm config.

𝜙 = atan2(𝑥𝑐 , 𝑦𝑐 )

𝛼 = atan2( 𝑟 2 − 𝑑2 , 𝑑)

𝑟 2 = 𝑥𝑐2 + 𝑦𝑐2

𝜃1 = 𝜙 − 𝛼

5
 17.11.2023

 Or right arm config

Projection

 After 𝜃1 we can solve 𝜃2 and 𝜃3 for each solutions

 Project 𝜃2 , 𝜃3 to the plane

6
 17.11.2023

Joint 3

 Law of cosines

Elbow up and down solutions

Joint 2

7
 17.11.2023

Inverse Kinematics for Orientation Alignment

 The remaining joint variables to be found for orientation

alignment
 Use Euler angles to define the 𝑅63 matrix

 For

 With known 𝜃1,2,3 from the translation to C

 To find unknown 𝜃4,5,6 to align the tool to the desired 𝑅

8
 17.11.2023

 We use
𝑟11 𝑟12 𝑟13
𝑅63 = 𝑅30 𝑇 𝑅 = 𝑟21 𝑟22 𝑟33
𝑟31 𝑟32 𝑟33
 For the desired orientation and find out Euler angles
𝜃>𝟎 𝜃<𝟎
2 2
𝜃 = atan2 𝑟33 , 1 − 𝑟33 𝜃 = atan2 𝑟33 , − 1 − 𝑟33
𝜙 = atan2 𝑟13 , 𝑟23 𝜙 = atan2 −𝑟13 , −𝑟23
𝜓 = atan2( − 𝑟31 , 𝑟32 ) 𝜓 = atan2( 𝑟31 , −𝑟32 )

 with atan2(𝑥, 𝑦) four quadrant solution of tan 𝛼 = 𝑦/𝑥

 Joint variables are

Example

 Let the alignment orientation be

0 − 3/2 1/2
𝑅63 = 1/2 − 3/4 −3/4
3/2 1/4 3/4
 Then, first find 𝜃
3 13
𝜃 = atan2 , = 1.1229 sin 1.1229 = > 0
4 4
1 3
𝜙 = atan2 ,− = −0.9828
2 2
𝜓 = atan2( − 3/2,1/4) = 2.8605
 Verify the solution on 𝑅

9
 MKT4831 Robotics Engineering
Homework #2

(a)

(b)

Fig. 1

1) Find the Denavit Hartenberg parameters of the robots shown

2) Using a symbolic toolbox (matlab or python or ?) to express 𝑇𝑖𝑖−1 for 𝑖 = 1, … , 𝑛 and 𝑇𝑛0
transformation matrices
 𝐿2 𝐿3

𝐿1

3) Arm shown in Fig. Let the offsets of the frames origins be L1=1, L2=1, and L3=1.
a) Find the wrist position 𝑜𝐶 with respect to the base
b) Find the inverse kinematic equations and give four sets of angles 𝜃1 , 𝜃2 , 𝜃3 (left up, left
down, right up, right down configs) to place the 𝑜𝐶 = (1.8, 2.6, 1.2)

4) The ZYZ Euler angles are defined as

Let’s assume a wrist with frames XYZ axes color coded as RGB

𝜃5
𝑜𝐶 𝑜𝐶
𝑜𝐶 𝜃6
𝜃4

Given a desired orientation of the wrist

𝑅 = [[−0.0474, −0.6597, 0.7500],

[0.7891, 0.4356, 0.4330],
[−0.6124, 0.6124, 0.5000]]
Find a set of joint angles 𝜃4 , 𝜃5 , 𝜃6.
R=np.array([[-0.0474,-0.6597, 0.7500],
[0.7891, 0.4356, 0.4330],
[-0.6124, 0.6124, 0.5000]])

print(np.arctan2(R[2,2],np.sqrt(1-R[2,2]**2))*180/np.pi)# ->30
print(np.arctan2(R[0,2],R[1,2])*180/np.pi) # -> 60
print(np.arctan2(-R[2,0],R[2,1])*180/np.pi) # -> 45