Architecture and

robotics : kinematics
INFO802.PT1 – 2024

Adrien Ameye, adrien.ameye@univ-smb.fr

Content
• Schedule
• Introduction
• Architecture
• Pose of rigid bodies
• Homogeneous transformation matrices
• Euler angles
• Quaternions
• Forward kinematics
• Inverse kinematics
• Trajectory planning
• Path
• Trajectory
• Scaling functions
• Mobile robotics
• Distance velocity and time
• Acceleration
• Kinematics
• Linear odometr

2
Schedule

• Lecture: 4x1:30h
• Mon 01-29-2024
• Mon 02-05-2024
• Tue 02-13-2024
• Tue 02-20-h2024
• Tutorial: 4x1:30h
• Fri 02-02-2024
• Fri 02-09-2024
• Fri 02-16-2024
• Fri 02-23-2024
• Practical work: 4x4h
• Mon 03-11-2024
• Tue 03-19-2024
• Tue 03-26-2024
• Mon 04-02-2024
• Exam: 1:30h
• 04-17-2024 → 1:30h

3
Introduction
The process of managing the entire lifecycle of a
product from inception, through engineering design
and manufacture, to service and disposal of
manufactured products.
Definition

• Robot: A robot is a versatile and flexible mechatronic device, combining

mechanics, electronics and computers, designed to automatically perform
tasks that imitate or reproduce human actions in a specific field.
• These systems can replace human intervention in repetitive, dangerous or
inaccessible tasks.

5
Why robotize in industry

Cost reduction: faster and

uninterrupted work Cost
reduction
Flexibility: robots are able to
perform different tasks.
Image
Flexibility
Responsiveness: robots can be (modernity)
The
reprogrammed to adapt to company benefits of
needs in real time. robotics

Increased quality: industrial robots

are capable of performing tasks with Quality Reactivity
extreme precision and repeatability

6
Industrial robots : field of application

Welding Handling Packaging

Assembly Painting Cutting

7
Cost

Type of robot DOF Price

Cartesian robots 3 linear (+1 €

optional
rotation)

SCARA robots 4 DOF €€

Self- Break-even
(Selective
financing point
Compliance

Cash flow
Assembly Robot
Arm)
Time
Articulated robots 6 DOF €€€
Payback
Investment periods Profit

8
Robotic arm
architecture
Composition of robots

Arm End Effector Controller HMI Off line

program

10
Robot arm

Architecture Workspace
Total volume that can be reached by
Second arm Wrist End effector;
tool
the tool. This volume depends on
the geometry and angle constraints
of the arm.

First arm
Actuator

Axis

Base

11
Robot arm

Architecture Workspace
Total volume that can be reached by
Second arm Wrist End effector; the tool. This volume depends on
tool the geometry and angle constraints
of the arm (mechanical stopper).

First arm
Actuator

Axis

Base

12
Articulated mechanical structures

Architecture of structures

Serial open Serial closed Parallel Hybrid Arborescent Arborescent

Serial structures Parallel structures

+ versatile + heavy load

+ large workspace + Precision
+ simple modeling + speed
- rigidity - Limited working
- limited load space
- Complex modeling
13
Study of solid chains

In general, the degrees of freedom (DOF) of

a mechanical system is the number of
independent parameters that define its
configuration or state (Wiki. EN)

In robotics, the term "degree of freedom"

(DOF) refers to the number of independent
axes along a robot, can move.
Each degree of freedom represents an
independent possibility of movement.
Study of solid chains

“Hyperstatism” or “Statically indeterminate ” is a term used in mechanics of solids to describe the

situation where an assembly has more constraints than necessary for its operation

Hyperstaticity
(3 screws)

Isostatism is a term used where an assembly has the exact number of constraints than necessary for
its operation
Isostatic (one
screw + one
support)

Hypostatism is a term used where an assembly has less constraints than necessary.

Hypostatic
(one screw)

15
Kinematic
linkages
Study of solid chains

Prismatic (1T) Spherical (3R)

Revolute (1R) Planar (2T, 1R)

17
Study of solid chains

18
RPP or PRP Structure

RRR Structure

PPP Structure

*R: revolute P: prismatic

19
Pose of rigid
bodies
Pose of rigid bodies

A rigid body is completely described in space by its position and orientation with respect to a
reference frame. O–xyz be the orthonormal reference frame and x, y, z be the unit vectors
of the frame axes. The position of a point O’ on the rigid body with respect to the coordinate
frame O–xyz is expressed by the relation

𝑂′ = 𝑂𝑥′ + 𝑂𝑦′ + 𝑂𝑧′

𝑦′
𝑥′
𝑧Ԧ 𝑜′

𝑧′
𝑦Ԧ pose = orientation + position
𝑜
𝑥Ԧ
21
Pose of rigid bodies

In order to describe the rigid body orientation, it is convenient to consider an orthonormal

frame attached to the body and express its unit vectors with respect to the reference frame.
Let then O’xyz be such a frame with origin in O’ and x’, y’, z’ be the unit vectors of the frame
axes.
These vectors are expressed with respect to the reference frame O–xyz by the equations:

𝑦′
𝑥′
𝑥 ′ = 𝑋𝑥′ . 𝑥Ԧ + 𝑋𝑦′ . 𝑦Ԧ + 𝑋𝑧′ . 𝑧Ԧ 𝑜′
𝑧Ԧ
𝑦 ′ = 𝑌𝑥′ . 𝑥Ԧ + 𝑌𝑌′ . 𝑦Ԧ + 𝑌𝑧′ . 𝑧Ԧ
𝑧 ′ = 𝑍𝑥′ . 𝑥Ԧ + 𝑍𝑌′ . 𝑦Ԧ + 𝑍𝑧′ . 𝑧Ԧ
𝑧′
𝑦Ԧ
𝑜
𝑥Ԧ

22
Pose of rigid bodies

It can be proved that the position and orientation of an object can be defined by:
• 3 coordiantes x, y & z for the position
• 3 angles: roll, pitch & yaw for the orientation
Those numbers determine the degrees of freedom of an object.

𝑧Ԧ
Yaw

Pitch
𝑜 𝑦Ԧ

𝑥Ԧ Roll

23
The rotary matrix

The rotary matrix helps to find the coordinates of a point on a coordinate system
after it rotates around an axis

We define two coordinates systems. A: (O, 𝑥,

Ԧ 𝑦,
Ԧ 𝑧)
Ԧ and B: (O, 𝑥′, 𝑦′, 𝑧′)
𝑧Ԧ
Projection of 𝑥′ 𝑦′ 𝑧′ on

∎ ∎ ∎ 𝑥Ԧ 𝑦′
𝑅𝐵𝐴 = ∎ ∎ ∎ 𝑦Ԧ 𝑥′
∎ ∎ ∎ 𝑧Ԧ 𝑜′
𝑜 𝑦Ԧ

𝑧′
𝑥Ԧ

24
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ ∎ ∎ ∎
𝑅𝐵𝐴 = ∎ ∎ ∎
𝑥′ ∎ ∎ ∎

𝑜 𝜃 𝑥Ԧ

𝑧′ = 𝑧Ԧ

25
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ ∎ ∎ ∎
𝑅𝐵𝐴 = ∎ ∎ ∎
𝑥′ ∎ ∎ ∎

𝑥′ = cos 𝜃 𝑥Ԧ + sin 𝜃 𝑦Ԧ + 0. 𝑧Ԧ
𝑜 𝜃 𝑥Ԧ

𝑧′ = 𝑧Ԧ

26
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ cos 𝜃 ∎ ∎
𝑅𝐵𝐴 = sin 𝜃 ∎ ∎
𝑥′ 0 ∎ ∎
𝑥′ = cos 𝜃 𝑥Ԧ + sin 𝜃 𝑦Ԧ + 0. 𝑧Ԧ
𝑜 𝜃 𝑥Ԧ

𝑧′ = 𝑧Ԧ

27
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ cos 𝜃 ∎ ∎
𝑅𝐵𝐴 = sin 𝜃 ∎ ∎
𝑥′ 0 ∎ ∎
𝑥′ = cos 𝜃 𝑥Ԧ + sin 𝜃 𝑦Ԧ + 0. 𝑧Ԧ
𝑜 𝜃 𝑥Ԧ
𝑦′ = −sin 𝜃 𝑥Ԧ + cos 𝜃 𝑦Ԧ + 0. 𝑧Ԧ

𝑧′ = 𝑧Ԧ

28
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ cos 𝜃 −sin 𝜃 ∎
𝑅𝐵𝐴 = sin 𝜃 cos 𝜃 ∎
𝑥′ 0 0 ∎
𝑥′ = cos 𝜃 𝑥Ԧ + sin 𝜃 𝑦Ԧ + 0. 𝑧Ԧ
𝑜 𝜃 𝑥Ԧ
𝑦′ = −sin 𝜃 𝑥Ԧ + cos 𝜃 𝑦Ԧ + 0. 𝑧Ԧ

𝑧′ = 𝑧Ԧ

29
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ cos 𝜃 −sin 𝜃 ∎
𝑅𝐵𝐴 = sin 𝜃 cos 𝜃 ∎
𝑥′ 0 0 ∎
𝑥′ = cos 𝜃 𝑥Ԧ + sin 𝜃 𝑦Ԧ + 0. 𝑧Ԧ
𝑜 𝜃 𝑥Ԧ
𝑦′ = −sin 𝜃 𝑥Ԧ + cos 𝜃 𝑦Ԧ + 0. 𝑧Ԧ
𝑧′ = 0. 𝑥Ԧ + 0. 𝑦Ԧ + 1𝑧Ԧ
𝑧′ = 𝑧Ԧ

30
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ cos 𝜃 −sin 𝜃 0
𝑅𝐵𝐴 = sin 𝜃 cos 𝜃 0
𝑥′ 0 0 1
𝑥′ = cos 𝜃 𝑥Ԧ + sin 𝜃 𝑦Ԧ + 0. 𝑧Ԧ
𝑜 𝜃 𝑥Ԧ
𝑦′ = −sin 𝜃 𝑥Ԧ + cos 𝜃 𝑦Ԧ + 0. 𝑧Ԧ
𝑧′ = 0. 𝑥Ԧ + 0. 𝑦Ԧ + 1𝑧Ԧ
𝑧′ = 𝑧Ԧ

31
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ 𝑋′ 𝑋
𝑌′ = 𝑅𝐵𝐴 𝑌
𝐴 𝑍′ 𝑍
4 𝑥′ 𝐵 𝐴

?
?
𝑜 30° 𝑥Ԧ
3

𝑧′ = 𝑧Ԧ

32
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ 𝑋′ cos 𝜃 −sin 𝜃 0 𝑋
𝑌′ = sin 𝜃 cos 𝜃 0 𝑌
𝐴 𝑍′ 0 0 1 𝑍
4 𝑥′ 𝐵 𝐴

?
?
𝑜 30° 𝑥Ԧ
3

𝑧′ = 𝑧Ԧ

33
The rotary matrix

3D Exemple

𝑦Ԧ
𝑦′ 𝑋′ cos 𝜃 −sin 𝜃 0 3
𝑌′ = sin 𝜃 cos 𝜃 0 4
𝐴 𝑍′ 0 0 1 0
4 𝑥′ 𝐵 𝐴

?
?
30° 𝑥Ԧ 𝑋′ 4.59
𝑜 𝑌′ = 1.96
3
𝑍′ 𝐵 0 𝐵

𝑧′ = 𝑧Ԧ

34
The rotary matrix

Common rotation matrices:

𝑧Ԧ 𝑥Ԧ 𝑦Ԧ
𝑧′ 𝑥′ 𝑦′
𝑦′ 𝑧′ 𝑥′

𝑜 𝑦Ԧ 𝑜 𝑧Ԧ 𝑜 𝑥Ԧ

𝑥′ = 𝑥Ԧ 𝑦′ = 𝑦Ԧ 𝑧′ = 𝑧Ԧ

1 0 0 cos 𝜃 0 sin 𝜃 cos 𝜃 −sin 𝜃 0

𝑅𝐵𝐴 = 0 cos 𝜃 − sin 𝜃 𝑅𝐵𝐴 = 0 1 0 𝑅𝐵𝐴 = sin 𝜃 cos 𝜃 0
0 sin 𝜃 cos 𝜃 − sin 𝜃 0 cos 𝜃 0 0 1

35
The rotary matrix

Common rotation matrices:

𝑥′
𝜙
𝑥′′ 𝑧′ = 𝑧′′

𝑦′′
𝑦′

36
The rotary matrix

Multiple rotations around multiple axis can be modelled as the product of all the rotation
matrices.

𝑧Ԧ 𝑧Ԧ 𝑧Ԧ 𝑦′

𝑅𝑋 𝑅𝑌′
𝑦Ԧ 𝑧′ 𝑦Ԧ
𝑜 𝑦Ԧ 𝑜 𝑜

𝑥Ԧ 𝑥Ԧ 𝑥Ԧ 𝑥′

𝑅𝑇 = R X R Y′

37
The rotary matrix

Multiple rotations around multiple axis can be modelled as the product of all the rotation
matrices.

In a moving frame of reference (in the frame of reference of the moving object), rotate along 𝑥 then y, then z :
𝑅𝑇 = R X R Y R Z
In a fixed frame of reference (in the frame of reference of the static object), rotate along 𝑥 then y, then z :

𝑅𝑇 = R Z R Y R X

38
The rotary matrix

Multiple rotations around multiple axis can be modelled as the product of all the rotation
matrices.

𝑅2

𝑅1 𝑅3

Base

𝑅𝑇 = 𝑅1 𝑅2 𝑅3 End effector
39
Displacement vector

Unfortunately, rotation matrices DO NOT consider displacements. Displacements are computed

using vectors

𝑥1
𝑧1
𝑦1 Displacements from Fame 0 to Frame 1
Frame 1 𝑎0
𝑜
𝑑10 = 0
𝑎1
𝑎1

𝑥0
𝑎0 𝑧0

Frame 0

𝑦0 40
Homogeneous transformation matrix

The homogeneous transformation matrix is a very powerful tool to compute rotations &
displacements at the same time.

Frame 1 rotated 90° around 𝑥0 and moved 𝑎0 along

𝑧1
𝑥1 𝑥0 and 𝑎1 along 𝑧0 .
𝑦1
𝑎0 1 0 0
Frame 1
𝑜 𝑑10 = 0 𝑅10 = 0 cos 𝜃 − sin 𝜃
𝑎1 0 sin 𝜃 cos 𝜃
𝑎1
𝑹𝟎𝟏 𝒅𝟎𝟏
𝑥0 𝑥0 1 0 0 𝑎0 𝑎0
𝑎0 𝑧0 𝑦0 0 cos 𝜃 − sin 𝜃 0 0
𝑧0 = 𝑎1
0 sin 𝜃 cos 𝜃 𝑎1
Frame 0 1 1 1
0 0 0
𝑻𝟎𝟏 = 𝟎𝑻𝟏
𝑦0 41
Homogeneous transformation matrix

Properties
Position vector
𝑑10 ∈ ℝ3
𝑑20 = 𝑑10 + 𝑑21
Rotation matrix
𝑅10 ∈ ℝ3×3
𝑅20 = 𝑅10 . 𝑅21
What is the inverse of the rotation matrix 𝑅10 ?
T
𝑅01 = 𝑅10 . 𝑅10 −1 = 𝑅10 . 𝑅10 = I3×3
Homogeneous matrix
0𝑇
1 ∈ ℝ4×4

What is the inverse of the rotation matrix0𝑇1 ?

0 −1 𝑇 𝑇
1𝑇 = 0𝑇 −1 = 𝑅1 𝑑10 0
= 𝑅1 −𝑅10 𝑑10
0 1
0 1 0 1
42
 Example

Finding the coordinates of the end-effector given the

joints’angles. It is a relatively easy and straight-forward
problem
𝑦3
𝑦2 𝑥3
𝑧1 𝑥2
𝑦1 𝑥1
𝑎2 𝑧3
𝑎1
𝜃2 𝑧2
𝑧0 𝑎0
𝑦0 𝜃1
𝑥0

43
 Example

Finding the coordinates of the end-effector given the

joints’angles. It is a relatively easy and straight-forward
problem
𝑦3 C 𝜃1 − S 𝜃1 0
𝑦2 𝑅10 = S 𝜃1 C 𝜃1 0
𝑥3
0 0 1
𝑧1 𝑥2
𝑦1 𝑥1 0
𝑑10 = 0
𝑎2 𝑧3 𝑎1
𝑎1
𝜃2 𝑧2 C 𝜃1 − S 𝜃1 0 0
𝑧0 𝑎0
𝐻10 = S 𝜃1 C 𝜃1 0 0
𝑦0 𝜃1 0 0 1 𝑎0
𝑥0 0 0 0 1

44
 Example

Finding the coordinates of the end-effector given the

joints’angles. It is a relatively easy and straight-forward
problem
𝑦3 1 0 0 C 𝜃2 − S 𝜃2 0
𝑦2 𝑅21 = 0 0 −1 S 𝜃2 C 𝜃2 0
𝑥3 0 1 0 0 0 1
𝑧1 𝑥2
𝑦1 𝑥1
𝑎1 C 𝜃2 − S 𝜃2 0 𝑎1
𝑎2 𝑧3 𝑑21 = 0 0 0 −1 0
𝐻21 =
0 S 𝜃2 C 𝜃2 0 0
𝑎1
𝜃2 𝑧2 0 0 0 1
𝑧0 𝑎0
𝑦0 𝜃1
𝑥0

45
 Example

Finding the coordinates of the end-effector given the

joints’angles. It is a relatively easy and straight-forward
problem
𝑦3 1 0 0
𝑅32 = 0 1 0
𝑦2 𝑥3 0 0 1
𝑥2
𝑎2
𝑎1
𝑎2 𝑧3 𝑑32 = 0
0
𝜃2 𝑧2
𝑧0,1 𝑎0
1 0 0 𝑎2
𝑦0,1 𝜃1 0 1 0 0
𝑥0,1 𝐻32 =
0 0 1 0
0 0 0 1

46
 Example

Finding the coordinates of the end-effector given the

joints’angles. It is a relatively easy and straight-forward
problem
𝑦3
𝐻20 = 𝐻10 . 𝐻21 . 𝐻32
𝑦2 𝑥3
𝑥2

𝑎1
𝑎2 𝑧3 C 𝜃1 C 𝜃2 − C 𝜃1 S 𝜃2 S 𝜃1 𝑎1 C 𝜃1 + 𝑎2 C 𝜃1 C 𝜃2
C 𝜃2 S 𝜃1 − S 𝜃1 S 𝜃2 − C 𝜃1 𝑎1 S 𝜃1 + 𝑎2 C 𝜃2 S 𝜃1
𝜃2 𝑧2 𝐻30 =
0 1 0 𝑎0 + 𝑎2 S 𝜃2
𝑧0,1 𝑎0
0 0 0 1
𝑦0,1 𝜃1
𝑥0,1

47
Example

Determine the transformation from frame 0 to frame 1

Frame 1 rotated 110° around 𝑥0 and moved 𝑎0 along

𝑧2 𝑦2
𝑥0 , 𝑎2 along y0 and 𝑎1 along 𝑧0 .
𝑥2
Frame 2 rotated -40° around 𝑦1 and moved 𝑏0 along
𝑥1 𝑦2 .
Frame 2
𝑎1
𝑎2 𝑏0
𝑦1
𝑥0 𝑧1
𝑧0 Frame 1
𝑎0

Frame 0

𝑦0

48
Homogeneous transformation matrix

Discontinuities

Gimbal lock

Euler (gimbal lock) Explained (youtube.com)

49
DENAVIT-HARTENBERG
Khalil-Kleinfinger
DH-KK

What ?
The modified Denavit-Hartenberg (DH-KK) convention allows us to characterize the relative
position of two solids with just four parameters (instead of six).
The convention sets rules for the choice of coordinate systems

Frame Rules :
1/ The Z axis must be the axis of revolution or the direction of motion (prismatic joints)

2/ The origin Oi​ of the coordinate system is placed at the intersection of the 𝑍𝑖 axis and 𝑍𝑖−1
axis

3/ The X-axis is chosen to form a right angle with the Z​ axis, pointing from joint i−1 to joint i.

4/ The Y axis must be drawn so the whole frame follows the right-hand rule
51
DH-KK

Steps
1. Numbering of joints from 1 to n

2. Placement of all 𝑍𝑖 axis

3. For each joint from 1 to n

N
3.1 Placing the origin of the frames
times
3.2 Placement of 𝑋𝑖 axis
4. Positioning the base frame

5. Positioning the end effector frame

52
DH-KK

Steps
Preferably 𝑍𝑖 in the direction of 𝑍𝑖+1
1. Numbering of joints from 1 to n
Case of revolute R joint:
2. Placement of all 𝑍𝑖 axis
3. For each joint from 1 to n

N
3.1 Placing the origin of the frames
times Case of revolute R joint:
3.2 Placement of 𝑋𝑖 axis
4. Positioning the base frame

5. Positioning the end effector frame

53
DH-KK

Steps If 𝑍𝑖 and 𝑍𝑖+1 are concurrent

1. Numbering of joints from 1 to n 𝑂𝑖 : intersection

of 𝑍𝑖 and 𝑍𝑖+1
2. Placement of all 𝑍𝑖 axis
3. For each joint from 1 to n If 𝑍𝑖 and 𝑍𝑖 are not concurrent

N
3.1 Placing the origin of the frames Common Common
normal normal
times 𝑂𝑖 : intersection of 𝑍𝑖
3.2 Placement of 𝑋𝑖 axis and common normal
4. Positioning the base frame
If 𝑍𝑖 and 𝑍𝑖 are coaxial
5. Positioning the end effector frame

𝑂𝑖 : on the axis at a
known distance
54
DH-KK

Steps If 𝑍𝑖 and 𝑍𝑖+1 are concurrent

1. Numbering of joints from 1 to n 𝑋Ԧ𝑖 = 𝑍Ԧ𝑖 ∧ 𝑍Ԧ𝑖+1

Direction : as simple as possible
2. Placement of all 𝑍𝑖 axis
3. For each joint from 1 to n If 𝑍𝑖 and 𝑍𝑖 are not concurrent

N
3.1 Placing the origin of the frames
times 𝑋Ԧ𝑖 on the common normal,
3.2 Placement of 𝑋𝑖 axis from 𝑍Ԧ𝑖 to 𝑍Ԧ𝑖+1
4. Positioning the base frame
If 𝑍𝑖 and 𝑍𝑖 are coaxial
5. Positioning the end effector frame

No rules exept 𝑋Ԧ𝑖 ⊥ 𝑍Ԧ𝑖

(as simple as possible)
55
DH-KK

Steps
We know the distance between
1. Numbering of joints from 1 to n the base and 𝑶𝟎 and 𝑶𝟏
• 𝑂0 at the base if we know the distance
2. Placement of all 𝑍𝑖 axis between the base and 𝑂0 ans 𝑂1
3. For each joint from 1 to n • 𝑋0 well chosen so that q=0

N
3.1 Placing the origin of the frames
times We don’t know the distance
3.2 Placement of 𝑋𝑖 axis between the base and 𝑶𝟎 and 𝑶𝟏

4. Positioning the base frame • 𝑂0 is merged with 𝑂1

• 𝑋0 well chosen so that q=0
5. Positioning the end effector frame

56
DH-KK

Steps
1. Numbering of joints from 1 to n
• 𝑂𝑒 located at the center of te end effector
2. Placement of all 𝑍𝑖 axis
• 𝑍𝐸 pointing in the direction of the object to
3. For each joint from 1 to n be gripped
• 𝑌𝐸 perpendicular to 𝑍𝐸 in the sliding plane of
N
3.1 Placing the origin of the frames the gripper
times • 𝑋𝐸 = 𝑌𝐸 ∧ 𝑍𝐸
3.2 Placement of 𝑋𝑖 axis
4. Positioning the base frame

5. Positioning the end effector frame

57
DH-KK

Exemple:

58
DH-KK

DH-KK table:
It records all rotations/displacements relationships between each pair of
frames:
• Number of rows = Number of frames - 1
• Number of columns = 4 (+1 for i)
• From two consecutive frames there will be 4 parameters:
• 2 to record rotations
• 2 to record displacements

𝒊 𝒂𝒊 𝜶𝒊−𝟏 𝒅𝒊 𝜽𝒊

59
DH-KK

DH-KK parameters:

𝑎𝑖−1 : Distance measured from Z(i-1) axis to Z(i) axis and in the direction of X(i-1) axis

𝑧𝑖−1

𝑋𝑖−1 𝑌𝑖 𝑋𝑖
𝑍𝑖

𝑌𝑖−1 𝑎𝑖−1

60
DH-KK

Denavit-Hartenberg parameters:

𝛼𝑖−1 = Angle measured from the Z(i-1) axis to the Z(i) axis around the X(i-1) axis

𝑋𝑖−1
𝑋𝑖
𝑍𝑖
𝑎𝑖−1
𝑧𝑖−1

𝑌𝑖−1
𝑌𝑖
61
DH-KK

Denavit-Hartenberg parameters:

𝑑𝑖 = The distance from the X(i-1) axis to the X(i) axis and measured in the direction of the Z(i) axis

𝑧𝑖−1 𝑧𝑖−1

𝑋𝑖−1 𝑌𝑖 𝑋𝑖
𝑍𝑖

𝑌𝑖−1
𝑑𝑖

62
DH-KK

Denavit-Hartenberg parameters:

𝜃𝑖 : Angle measured around the 𝑍𝑖 axis from the 𝑋𝑖−1 axis to the 𝑋𝑖 axis

𝑍𝑖
𝑌𝑖

𝑋𝑖−1
𝜃

𝑋𝑖

63
DH-KK

Denavit-Hartenberg parameters:
𝑎𝑖−1 = Distance measured from Z(i-1) axis to Z(i) axis and in the direction of X(i-1) axis
𝛼𝑖−1 = Angle measured from the Z(i-1) axis to the Z(i) axis around the X(i-1) axis
𝑑𝑖 = The distance from the X(i-1) axis to the X(i) axis and measured in the direction of the Z(i) axis
𝜃𝑖 = Angle measured from the X(i-1) axis to the X(i) axis around the Z(i) axis

64
DH-KK

Exemple

65
DH-KK

Exemple

66
DH-KK

Exemple

𝑍2 𝑍33

𝑍4

𝑍1
1

67
DH-KK

Exemple

𝑍2 𝑍33

𝑍4

𝑍1
1
𝑋1

68
DH-KK

Exemple

𝑍2 𝑍33

2
𝑋2
4

𝑍4

𝑍1
1
𝑋1

69
DH-KK

Exemple

𝑍2 𝑍33
𝑋3
2
𝑋2
4

𝑍4

𝑍1
1
𝑋1

70
DH-KK

Exemple

𝑍2 𝑍33
𝑋3
2
𝑋2
4
𝑋4
𝑍4

𝑍1
1
𝑋1

71
DH-KK

Exemple

𝑍2 𝑍33
𝑌2 𝑋3
2
𝑋2 𝑌4
𝑌3 4
𝑋4
𝑍4

𝑍1 𝑌1
1
𝑋1

72
DH-KK

Exemple

𝑍2 𝑍33
𝑌2 𝑋3
2
𝑋2 𝑌4
𝑌3 4
𝑋4
𝑍4

𝑍0,1 𝑌0,1
1
𝑋0,1

73
DH-KK

Exemple

𝑍2 𝑍33
𝑌2 𝑋3
2 𝒊 𝒂𝒊−𝟏 𝜶𝒊−𝟏 𝒅𝒊 𝜽𝒊
𝑋2 𝑌4 1
𝑌3 4
𝑋4 2

𝑍4 3

4
𝑍0,1 𝑌0,1
1
𝑋0,1

74
DH-KK

Exemple

𝑍2 𝑍33
𝑌2 𝑋3
2 𝒊 𝒂𝒊−𝟏 𝜶𝒊−𝟏 𝒅𝒊 𝜽𝒊
𝑋2 𝑌4 1 0 0 0 −𝜃1
𝑌3 4
𝑋4 2

𝑍4 3

4
𝑍0,1 𝑌0,1
1
𝑋0,1

75
DH-KK

Exemple

𝑍2 𝑍33
𝑌2 𝑋3
2 𝒊 𝒂𝒊−𝟏 𝜶𝒊−𝟏 𝒅𝒊 𝜽𝒊
𝑋2 𝑌4 1 0 0 0 −𝜃1
𝑌3 4
𝑋4 2 0 0 𝑑2 0

𝑍4 3

4
𝑍0,1 𝑌0,1
1
𝑋0,1

76
DH-KK

Exemple

𝑍2 𝑍33
𝑌2 𝑋3
2 𝒊 𝒂𝒊−𝟏 𝜶𝒊−𝟏 𝒅𝒊 𝜽𝒊
𝑋2 𝑌4 1 0 0 0 −𝜃1
𝑌3 4
𝑋4 2 0 0 𝑑2 0
𝜋
𝑍4 3 𝐿2 0 0 + − 𝜃3
2
4
𝑍0,1 𝑌0,1
1
𝑋0,1

77
DH-KK

Exemple

𝑍2 𝑍33
𝑌2 𝑋3
2 𝒊 𝒂𝒊−𝟏 𝜶𝒊−𝟏 𝒅𝒊 𝜽𝒊
𝑋2 𝑌4 1 0 0 0 −𝜃1
𝑌3 4
𝑋4 2 0 0 𝑑2 0
𝜋
𝑍4 3 𝐿2 0 0 + − 𝜃3
2
𝜋
4 0 𝐿3 0
𝑍0,1 2
1 𝑌0,1
𝑋0,1

78
DH-KK

Exemple

𝒊 𝒂𝒊−𝟏 𝜶𝒊−𝟏 𝒅𝒊 𝜽𝒊

79
DH-KK

Forward

𝐶𝜃 −𝑆𝜃 0 𝑎
i 𝐶𝛼𝑆𝜃 𝐶𝛼𝐶𝜃 −𝑆𝛼 −𝑑𝑆𝛼
𝒊 𝒂𝒊−𝟏 𝜶𝒊−𝟏 𝒅𝒊 𝜽𝒊 Hi+1 =
S𝛼𝑆𝜃 S𝛼C𝜃 𝐶𝛼 𝑑𝐶𝛼
1 𝑎 𝛼 𝑑 𝜃 0 0 0 1
2 𝑎 𝛼 𝑑 𝜃

… … … … …

n 𝑎 𝛼 𝑑 𝜃

80
QUATERNIONS
& EXERCISES
Quaternions

Idea
In the complex plane, we can perform 2D rotations using complex number.

Ex :
• 90° rotation: Multiply by −𝑖 . 3 + 4𝑖
1 1
• 45° rotation: Multiply by + 𝑖
2 2

82
Quaternions

Idea
In the complex plane, we can perform 2D rotations using complex number.

Ex :
• 90° rotation: Multiply by −𝑖 . 3 + 4𝑖 𝑖ℝ
1 1
• 45° rotation: Multiply by + 𝑖
2 2
ℝ

83
Quaternions

Idea
In the complex plane, we can perform 2D rotations using complex number.

Ex :
• 90° rotation: Multiply by −𝑖 . 3 + 4𝑖 ∗ −𝑖 = −3𝑖 + 4 𝑖ℝ
1 1
• 45° rotation: Multiply by + 𝑖
2 2
ℝ

84
Quaternions

Def quaternions
Quaternions are an important mathematical concept used in robotics to represent the
orientation of objects in three-dimensional space.
Quaternions have 1 real part, and 3 imaginary parts labeled 𝑖,Ƹ 𝑗Ƹ and 𝑘෠

𝑞 = 𝜔 + 𝑥 𝑖Ƹ + 𝑦𝑗Ƹ + 𝑧𝑘෠

𝑖Ƹ ∗ 𝑖Ƹ = −1 𝑗Ƹ ∗ 𝑗Ƹ = −1 𝑘෠ ∗ 𝑘෠ = −1

𝑖Ƹ ∗ 𝑗Ƹ = 𝑘෠ 𝑗Ƹ ∗ 𝑘෠ = 𝑖Ƹ 𝑘෠ ∗ 𝑖Ƹ = 𝑗Ƹ

𝑗Ƹ ∗ 𝑖Ƹ = −𝑘෠ 𝑘෠ ∗ 𝑗Ƹ = −𝑖Ƹ 𝑖Ƹ ∗ 𝑘෠ = −𝑗Ƹ

The scalar component 𝜔 describes the amplitude of rotation, while the vector
components define the axis around which the rotation takes place.
85
Quaternions

Representation

We can represent a quaternion in several ways,

• as a linear combination of 1, 𝑖, 𝑗, and 𝑘
𝑞 = 𝜔 + 𝑥𝑖Ƹ + 𝑦𝑗Ƹ + 𝑧𝑘෠

• as a vector of the four coefficients in this linear combination,

෠ 𝜔
𝑞 = 𝑖,Ƹ 𝑗,Ƹ 𝑘,

• or as a scalar for the coefficient of 1 and a vector for the coefficients of the imaginary terms.
𝑞 = 𝑠, 𝑣Ԧ 𝑠 ∈ ℝ; 𝑣 = [𝑥, 𝑦, 𝑧] ∈ ℝ3
𝑞 = cos 𝛼Τ2 + sin(𝛼Τ2)𝑢

𝑠 𝑣Ԧ
86
Quaternions

Quaternion Conjugate.
The conjugate and magnitude of a quaternion are found in much the same
way as complex conjugate and magnitude.
𝑞 = 𝜔 + 𝑥𝑖Ƹ + 𝑦𝑗Ƹ + 𝑧𝑘෠
𝑞 ∗ = 𝜔 − 𝑥 𝑖Ƹ − 𝑦𝑗Ƹ − 𝑧𝑘෠

Quaternion Norm
𝑞 = 𝑞 ∗ 𝑞∗ = 𝜔2 + 𝑥 2 + 𝑦2 + 𝑧2

87
Rotation using quaternions

We will compute a rotation about the unit vector, 𝑢 = 𝑎𝑖Ƹ + 𝑏𝑗Ƹ + 𝑐 𝑘෠ by an angle 𝜃. The
quaternion that computes this rotation is
𝑞 = 𝑠, 𝑣Ԧ
𝑠 = cos 𝜃Τ2
𝑣Ԧ = 𝑢. sin 𝜃Τ2
We will represent a point 𝑝 in space by the quaternion 𝑃 = (0, 𝑝) We compute the desired
rotation of that point by this formula:

𝑷𝒓𝒐𝒕𝒂𝒕𝒆𝒅 = 𝒒𝑷𝒒−𝟏

You may want to confirm that q is a unit quaternion, since that will allow us to use the fact
that the inverse of 𝑞 is 𝑞∗

88
Quaternions

Exemple

We consider the rotation around the axis directed by

𝑣Ԧ = 1𝑖Ƹ + 1𝑗Ƹ + 1𝑘෠
and
2𝜋
𝛼 = 120° = 𝑟𝑎𝑑 𝑢
3

89
Quaternions

Exemple
1 + 𝑖Ƹ + 𝑗 Ƹ + 𝑘 1 − 𝑖Ƹ − 𝑗 Ƹ − 𝑘
𝑞= 𝑞∗ =
2 2
We now represent a point p in space by the quaternion 𝑃 𝑎
𝑏
𝑐
Ƹ 𝑗+
1+𝑖+ Ƹ 𝑘 ෠ Ƹ 𝑗−
1−𝑖− ෠
Ƹ 𝑘
𝑞 𝑎𝑖Ƹ + 𝑏𝑗Ƹ + 𝑐 𝑘෠ 𝑞∗ = 𝑎𝑖Ƹ + 𝑏𝑗Ƹ + 𝑐 𝑘෠
2 2

= 𝑐 𝑖Ƹ + 𝑎𝑗Ƹ + 𝑗𝑘෠

90
Inverse
kinematics
Inverse kinematics

𝑥
𝑦

𝑎1
𝜃3 𝑎3
𝜃2
𝑎2
𝜃1
𝑎0
Forward kinematic

Forward kinematic
𝜃1
𝑥
𝜃2
𝑦
∶
𝑧
𝜃𝑛 Inverse kinematic
Inverse kinematic 92
Inverse kinematics

Number of solutions
For the same position in space, there may be different number of solutions
(0, 1, 2, and more)

2 possible solutions An infinite number of possible solutions

93
Inverse kinematics

Using trigonometry Find joints angles given the end effector position
𝑥3
𝑥
𝐴= 𝑦
𝑦3 𝐴
𝑂3
𝐿2

𝑦2
𝜃2 𝑥2
𝑦1 𝑜2
𝐿1

𝜃1
𝑜1 𝑥1 94
Inverse kinematics

Using trigonometry Find joints angles given the end effector position
𝑥3
𝑥
𝐴= 𝑦
𝑦3 𝐴
𝑂3 𝑂1 𝐴 = 𝑂1 𝑂2 + 𝑂2 𝐴
𝐿2
𝑂1 𝑂2 = 𝐿1 𝑥2 𝑂2 𝐴 = 𝐿2 𝑥3
𝑦2
𝜃2 𝑥2 𝑥3 = 𝐶𝜃2 𝑥2 + 𝑆𝜃2 𝑦2

𝑦1 𝑜2 𝑥2 = 𝐶𝜃1 𝑥1 + 𝑆𝜃1 𝑦1
𝐿1
𝑦2 = 𝐶𝜃1 𝑦1 − 𝑆𝜃1 𝑥1
𝜃1
𝑜1 𝑥1 95
Inverse kinematics

Using trigonometry Find joints angles given the end effector position
𝑥3
𝑥
𝐴= 𝑦
𝑦3 𝐴
𝑂3 𝑂1 𝐴 = 𝑂1 𝑂2 + 𝑂2 𝐴
𝐿2
𝑥 = 𝐿1 𝐶𝜃1 + 𝐿2 𝐶𝜃2 𝐶𝜃1 − 𝐿2 𝑆𝜃2 𝑆𝜃1
𝑦2 𝑦 = 𝐿1 𝑆𝜃1 + 𝐿2 𝐶𝜃2 𝑆𝜃1 + 𝐿2 𝑆𝜃2 𝐶𝜃1
𝜃2 𝑥2
𝑦1 𝑜2
𝐿1

𝜃1
𝑜1 𝑥1 96
Inverse kinematics

Using trigonometry Find joints angles given the end effector position
𝑥3
𝑥
𝐴= 𝑦
𝑦3 𝐴
𝑂3
𝐿2
For 𝐿1 =𝐿2 =𝐿
𝑦2
𝜃2 𝑥2 −𝑥 2 − 𝑦 4 /𝐿2 + 4𝐿2
𝜃2 = −2. atan
𝑥2 + 𝑦2
𝑦1 𝑜2
𝐿1
2𝑦 + −𝑥 4 /𝐿2 − 2𝑥 2 𝑦 2 /𝐿2 + 4𝑥 2 − 𝑦 4 /𝐿2 + 4𝑦 2
𝜃1 = 2. atan
𝜃1 𝑥 2 /𝐿 + 𝑦 2 /𝐿 + 2𝑥
𝑜1 𝑥1 97
 Inverse kinematics

Back to our example

𝑦3
𝑦2 𝑥3
𝑦1 𝑥2

𝑎1
𝑎2 𝑧3
𝑎2
𝜃2 𝑧2
𝑧0 𝑎0
𝑧
𝑦0 𝜃1 1
𝑥0

98
 Inverse kinematics

Back to an example
C 𝜃1 C 𝜃2 − C 𝜃1 S 𝜃2 S 𝜃1 𝑎1 C 𝜃1 + 𝑎2 C 𝜃1 C 𝜃2
C 𝜃2 S 𝜃1 − S 𝜃1 S 𝜃2 − C 𝜃1 𝑎1 S 𝜃1 + 𝑎2 C 𝜃2 S 𝜃1
𝐻30 =
𝑦3 0 1 0 𝑎0 + 𝑎2 S 𝜃2
0 0 0 1
𝑦2 𝑥3
𝑦1 𝑥2

𝑎1
𝑎2 𝑧3
𝑎2
𝜃2 𝑧2
𝑧0 𝑎0
𝑧
𝑦0 𝜃1 1
𝑥0

99
Trajectory
planning
Trajectory planning

Path
A path denotes the locus of points in the joint space, or in the operational space, which the
manipulator must follow in the execution of the assigned motion; a path is then a pure
geometric description of motion [1].

𝜃2 𝜃 1 C-SPACE
(configuration space)

𝜃 0
𝜃1
[1]. B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling, Planning and
Control, Springer, London, 2009, DOI 10.1007/978-1-4471-0449-0 101
Trajectory planning

Trajectory
a trajectory is a path on which a timing law is specified, for instance in terms of velocities
and/or accelerations at each point [1].

𝜃1 T-SPACE 𝜃1

𝑡 𝑡
[1]. B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo, Robotics: Modelling, Planning and
Control, Springer, London, 2009, DOI 10.1007/978-1-4471-0449-0 102
Trajectory planning

Time scaling
Function that controls how fast the path is followed. It maps the path to the time domain

𝜃 𝑠 𝑡 , 𝑠: 0, 𝑇 → 0,1 𝜃 and 𝑆 must be twice differenciable.

𝑑𝜃
𝜃ሶ = 𝑠ሶ
𝑑𝑡
𝑑𝜃 𝑑2𝜃
𝜃ሷ = 𝑠ሷ + 2 𝑠ሶ
𝑑𝑡 𝑑𝑡

103
Trajectory planning

Third order polynomial scaling

𝑠 𝑡 = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 2 + 𝑎3 𝑡 3

104
Trajectory planning

Fifth order polynomial scaling

𝑠 𝑡 = 𝑎0 + 𝑎1 𝑡 + 𝑎2 𝑡 2 + 𝑎3 𝑡 3 + 𝑎4 𝑡 4 + 𝑎5 𝑡 5

𝑠 𝑠ሷ
0

Time Time

105
Trajectory planning

Trapezoidal scaling

𝑠ሶ coast

𝑡𝑎 𝑇-𝑡𝑎 𝑇 𝑡
Time

106
Trajectory planning

S-curve scaling

𝑠ሶ coast
3 phases:
• Constant jerk
• Constant velocity
• Constant acceleration
𝑡𝑎 𝑇-𝑡𝑎 𝑇 𝑡
Time

107