Artificial Intelligence & Neuro Cognitive Systems

Fakultät für Informatik

Robot Dynamics
Dr.-Ing. John Nassour

25.01.2018 J.Nassour 1
 Introduction
• Dynamics concerns the motion of bodies

• Includes
• Kinematics – study of motion without reference to the force
that cause it
• Kinetics – relates these forces to the motion

• Dynamic behaviour of a robot: rate of change of

arm configuration in relation to the torques
exerted by the actuators.
25.01.2018 J.Nassour 2
 Forward vs. Inverse
Forward dynamics
Given vector of joint torques, work out the resulting
manipulator motion.
𝜏1 𝑞1
𝜏 𝑡 = ⋮ → 𝑞(𝑡) = ⋮
𝜏𝑛 𝑞𝑛
Inverse Dynamics
Given a vector of manipulator positions, velocities and
acceleration. Find the required vector of joint torques.

𝑞 𝑡 , 𝑞 𝑡 , 𝑞(𝑡) → 𝜏 𝑡

25.01.2018 J.Nassour 3
 Torques
The actuator has to balance torques from 4 different
sources:
• Dynamic torques (caused by the motion)
• Inertial (promotional to joint acceleration, according to Newton’s law)
• Centripetal (promotional to square of joint velocity, direction toward the
centre of circular motion)
• Coriolis (vertical forces, interaction of two rotating links)

• Static torques (caused by friction)

• Gravity torques (caused by gravity)
• External torques (exerted on the end effector, caused by the task)

25.01.2018 J.Nassour 4
 Centripetal Force
A centripetal force is a force that makes a body follow a curved path.
Its direction is always orthogonal to the velocity of the body and
towards the fixed point of the instantaneous center of curvature of
the path.

25.01.2018 J.Nassour 5
 Centrifugal Force

25.01.2018 J.Nassour 6
 The Coriolis Effect

25.01.2018 J.Nassour 7
 Manipulator Dynamics
Mathematical equations describing the dynamic behaviour of the
manipulator.

Why needed?
Simulation of the manipulator
Design a suitable controller
Evaluate the robot structure

Joint torques ⟺ Robot motion, i.e. acceleration, velocity, position

25.01.2018 J.Nassour 8
 Approaches to Dynamic Modeling
Newton-Euler Formulation
Balance of forces/torques
Dynamic equations in numeric/recursive form.

Lagrange Formulation
Energy-Based approach
Dynamic equations in symbolic/closed form

Other Formulations

25.01.2018 J.Nassour 9
 Joint Space Dynamics

𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 + 𝑮 𝒒 = 𝜞
𝒒: Generalized joint coordinates
𝑴 𝒒 : Mass Matrix – Kinetic Energy Matrix
𝑽 𝒒, 𝒒 : Centrifugal and Coriolis forces
𝑮 𝒒 : Gravity force
𝜞: Generalized forces

25.01.2018 J.Nassour 10
 Newton-Euler Formulation
In static equilibrium Fi and Ni are equal to 0.

Newton:
Linear motion
𝒎𝒊𝒗𝒄𝒊 = 𝑭𝒊

Euler:
Angular Moment = inertia. Acc. + Centrifugal and -ni+1
Coriolis forces.
𝑵𝒊 = 𝑰𝑪𝒊 𝝎𝒊 + 𝝎𝒊 × 𝑰𝑪𝒊 𝝎𝒊 Fi
By projecting on each axis we eliminate -fi+1
the internal forces acting on the links,
we find therefor the torques applied on
Ni ICi
each joint axis. -ni-1
𝑛𝑖 𝑇 . 𝑧𝑖 Revolute
mi
𝜏𝑖 = 𝑇
𝑓𝑖 . 𝑧𝑖 Prismatic -fi-1
Link i
25.01.2018 J.Nassour 11
 Lagrange Formulation
Joint n-1
Link 2
Joint 3

Link n-1
Link 1 Joint 2 Joint n

Kinetic Energy of each link Ki

Potential energy Pi
Joint 1
The kinetic energy of the robot:

Base 𝐾= 𝐾𝑖
𝑖

1 𝑇
𝐾= 𝑞 . 𝑀. 𝑞
2

By computing M, we can use it in:

𝑴𝒒 + 𝑽 + 𝑮 = 𝝉
25.01.2018 J.Nassour 12
 Lagrange Formulation
𝑑 𝜕𝐿 𝜕𝐿
− = 𝜏𝑖
𝑑𝑡 𝜕𝑞𝑖 𝜕𝑞𝑖

Lagrange function is defined

𝐿 =𝐾−𝑃
𝐾: Total kinetic energy of the robot.
𝑃: Total potential energy of the robot.
𝑞𝑖 : Joint variable of i-th joint.
𝑞𝑖 : first time derivative of 𝑞𝑖
𝜏𝑖 : Generalized force (torque) at i-th joint.

25.01.2018 J.Nassour 13
 Center of Mass
COM is the point on a body that moves in the same way
that a single particle subject to the same external force will
move.

𝑖
1 1
𝑟𝑐𝑚 = 𝑟. 𝑑𝑚 = 𝑚𝑖 𝑟𝑖
𝑀 𝑀
1
𝑟𝑐𝑚 is the location of the centre of mass.
𝑀 is the total mass of the object
𝑟 is the location of the reference frame.
𝑑𝑚 is the differential element of mass at point 𝑟.
25.01.2018 J.Nassour 14
 Inertia
The tendency of a body to remain in a state of rest or
uniform motion.

Inertial frame: the frame in which this state is measured (a

frame at rest or moving with constant velocity).

Non-Inertial frame: a frame that is accelerating with respect

to the inertial frame.

25.01.2018 J.Nassour 15
 Moment of Inertia
Moment of inertia is the rotational inertia of a body with
respect to the axis of rotation. Moment of inertia depends
on the axis and on the manner in which the mass is
distributed.
It is equal to the sum of the product between mass of
particles and the square of their distance to the axis of
rotation.
𝑖

𝐼= 𝑚𝑖 𝑟𝑖 2
1
Where 𝑚𝑖 is the mass of particle 𝑖, and 𝑟𝑖 is the distance to
the particle 𝑖.
25.01.2018 J.Nassour 16
 Moment of Inertia
For a rigid body with a continuous distribution of mass, summation
becomes integral over the whole body:

𝐼= 𝑟 2 𝑑𝑚

Example: Inertia of a cylinder (length 𝒍, mass 𝑴):

Consider the cylinder made up of an infinite number of cylinders

thickness 𝒅𝒓, at radius 𝒓, of mass 𝒅𝒎, and volume 𝒅𝑽.

𝑑𝑚 = 𝜌. 𝑑𝑉 = 2𝜋𝑙𝜌𝑟𝑑𝑟
𝑟 4−𝑟 4 𝑟 2+𝑟 2
2 1 2 1
𝐼= 𝑟 2 𝑑𝑚 = 2𝜋𝑙 𝜌𝑟 3 𝑑𝑟 = 2𝜋𝑙𝜌 =𝑀
4 4

25.01.2018 J.Nassour 17
 Inertia Tensor
A rigid body, free to move in space, has an infinite number of
possible rotation axis.

The inertia matrix ( Inertia tensor) is the integral over the volume
of all the vectors 𝒓 locating all the points 𝒅𝒗 on the rigid body and
scaled by the density of all the masses 𝒅𝒎 of the rigid body.

For a rigid body rotating about a fixed point, which is

not the centre of the mass, the inertia tensor is:

𝐼𝑥𝑥 −𝐼𝑥𝑦 −𝐼𝑥𝑧

𝐼 = −𝐼𝑦𝑥 𝐼𝑦𝑦 −𝐼𝑦𝑧
−𝐼𝑧𝑥 −𝐼𝑧𝑦 𝐼𝑧𝑧

25.01.2018 J.Nassour 18
 Inertia Tensor
𝐼𝑥𝑥 −𝐼𝑥𝑦 −𝐼𝑥𝑧
𝐼 = −𝐼𝑦𝑥 𝐼𝑦𝑦 −𝐼𝑦𝑧
−𝐼𝑧𝑥 −𝐼𝑧𝑦 𝐼𝑧𝑧

Mass moments of inertia (diagonal Mass products of inertia (off-

terms): diagonal terms):

𝐼𝑥𝑥 = 𝑦 2 + 𝑧 2 𝜌 𝑑𝑣 𝐼𝑥𝑦 = 𝑥𝑦 𝜌 𝑑𝑣

𝐼𝑦𝑦 = 𝑥 2 + 𝑧 2 𝜌 𝑑𝑣 𝐼𝑥𝑧 = 𝑥𝑧 𝜌 𝑑𝑣

𝐼𝑧𝑧 = 𝑥 2 + 𝑦 2 𝜌 𝑑𝑣 𝐼𝑦𝑧 = 𝑦𝑧 𝜌 𝑑𝑣
25.01.2018 J.Nassour 19
 Example
Find the inertia tensor of a
rectangle rotating about a fixed
point A.
𝐼𝑥𝑥 = 𝑦 2 + 𝑧 2 𝜌 𝑑𝑣

ℎ 𝑙 𝑤
𝐼𝑥𝑥 = 𝑦 2 + 𝑧 2 𝜌 𝑑𝑥𝑑𝑦𝑑𝑧
0 0 0
ℎ 𝑙
𝐼𝑥𝑥 = 𝑦 2 + 𝑧 2 𝑤 𝜌 𝑑𝑦𝑑𝑧
0 0
ℎ
ℎ 𝑙3 𝑧 𝑧 3 𝑙
𝑦3 𝐼𝑥𝑥 = + 𝑤𝜌
𝐼𝑥𝑥 = [ + 𝑧 2 𝑦 ]𝑙0 𝑤 𝜌 𝑑𝑧 3 3
0 3 0

ℎ
𝑙3 𝒍 𝟑 𝒉 𝒉𝟑 𝒍
𝐼𝑥𝑥 = + 𝑧 2 𝑙 𝑤 𝜌 𝑑𝑧 𝑰𝒙𝒙 = + 𝒘𝝆
3 𝟑 𝟑
0
25.01.2018 J.Nassour 20
 Example
Find the inertia tensor of a
rectangle rotating about a fixed
point A.

𝒍𝟑 𝒉 𝒉 𝟑 𝒍
𝑰𝒙𝒙 = + 𝒘𝝆
𝟑 𝟑

Since the mass of the rectangle:

𝑚 = 𝑤𝑙ℎ 𝜌

𝒎 𝟐
𝑰𝒙𝒙 = 𝒍 + 𝒉𝟐
𝟑

25.01.2018 J.Nassour 21
 Example
Find the inertia tensor of a
rectangle rotating about a fixed
point A.

25.01.2018 J.Nassour 22
 Translation of Inertia Tensor
Parallel axis theorem.

Moments of inertia:
𝐴 𝐶𝑀
𝐼𝑧𝑧 = 𝐼𝑧𝑧 + 𝑚(𝑟𝑥2 + 𝑟𝑦2 )

Product of inertia:
𝐴 𝐶𝑀
𝐼𝑥𝑦 = 𝐼𝑥𝑦 + 𝑚(𝑟𝑥 𝑟𝑦 )

25.01.2018 J.Nassour 23
 Example
Find the inertia tensor of a
rectangle rotating about the
COM.

25.01.2018 J.Nassour 24
 Example
Find the inertia tensor of a
rectangle rotating about the
COM.

25.01.2018 J.Nassour 25
 Example
Find the inertia tensor of a
rectangle rotating about the
COM.

25.01.2018 J.Nassour 26
 Example
Find the inertia tensor of a
rectangle rotating about the
COM.

25.01.2018 J.Nassour 27
 Example
Find the inertia tensor of a
rectangle rotating about the
COM.

25.01.2018 J.Nassour 28
 Example

Moving the axes of rotation to the centre of mass results in a

diagonal inertia tensor.
25.01.2018 J.Nassour 29
 Lagrange Formulation
𝒅 𝝏𝑳 𝝏𝑳
− =𝝉
𝒅𝒕 𝝏𝒒 𝝏𝒒

Lagrange function is defined

𝑳=𝑲−𝑷
𝐾: Total kinetic energy of the robot.
𝑃: Total potential energy of the robot.
𝑞 : Joint variable.
𝑞 : first time derivative of 𝑞
𝜏 : Generalized force (torque).

25.01.2018 J.Nassour 30
 Lagrange Formulation
𝒅 𝝏𝑳 𝝏𝑳
− =𝝉
𝒅𝒕 𝝏𝒒 𝝏𝒒

Lagrange function is defined

𝑳=𝑲−𝑷
Since 𝑃 = 𝑃 𝑞

𝒅 𝝏𝑲 𝝏𝑲 𝝏𝑷
− + =𝝉
𝒅𝒕 𝝏𝒒𝒊 𝝏𝒒 𝝏𝒒

Inertial forces Gravity vector (gradient of

potential energy)
25.01.2018 J.Nassour 31
 Lagrange Formulation
𝒅 𝝏𝑲 𝝏𝑲 𝝏𝑷
− =𝝉 −𝑮 𝒒 , 𝑮 𝒒 =
𝒅𝒕 𝝏𝒒𝒊 𝝏𝒒 𝝏𝒒
Inertial forces

This equation can be written in the following from:

Joint Space Dynamics

𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 + 𝑮 𝒒 = 𝜞

𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 = 𝜞 − 𝑮 𝒒
25.01.2018 J.Nassour 32
 Lagrange Formulation
𝒅 𝝏𝑲 𝝏𝑲
− =𝝉 −𝑮
𝒅𝒕 𝝏𝒒𝒊 𝝏𝒒
𝟏 𝑻
𝑲 = 𝒒 𝑴(𝒒)𝒒
𝟐
𝝏𝑲 𝝏 𝟏 𝑻
= 𝒒 𝑴 𝒒 𝒒 = 𝑴(𝒒)𝒒
𝝏𝒒𝒊 𝝏𝒒𝒊 𝟐
𝒅 𝝏𝑲 𝒅
= 𝑴𝒒 = 𝑴𝒒 + 𝑴𝒒
𝒅𝒕 𝝏𝒒𝒊 𝒅𝒕

𝝏𝑴
𝒒𝑻 𝒒
𝒅 𝝏𝑲 𝝏𝑲 𝟏 𝝏𝒒𝟏
− = 𝑴 𝒒 + 𝑴𝒒 − ⋮ = 𝑴𝒒 + 𝑽(𝒒, 𝒒)
𝒅𝒕 𝝏𝒒𝒊 𝝏𝒒 𝟐 𝝏𝑴
𝒒 𝑻 𝒒
25.01.2018 J.Nassour
𝝏𝒒𝒏 33
 Lagrange Formulation
𝝏𝑴
𝒒𝑻
𝒒
𝒅 𝝏𝑲 𝝏𝑲 𝟏 𝝏𝒒𝟏
− = 𝑴 𝒒 + 𝑴𝒒 − ⋮ = 𝑴𝒒 + 𝑽(𝒒, 𝒒)
𝒅𝒕 𝝏𝒒𝒊 𝝏𝒒 𝟐 𝝏𝑴
𝒒 𝑻 𝒒
𝝏𝒒𝒏 Centrifugal and
Coriolis forces
If 𝒒 = 𝟎, 𝑽 𝒒, 𝒒 = 𝟎

If 𝑀 = 𝐶𝑂𝑁𝑆𝑇𝐴𝑁𝑇, , 𝑽 𝒒, 𝒒 = 𝟎

We need to compute M from the kinetic energy equations then we

have the dynamics of the robot.
25.01.2018 J.Nassour 34
 Dynamic Equations
𝝏𝑴
𝒒𝑻
𝒒
𝒅 𝝏𝑲 𝝏𝑲 𝟏 𝝏𝒒𝟏
− = 𝑴 𝒒 + 𝑴𝒒 − ⋮ = 𝑴𝒒 + 𝑽(𝒒, 𝒒)
𝒅𝒕 𝝏𝒒𝒊 𝝏𝒒 𝟐 𝝏𝑴
𝒒 𝑻 𝒒
𝝏𝒒𝒏

𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 + 𝑮 𝒒 = 𝝉

𝟏 𝑻
𝑲= 𝒒 𝑴 𝒒 𝒒⟹𝑴 𝒒
𝟐

𝑴 𝒒 ⟹ 𝑽 𝒒, 𝒒

25.01.2018 J.Nassour 35
 Dynamic Equations

Total kinetic energy

𝐾=
1
𝐾𝐿𝑖𝑛𝑘 𝑖 = 𝑞 𝑇 𝑀 𝑞
𝝎𝒊 𝒗𝑪 𝒊
2

Joint i
Link 2
Joint 3 Link i

Link 1 Joint 2 Joint i+1

Joint 1

25.01.2018 Base J.Nassour 36

 Kinetic Energy
Work done by external forces to bring the system
from rest to its current state. 𝑣
1 𝑚 𝐹
𝐾 = 𝑚𝑣 2
2
𝜔
1 𝑇
𝐾 = 𝜔 𝐼𝐶 𝜔 𝜏
2 𝐼𝐶
𝐼𝐶 is a matrix
𝜔 is a vector

25.01.2018 J.Nassour 37
 Dynamic Equations

The kinetic energy for link i:

𝝎𝒊 𝒗𝑪 𝒊
1
𝐾𝑖 = (𝑚𝑖 𝑣𝐶𝑇𝑖 𝑣𝐶𝑖 + 𝜔𝑖𝑇 𝐼𝐶𝑖 𝜔𝑖 )
2
Joint i
Link 2
Total kinetic energy: Joint 3 Link i
𝑛

𝐾= 𝐾𝑖 Link 1 Joint 2 Joint i+1

𝑖=1

Joint 1

25.01.2018 Base J.Nassour 38

 Dynamic Equations
Kinetic energy in quadratic form of generalized velocities:

1 𝑇
𝐾 = 𝑞 𝑀𝑞
2

Then we can write:

𝑛
1 𝑇 1
𝑞 𝑀𝑞 = (𝑚𝑖 𝑣𝐶𝑇𝑖 𝑣𝐶𝑖 + 𝜔𝑖𝑇 𝐼𝐶𝑖 𝜔𝑖 )
2 2
𝑖=1

How extract the mass matrix 𝑴?

25.01.2018 J.Nassour 39
 Dynamic Equations
Kinetic energy in quadratic form of generalized velocities:

1 𝑇
𝐾 = 𝑞 𝑀𝑞
2

Then we can write:

𝑛
1 𝑇 1
𝑞 𝑀𝑞 = (𝑚𝑖 𝑣𝐶𝑇𝑖 𝑣𝐶𝑖 + 𝜔𝑖𝑇 𝐼𝐶𝑖 𝜔𝑖 )
2 2
𝑖=1

How extract the mass matrix 𝑴?

𝑣𝐶𝑖 = 𝒥𝑣𝑖 𝑞 , 𝜔𝑖 = 𝒥𝜔𝑖 𝑞

25.01.2018 J.Nassour 40
 Dynamic Equations

𝑣𝐶𝑖 = 𝒥𝑣𝑖 𝑞
𝜔𝑖 = 𝒥𝜔𝑖 𝑞 𝝎𝒊 𝒗𝑪 𝒊

Joint i
Link 2
Joint 3 Link i

Joint 2 Joint i+1

Link 1
𝒑𝑪𝒊

Joint 1

25.01.2018 Base J.Nassour 41

 Dynamic Equations
𝑛
1 𝑇 1
𝑞 𝑀𝑞 = (𝑚𝑖 𝑣𝐶𝑇𝑖 𝑣𝐶𝑖 + 𝜔𝑖𝑇 𝐼𝐶𝑖 𝜔𝑖 )
2 2
𝑖=1

𝑣𝐶𝑖 = 𝒥𝑣𝑖 𝑞 , 𝜔𝑖 = 𝒥𝜔𝑖 𝑞

𝑛
1 𝑇 1
𝑞 𝑀𝑞 = (𝑚𝑖 𝑞 𝑇 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 𝑞 + 𝑞 𝑇 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 𝑞)
2 2
𝑖=1

𝑛
1 𝑇 1 𝑇
𝑞 𝑀𝑞 = 𝑞 [ (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 )] 𝑞
2 2
𝑖=1

25.01.2018 J.Nassour 42
 Dynamic Equations
𝒏

𝑴= (𝒎𝒊 𝓙𝒗𝒊 𝑻 𝓙𝒗𝒊 + 𝓙𝝎𝒊 𝑻 𝑰𝑪𝒊 𝓙𝝎𝒊 )

𝒊=𝟏

The mass matrix is the sum of Jacobians transpose Jacobians scaled

by the mass properties (𝑚𝑖 , 𝐼𝐶𝑖 ).

If the robot has 1 DOF, M = …

With multiple links, the Jacobian matrices of all links contribute in

the mass matrix.

Each link has an impact on the total mass matrix.

25.01.2018 J.Nassour 43
 𝑛
Dynamic Equations
𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 )
𝑖=1

𝜕𝑝𝐶𝑖 𝜕𝑝𝐶𝑖 𝜕𝑝𝐶𝑖

𝒥𝑣𝑖 = … 0 …0 𝝎𝒊 𝒗𝑪 𝒊
𝜕𝑞1 𝜕𝑞2 𝜕𝑞𝑖
Joint i
Link 2
Joint 3 Link i

Joint 2 Joint i+1

Link 1
𝒑𝑪𝒊

Joint 1

25.01.2018 Base J.Nassour 44

 𝑛
Dynamic Equations
𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 )
𝑖=1

𝒥𝜔𝑖 = 𝜌1𝑧0 𝜌2𝑧1 … 𝜌𝑖𝑧𝑖 − 1 0 … 0

𝝎𝒊 𝒗𝑪 𝒊

Joint i
Link 2
Joint 3 Link i

Joint 2 Joint i+1

Link 1
𝒑𝑪𝒊

Joint 1

25.01.2018 Base J.Nassour 45

 Mass Matrix
𝑚11 𝑚12 ⋯ 𝑚1𝑛
𝑚21 𝑚22 … 𝑚2𝑛
𝑀 𝑞 = ⋮ ⋮ ⋮ ⋮
𝑚𝑛1 𝑚𝑛2 … 𝑚𝑛𝑛
𝑚11 represents the inertia of the arm perceived at joint 1.
𝑚11 = 𝑓 𝑞2:𝑛
𝑚22 = 𝑓 𝑞3:𝑛
……
𝑚𝑛𝑛 = 𝑐𝑜𝑛𝑠𝑡𝑎𝑛𝑡

25.01.2018 J.Nassour 46
 Mass Matrix
𝑚11 𝑚12 ⋯ 𝑚1𝑛
𝑚21 𝑚22 … 𝑚2𝑛
𝑀 𝑞 = ⋮ ⋮ ⋮ ⋮
𝑚𝑛1 𝑚𝑛2 … 𝑚𝑛𝑛

𝑚12 represents the coupling between the acceleration

of joint 2 on joint 1.

25.01.2018 J.Nassour 47
 Mass Matrix
𝑚11 𝑚12 ⋯ 𝑚1𝑛
𝑚21 𝑚22 … 𝑚2𝑛
𝑀 𝑞 = ⋮ ⋮ ⋮ ⋮
𝑚𝑛1 𝑚𝑛2 … 𝑚𝑛𝑛

25.01.2018 J.Nassour 48
 Mass Matrix
𝑚11 𝑚12 ⋯ 𝑚1𝑛
𝑚21 𝑚22 … 𝑚2𝑛
𝑀 𝑞 = ⋮ ⋮ ⋮ ⋮
𝑚𝑛1 𝑚𝑛2 … 𝑚𝑛𝑛

25.01.2018 J.Nassour 49
 Mass Matrix
𝑚11 𝑚12 ⋯ 𝑚1𝑛
𝑚21 𝑚22 … 𝑚2𝑛
𝑀 𝑞 = ⋮ ⋮ ⋮ ⋮
𝑚𝑛1 𝑚𝑛2 … 𝑚𝑛𝑛

25.01.2018 J.Nassour 50
 Example RP
Work out the mass matrix 𝑴.

𝒍𝟏 𝒎𝟐
𝒚𝟎 𝑰𝒄𝟐

𝒎𝟏
𝑰𝒄𝟏
𝒙𝟎

𝜽𝟏 𝒅𝟐

25.01.2018 J.Nassour 51
 Example RP
Work out the mass matrix 𝑴.
𝒍𝟏 𝒎𝟐
𝑛 𝒚𝟎 𝑰 𝒄𝟐
𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 )
𝑖=1
𝒎𝟏
𝑰 𝒄𝟏
𝜕𝑝𝐶𝑖 𝜕𝑝𝐶𝑖 𝜕𝑝𝐶𝑖 𝒙𝟎
𝒥𝑣𝑖 = … 0 …0
𝜕𝑞1 𝜕𝑞2 𝜕𝑞𝑖 𝜽𝟏 𝒅𝟐

𝒥𝜔𝑖 = 𝜌1𝑧0 𝜌2𝑧1 … 𝜌𝑖𝑧𝑖 − 1 0 … 0

𝐼𝑥𝑥 −𝐼𝑥𝑦 −𝐼𝑥𝑧

𝐼 = −𝐼𝑦𝑥 𝐼𝑦𝑦 −𝐼𝑦𝑧
−𝐼𝑧𝑥 −𝐼𝑧𝑦 𝐼𝑧𝑧
25.01.2018 J.Nassour 52
 Example RP
Work out the mass matrix 𝑴.
𝑛

𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 )

𝑖=1
𝑀 = 𝑚1 𝒥𝑣1 𝑇 𝒥𝑣1 + 𝒥𝜔1 𝑇 𝐼𝐶1 𝒥𝜔1 + 𝑚2 𝒥𝑣2 𝑇 𝒥𝑣2 + 𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2

In frame (o0,x0,y0,z0)
𝑙1 𝑐1 𝑑2 𝑐1
𝑝𝐶1 = 𝑙1 𝑠1 , 𝑝𝐶2 = 𝑑2 𝑠1 𝒍𝟏 𝒎𝟐
0 0 𝒚𝟎 𝑰 𝒄𝟐
−𝑙1 𝑠1 0
𝒥𝑣1 = 𝑙1 𝑐1 0
0 0 𝒎𝟏
−𝑑2 𝑠1 𝑐1 𝑰 𝒄𝟏
𝒙𝟎
𝒥𝑣2 = 𝑑2 𝑐1 𝑠1
0 0 𝜽𝟏 𝒅𝟐
25.01.2018 J.Nassour 53
 Example RP
Work out the mass matrix 𝑴.
𝑀 = 𝑚1 𝒥𝑣1 𝑇 𝒥𝑣1 + 𝒥𝜔1 𝑇 𝐼𝐶1 𝒥𝜔1 + 𝑚2 𝒥𝑣2 𝑇 𝒥𝑣2 + 𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2
−𝑙1 𝑠1 0
𝒥𝑣1 = 𝑙1 𝑐1 0
0 0
−𝑑2 𝑠1 𝑐1
𝒥𝑣2 = 𝑑2 𝑐1 𝑠1
0 0 𝒍𝟏 𝒎𝟐
𝒚𝟎 𝑰 𝒄𝟐
𝑇
𝑚1 𝒥𝑣1 𝒥𝑣1 = 𝑚1 𝑙12 0
0 0

𝑚2 𝑑22 0 𝒎𝟏
𝑇
𝑚2 𝒥𝑣2 𝒥𝑣2 = 𝑰 𝒄𝟏
0 𝑚2 𝒙𝟎

𝜽𝟏 𝒅𝟐
25.01.2018 J.Nassour 54
 Example RP
Work out the mass matrix 𝑴.
𝑀 = 𝑚1 𝒥𝑣1 𝑇 𝒥𝑣1 + 𝒥𝜔1 𝑇 𝐼𝐶1 𝒥𝜔1 + 𝑚2 𝒥𝑣2 𝑇 𝒥𝑣2 + 𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2

𝒥𝜔𝑖 = 𝜌1𝑧0 𝜌2𝑧1 … 𝜌𝑖𝑧𝑖 − 1 0 … 0

0 0
𝒥𝜔1 = 0 0
1 0
𝒍𝟏 𝒎𝟐
0 0 𝒚𝟎 𝑰 𝒄𝟐
𝒥𝜔2 = 0 0
1 0

𝑇 𝐼 0 𝒎𝟏
𝒥𝜔1 𝐼𝐶1 𝒥𝜔1 = 𝑧𝑧1 𝑰 𝒄𝟏
0 0 𝒙𝟎

𝐼𝑧𝑧2 0
𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2 = 𝜽𝟏 𝒅𝟐
0 0
25.01.2018 J.Nassour 55
 Example RP
Work out the mass matrix 𝑴.
𝑀 = 𝑚1 𝒥𝑣1 𝑇 𝒥𝑣1 + 𝒥𝜔1 𝑇 𝐼𝐶1 𝒥𝜔1 + 𝑚2 𝒥𝑣2 𝑇 𝒥𝑣2 + 𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2

𝑀= 𝑚1 𝑙1
2
0 + 𝑚2 𝑑22 0 𝐼
+ 𝑧𝑧1
0 𝐼
+ 𝑧𝑧2
0
0 0 0 𝑚2 0 0 0 0

𝑚1 𝑙12 + 𝐼𝑧𝑧1 + 𝑚2 𝑑22 + 𝐼𝑧𝑧2 0 𝒍𝟏

𝑀= 𝒎𝟐
0 𝑚2 𝒚𝟎 𝑰 𝒄𝟐

𝒎𝟏
𝑰 𝒄𝟏
𝒙𝟎

𝜽𝟏 𝒅𝟐
25.01.2018 J.Nassour 56
 Dynamic Equations
𝝏𝑴
𝒒𝑻
𝒒
𝒅 𝝏𝑲 𝝏𝑲 𝟏 𝝏𝒒𝟏
− = 𝑴 𝒒 + 𝑴𝒒 − ⋮ = 𝑴𝒒 + 𝑽(𝒒, 𝒒)
𝒅𝒕 𝝏𝒒𝒊 𝝏𝒒 𝟐 𝝏𝑴
𝒒 𝑻 𝒒
𝝏𝒒𝒏

𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 + 𝑮 𝒒 = 𝝉

𝟏 𝑻
𝑲= 𝒒 𝑴 𝒒 𝒒⟹𝑴 𝒒
𝟐

𝑴 𝒒 ⟹ 𝑽 𝒒, 𝒒

25.01.2018 J.Nassour 57
 Centrifugal and Coriolis Forces
𝝏𝑴
𝒒𝑻 𝒒
𝟏 𝝏𝒒𝟏
𝑽 𝒒, 𝒒 = 𝑴𝒒 − ⋮
𝟐 𝝏𝑴
𝒒𝑻 𝒒
𝝏𝒒𝒏

𝑚11 𝑚12 ⋯ 𝑚1𝑛

𝑚21 𝑚22 … 𝑚2𝑛
𝑀= ⋮ ⋮ ⋮ ⋮
𝑚𝑛1 𝑚𝑛2 … 𝑚𝑛𝑛

𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 + 𝑮 𝒒 = 𝝉

25.01.2018 J.Nassour 58
 Centrifugal and Coriolis Forces
𝝏𝑴
𝒒𝑻𝒒
𝟏 𝝏𝒒𝟏
𝑽 𝒒, 𝒒 = 𝑴𝒒 − ⋮
𝟐 𝝏𝑴
𝒒𝑻 𝒒
𝝏𝒒𝒏

For robot with 2 DOF…

𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 + 𝑮 𝒒 = 𝝉

𝑚11 𝑚12 𝑞1 𝑣1 𝑔1 𝜏1
𝑚12 𝑚22 𝑞2 + 𝑣2 + 𝑔2 = 𝜏2

25.01.2018 J.Nassour 59
 Centrifugal and Coriolis Forces
𝝏𝑴
𝒒𝑻 𝒒
𝟏 𝝏𝒒𝟏
𝑽 𝒒, 𝒒 = 𝑴𝒒 − ⋮
𝟐 𝝏𝑴
𝒒𝑻 𝒒
𝝏𝒒𝒏
𝒎𝟏𝟏𝟏 𝒎𝟏𝟐𝟏
𝟏 𝒒𝑻 𝒎𝟏𝟐𝟏 𝒎𝟐𝟐𝟏 𝒒
𝒎𝟏𝟏 𝒎𝟏𝟐
𝑽 𝒒, 𝒒 = 𝒒− 𝒎𝟏𝟏𝟐 𝒎𝟏𝟐𝟐
𝒎𝟏𝟐 𝒎𝟐𝟐 𝟐 𝒒𝑻 𝒎𝟏𝟐𝟐 𝒎𝟐𝟐𝟐 𝒒

𝜕𝑚𝑖𝑗
𝑚𝑖𝑗𝑘 =
𝜕𝑞𝑘

𝑚𝑖𝑗 = 𝑚𝑖𝑗1 𝑞1 + 𝑚𝑖𝑗2 𝑞2 + … + 𝑚𝑖𝑗𝑘 𝑞𝑘

25.01.2018 J.Nassour 60
 Centrifugal and Coriolis Forces
𝑚111 𝑚121
1 𝑞𝑇 𝑚121 𝑚221 𝑞
𝑚11 𝑚12
𝑉 𝑞, 𝑞 = 𝑞− 𝑚112 𝑚122
𝑚12 𝑚22 2 𝑞𝑇 𝑚122 𝑚222 𝑞

𝟏 𝟏
(𝒎𝟏𝟏𝟏 + 𝒎𝟏𝟏𝟏 − 𝒎𝟏𝟏𝟏 ) (𝒎𝟏𝟐𝟐 + 𝒎𝟏𝟐𝟐 − 𝒎𝟐𝟐𝟏 ) 𝒒𝟐
𝑉 𝑞, 𝑞 = 𝟐 𝟐 𝟏
𝟏 𝟏 𝒒𝟐𝟐
(𝒎𝟐𝟏𝟏 + 𝒎𝟐𝟏𝟏 − 𝒎𝟏𝟏𝟐 ) (𝒎𝟐𝟐𝟐 + 𝒎𝟐𝟐𝟐 − 𝒎𝟐𝟐𝟐 )
𝟐 𝟐
𝒎 +𝒎 −𝒎
+ 𝒎𝟏𝟏𝟐 + 𝒎𝟏𝟐𝟏 − 𝒎𝟏𝟐𝟏 𝒒𝟏 𝒒𝟐
𝟐𝟏𝟐 𝟐𝟐𝟏 𝟏𝟐𝟐

25.01.2018 J.Nassour 61
 Centrifugal and Coriolis Forces
1 1
(𝑚111 + 𝑚111 − 𝑚111 ) (𝑚122 + 𝑚122 − 𝑚221 ) 𝑞 2
𝑉 𝑞, 𝑞 = 2 2 1
1 1 𝑞22
(𝑚211 + 𝑚211 − 𝑚112 ) (𝑚222 + 𝑚222 − 𝑚222 )
2 2
𝑚112 + 𝑚121 − 𝑚121
+ 𝑚 𝑞1 𝑞2
212 + 𝑚221 − 𝑚122

1
Christoffel Symbols: 𝑏𝑖𝑗𝑘 = (𝑚𝑖𝑗𝑘 + 𝑚𝑖𝑘𝑗 − 𝑚𝑗𝑘𝑖 )
2

𝒃 𝒃𝟏𝟐𝟐 𝒒𝟐𝟏 𝟐𝒃𝟏𝟏𝟐

𝑽 𝒒, 𝒒 = 𝟏𝟏𝟏 + 𝒒𝟏 𝒒𝟐
𝒃𝟐𝟏𝟏 𝒃𝟐𝟐𝟐 𝒒𝟐𝟐 𝟐𝒃𝟐𝟏𝟐

𝑪 𝒒
Terms of the type 𝒒𝟐𝒊 arecalled centrifugal. 𝑩 𝒒
Terms of the type𝒒𝒊 𝒒𝒌 are called coriolis.
25.01.2018 J.Nassour 62
 Example RP
Work out the Centrifugal and Coriolis Vector V.

𝒍𝟏 𝒎𝟐
𝒚𝟎 𝑰𝒄𝟐

𝒎𝟏
𝑰𝒄𝟏
𝒙𝟎

𝜽𝟏 𝒅𝟐

25.01.2018 J.Nassour 63
 Example RP
Work out the Centrifugal and Coriolis Vector V.
𝑪 𝒒 𝑩 𝒒 𝒍𝟏 𝒎𝟐
𝒚𝟎 𝑰𝒄𝟐
𝒃𝟏𝟏𝟏 𝒃𝟏𝟐𝟐 𝒒𝟐𝟏 𝟐𝒃𝟏𝟏𝟐
𝑽 𝒒, 𝒒 = + 𝒒𝟏 𝒒𝟐
𝒃𝟐𝟏𝟏 𝒃𝟐𝟐𝟐 𝒒𝟐𝟐 𝟐𝒃𝟐𝟏𝟐
1 𝒎𝟏
𝑏𝑖𝑗𝑘 = (𝑚𝑖𝑗𝑘 + 𝑚𝑖𝑘𝑗 − 𝑚𝑗𝑘𝑖 )
2 𝑰 𝒄𝟏
𝜕𝑚𝑖𝑗 𝒙𝟎
𝑚𝑖𝑗𝑘 =
𝜕𝑞𝑘 𝜽𝟏 𝒅𝟐
𝟐𝒃𝟏𝟏𝟐 𝟐𝒎𝟐 𝒅𝟐
𝑩= =
𝟎 𝟎
𝟎 𝒃𝟏𝟐𝟐 𝟎 𝟎
𝑪= = 𝑚1 𝑙12 + 𝐼𝑧𝑧1 + 𝑚2 𝑑22 + 𝐼𝑧𝑧2 0
𝒃𝟐𝟏𝟏 𝟎 −𝒎𝟐 𝒅𝟐 𝟎 𝑀=
0 𝑚2

𝟐𝒎𝟐 𝒅𝟐 𝟎 𝟎 𝜽𝟐𝟏
𝑽= 𝜽𝟏 𝒅𝟐 +
𝟎 −𝒎𝟐 𝒅𝟐 𝟎 𝒅𝟐𝟐
25.01.2018 J.Nassour 64
 Dynamic Equations
𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 + 𝑮 𝒒 = 𝝉

Gravity vector?

25.01.2018 J.Nassour 65
 Link i
Gravity vector
𝝏𝑷
𝑮 𝒒 =
𝝏𝒒 𝒑𝑪𝒊 ℎ𝑖
𝑔

Potential energy:
𝒏
𝑃𝑖 = 𝑚𝑖 𝑔0 ℎ𝑖 + 𝑃0 𝝏𝑷 𝝏𝒑𝒄𝒊
𝑮𝒌 𝒒 = =− (𝒎𝒊 𝒈𝑻 )
𝝏𝒒𝒌 𝝏𝒒𝒌
𝒊
𝑃𝑖 = 𝑚𝑖 −𝑔𝑇 . 𝑝𝑐𝑖 𝑚1 𝑔
𝑚2 𝑔
𝐺 = − 𝒥𝑣1
𝑇
𝒥𝑣2 𝑇 ⋯ 𝒥𝑣𝑛 𝑇
⋮
𝑃= 𝑃𝑖 𝑚𝑛 𝑔
𝑖

25.01.2018 J.Nassour 66
 Gravity vector
Joint i
Link 2
Link i

Link 1
𝒎𝟐 𝒈 𝒎𝒊 𝒈

𝒎𝟏 𝒈

𝒎𝒏 𝒈
Base
𝑚1 𝑔
𝑇 𝑚2 𝑔
𝐺 = − 𝒥𝑣1 𝒥𝑣2 𝑇 ⋯ 𝒥𝑣𝑛 𝑇
⋮
𝑚𝑛 𝑔
25.01.2018 J.Nassour 67
 Example RP
Work out the gravity vector G.

𝒍𝟏 𝒎𝟐 𝑔
𝒚𝟎 𝑰𝒄𝟐

𝒎𝟏
𝑰𝒄𝟏
𝒙𝟎

𝜽𝟏 𝒅𝟐

25.01.2018 J.Nassour 68
 Example RP
Work out the gravity vector G. 𝑔
𝑚1 𝙜 𝒍𝟏 𝒎𝟐
𝑇 𝑚2 𝙜 𝒚𝟎
𝐺 = − 𝒥𝑣1 𝒥𝑣2 𝑇 ⋯ 𝒥𝑣𝑛 𝑇 𝑰𝒄𝟐
⋮
𝑚𝑛 𝙜
𝐺 = −𝒥𝑣1 𝑇 𝑚1 𝙜 − 𝒥𝑣2 𝑇 𝑚2 𝙜
𝒎𝟏
𝑰 𝒄𝟏
0 𝒙𝟎
In frame (o0,x0,y0,z0): 𝙜 = −𝑔
0 𝜽𝟏 𝒅𝟐
The gravity vector is:

𝑇 𝑇
−𝑙1 𝑠1 0 0 −𝑑2 𝑠1 𝑐1 0
𝒎𝟏 𝒍𝟏 + 𝒎𝟐 𝒅𝟐 𝒈𝒄𝟏
𝐺 = − 𝑙1 𝑐1 0 −𝑚1 𝑔 − 𝑑2 𝑐1 𝑠1 −𝑚2 𝑔 =
𝒎𝟐 𝒈𝒔𝟏
0 0 0 0 0 0

25.01.2018 J.Nassour 69
 Example RP
Work out the equation of motion.

𝒍𝟏 𝒎𝟐 𝑔
𝒚𝟎 𝑰𝒄𝟐

𝒎𝟏
𝑰𝒄𝟏
𝒙𝟎

𝜽𝟏 𝒅𝟐

25.01.2018 J.Nassour 70
 Example RP
Work out the equation of motion. 𝑔
𝒍𝟏 𝒎𝟐
𝒚𝟎 𝑰𝒄𝟐

𝑚1 𝑙12 + 𝐼𝑧𝑧1 + 𝑚2 𝑑22 + 𝐼𝑧𝑧2 0 𝜃1

0 𝑚2 𝑑 2 𝒎𝟏
𝑰 𝒄𝟏
𝒙𝟎
2𝑚2 𝑑2 0 0 𝜃12
+ 𝜃1 𝑑2 +
0 −𝑚2 𝑑2 0 𝑑22 𝜽𝟏 𝒅𝟐
𝑚1 𝑙1 + 𝑚2 𝑑2 𝑔𝑐1 𝜏1
+ = 𝑓
𝑚2 𝑔𝑠1 2

𝒎𝟏 𝒍𝟐𝟏 + 𝑰𝒛𝒛𝟏 + 𝒎𝟐 𝒅𝟐𝟐 + 𝑰𝒛𝒛𝟐 𝜽𝟏 + 𝟐𝒎𝟐 𝒅𝟐 𝜽𝟏 𝒅𝟐 + 𝒎𝟏 𝒍𝟏 + 𝒎𝟐 𝒅𝟐 𝒈𝒄𝟏 = 𝝉𝟏

𝒎𝟐 𝒅𝟐 −𝒎𝟐 𝒅𝟐 𝜽𝟐𝟏 + 𝒎𝟐 𝒈𝒔𝟏 = 𝒇𝟐

25.01.2018 J.Nassour 71
 Example PR
Work out the mass matrix 𝑴.
𝑛

𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 )

𝑖=1
𝑀 = 𝑚1 𝒥𝑣1 𝑇 𝒥𝑣1 + 𝒥𝜔1 𝑇 𝐼𝐶1 𝒥𝜔1 + 𝒅
𝑇 𝑇
𝑚2 𝒥𝑣2 𝒥𝑣2 + 𝒥𝜔2 𝐼𝐶2 𝒥𝜔2
𝒚𝟎
In frame (o0,x0,y0,z0)
𝑞1 − 𝑙 𝑞1 + 𝑑𝑐2 𝒎𝟐
𝑝𝐶1 = 0 , 𝑝𝐶2 = 𝑑𝑠2 𝑰 𝒄𝟐 𝒒𝟐
0 0
1 0 𝒎𝟏 𝑰 𝒄𝟏
𝒥𝑣1 = 0 0 𝒙𝟎
0 0
𝒍
1 −𝑑𝑠2
𝒥𝑣2 = 0 𝑑𝑐2
0 0 𝒒𝟏
25.01.2018 J.Nassour 72
 Example PR
Work out the mass matrix 𝑴.
𝑛

𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 )

𝑖=1
𝑀 = 𝑚1 𝒥𝑣1 𝑇 𝒥𝑣1 + 𝒥𝜔1 𝑇 𝐼𝐶1 𝒥𝜔1 + 𝒅
𝑇 𝑇
𝑚2 𝒥𝑣2 𝒥𝑣2 + 𝒥𝜔2 𝐼𝐶2 𝒥𝜔2
𝒚𝟎
In frame (o0,x0,y0,z0)
𝑞1 − 𝑙 𝑞1 + 𝑑𝑐2 𝒎𝟐
𝑝𝐶1 = 0 , 𝑝𝐶2 = 𝑑𝑠2 𝑰 𝒄𝟐 𝒒𝟐
0 0
0 0 𝒎𝟏 𝑰 𝒄𝟏
𝒥𝜔1 = 0 0 𝒙𝟎
0 0
0 0 𝒍
𝒥𝜔2 = 0 0
0 1
𝒒𝟏
25.01.2018 J.Nassour 73
 Example PR
Work out the mass matrix 𝑴.

𝑀 = 𝑚1 𝒥𝑣1 𝑇 𝒥𝑣1 + 𝒥𝜔1 𝑇 𝐼𝐶1 𝒥𝜔1 +

𝑚2 𝒥𝑣2 𝑇 𝒥𝑣2 + 𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2
𝒅
𝒎 𝟏 + 𝒎𝟐 −𝒎𝟐 𝒅𝒔𝟐
𝑴=
−𝒎𝟐 𝒅𝒔𝟐 𝑰𝒛𝒛𝟐 + 𝒎𝟐 𝒅𝟐 𝒚𝟎

𝒎𝟐

𝑰 𝒄𝟐 𝒒𝟐
𝒎𝟏 𝑰 𝒄𝟏
𝒙𝟎

𝒒𝟏
25.01.2018 J.Nassour 74
 Example PR 𝒅

Work out the gravity vector G.

𝑚1 𝙜 𝒚𝟎
𝑇 𝑚2 𝙜
𝐺 = − 𝒥𝑣1 𝒥𝑣2 𝑇 ⋯ 𝒥𝑣𝑛 𝑇 𝒎𝟐
⋮
𝑚𝑛 𝙜 𝑰 𝒄𝟐 𝒒𝟐
𝐺 = −𝒥𝑣1 𝑇 𝑚1 𝙜 − 𝒥𝑣2 𝑇 𝑚2 𝙜 𝒎𝟏 𝑰 𝒄𝟏
𝒙𝟎
0
In frame (o0,x0,y0,z0): 𝙜 = −𝑔 𝒍
0
The gravity vector is:
𝒒𝟏
𝑇 𝑇
1 0 0 1 −𝑑𝑠2 0
𝟎
𝐺=− 0 0 −𝑚1 𝑔 − 0 𝑑𝑐2 −𝑚2 𝑔 = 𝒅𝒄 𝒎 𝒈
𝟐 𝟐
0 0 0 0 0 0

25.01.2018 J.Nassour 75
 Example PR 𝒅

Work out the Centrifugal and Coriolis

𝒚𝟎
Vector V.
𝒃𝟏𝟏𝟏 𝒃𝟏𝟐𝟐 𝒒𝟐𝟏 𝟐𝒃𝟏𝟏𝟐 𝒎𝟐
𝑽 𝒒, 𝒒 = + 𝒒𝟏 𝒒𝟐
𝒃𝟐𝟏𝟏 𝒃𝟐𝟐𝟐 𝒒𝟐𝟐 𝟐𝒃𝟐𝟏𝟐
𝑰 𝒄𝟐 𝒒𝟐
1
𝑏𝑖𝑗𝑘 = (𝑚𝑖𝑗𝑘 + 𝑚𝑖𝑘𝑗 − 𝑚𝑗𝑘𝑖 ) 𝒎𝟏
2 𝑰 𝒄𝟏
𝜕𝑚𝑖𝑗 𝒙𝟎
𝑚𝑖𝑗𝑘 =
𝜕𝑞𝑘 𝒍

𝟎
𝑩=
𝟎 𝒒𝟏
𝟎 −𝒎𝟐 𝒅𝒄𝟐
𝑪=
𝟎 𝟎
𝒎𝟏 + 𝒎𝟐 −𝒎𝟐 𝒅𝒔𝟐
𝑴=
𝟐 −𝒎𝟐 𝒅𝒔𝟐 𝑰𝒛𝒛𝟐 + 𝒎𝟐 𝒅𝟐
𝟎 𝟎 −𝒎𝟐 𝒅𝒄𝟐 𝒒𝟏
𝑽= 𝒒 𝒒 +
𝟎 𝟏 𝟐 𝟎 𝟎 𝒒𝟐𝟐

25.01.2018 J.Nassour 76
 Example PR 𝒅

Work out the equation of motion.

𝒚𝟎

𝒎𝟐

𝑰 𝒄𝟐 𝒒𝟐
𝑚1 + 𝑚2 −𝑚2 𝑑𝑠2 𝑞1
−𝑚2 𝑑𝑠2 𝐼𝑧𝑧2 + 𝑚2 𝑑 2 𝑞2 𝒎𝟏 𝑰 𝒄𝟏
𝒙𝟎
2
0 0 −𝑚2 𝑑𝑐2 𝑞1 𝒍
+ 𝑞 𝑞 +
0 1 2 0 0 𝑞22

0 𝑓 𝒒𝟏
+ = 1
𝑑𝑐2 𝑚2 𝑔 𝜏2

𝒎𝟏 + 𝒎𝟐 𝒒𝟏 − 𝒎𝟐 𝒅𝒔𝟐 𝒒𝟐 − 𝒎𝟐 𝒅𝒄𝟐 𝒒𝟐𝟐 = 𝒇𝟏

−𝒎𝟐 𝒅𝒔𝟐 𝒒𝟏 + 𝑰𝒛𝒛𝟐 + 𝒎𝟐 𝒅𝟐 𝒒𝟐 + 𝒅𝒄𝟐 𝒎𝟐 𝒈 = 𝝉𝟐

25.01.2018 J.Nassour 77
 Example PP
Work out the equation of motion.

𝒎𝟏 + 𝒎𝟐 𝟎
𝑴=
𝟎 𝒎𝟐

𝒎𝟏 + 𝒎𝟐 𝒒𝟏 + 𝒈 𝒎𝟏 + 𝒎𝟐 = 𝒇𝟏

𝒎𝟐 𝒒𝟐 = 𝒇𝟐

25.01.2018 J.Nassour 78
 𝑔
Example RR 𝒍𝟐
Work out the equation of motion.
𝒚𝟎 𝒎𝟐

𝒍𝟏 𝒍𝒄𝟐
𝑰𝒄𝟐
𝒒𝟐
𝒍𝒄𝟏

𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
𝒙𝟎

𝑴 𝒒 𝒒 + 𝑽 𝒒, 𝒒 + 𝑮 𝒒 = 𝝉

25.01.2018 J.Nassour 79
 𝑔
Example RR 𝒍𝟐
Work out the mass matrix 𝑴.
𝑛 𝒚𝟎 𝒎𝟐
𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 ) 𝒍𝟏 𝒍𝒄𝟐
𝑰𝒄𝟐
𝑖=1 𝒒𝟐
𝑇 𝑇
𝑀 = 𝑚1 𝒥𝑣1 𝒥𝑣1 + 𝒥𝜔1 𝐼𝐶1 𝒥𝜔1 + 𝒍𝒄𝟏
𝑚2 𝒥𝑣2 𝑇 𝒥𝑣2 + 𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2 𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
In frame (o0,x0,y0,z0)
𝑙𝑐1 𝑐1 𝑙𝑐2 𝑐12 + 𝑙1 𝑐1 𝒙𝟎
𝑝𝐶1 = 𝑙𝑐1 𝑠1 , 𝑝𝐶2 = 𝑙𝑐2 𝑠12 + 𝑙1 𝑠1
0 0
−𝑙𝑐1 𝑠1 0
𝒥𝑣1 = 𝑙𝑐1 𝑐1 0
0 0
−𝑙𝑐2 𝑠12 − 𝑙1 𝑠1 −𝑙𝑐2 𝑠12
𝒥𝑣2 = 𝑙𝑐2 𝑐12 + 𝑙1 𝑐1 𝑙𝑐2 𝑐12
0 0
25.01.2018 J.Nassour 80
 𝑔
Example RR 𝒍𝟐
Work out the mass matrix 𝑴.
𝑛 𝒚𝟎 𝒎𝟐
𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 ) 𝒍𝟏 𝒍𝒄𝟐
𝑰𝒄𝟐
𝑖=1 𝒒𝟐
𝑇 𝑇
𝑀 = 𝑚1 𝒥𝑣1 𝒥𝑣1 + 𝒥𝜔1 𝐼𝐶1 𝒥𝜔1 + 𝒍𝒄𝟏
𝑚2 𝒥𝑣2 𝑇 𝒥𝑣2 + 𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2 𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
In frame (o0,x0,y0,z0)
𝑙𝑐1 𝑐1 𝑙𝑐2 𝑐12 + 𝑙1 𝑐1 𝒙𝟎
𝑝𝐶1 = 𝑙𝑐1 𝑠1 , 𝑝𝐶2 = 𝑙𝑐2 𝑠12 + 𝑙1 𝑠1
0 0
0 0
𝒥𝜔1 = 0 0
1 0
0 0
𝒥𝜔2 = 0 0
1 1

25.01.2018 J.Nassour 81
 𝑔
Example RR 𝒍𝟐
Work out the mass matrix 𝑴.
𝑛 𝒚𝟎 𝒎𝟐
𝑀= (𝑚𝑖 𝒥𝑣𝑖 𝑇 𝒥𝑣𝑖 + 𝒥𝜔𝑖 𝑇 𝐼𝐶𝑖 𝒥𝜔𝑖 ) 𝒍𝟏 𝒍𝒄𝟐
𝑰𝒄𝟐
𝑖=1 𝒒𝟐
𝑇 𝑇
𝑀 = 𝑚1 𝒥𝑣1 𝒥𝑣1 + 𝒥𝜔1 𝐼𝐶1 𝒥𝜔1 + 𝒍𝒄𝟏
𝑚2 𝒥𝑣2 𝑇 𝒥𝑣2 + 𝒥𝜔2 𝑇 𝐼𝐶2 𝒥𝜔2 𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
𝒙𝟎

𝒎𝟐 𝒍𝟏 𝟐 + 𝟐𝒎𝟐 𝒍𝟏 𝒍𝒄𝟐 𝒄𝟐 + 𝒎𝟏 𝒍𝒄𝟏 𝟐 + 𝒎𝟐 𝒍𝒄𝟐 𝟐 + 𝑰𝒛𝒛𝟏 𝒎𝟐 𝑰𝒄𝟐 𝟐 + 𝒍𝟏 𝒎𝟐 𝒍𝒄𝟐 𝒄𝟐 + 𝑰𝒛𝒛𝟐

𝑴=
𝒎𝟐 𝑰𝒄𝟐 𝟐 + 𝒍𝟏 𝒎𝟐 𝑰𝒄𝟐 𝒄𝟐 + 𝑰𝒛𝒛𝟐 𝒎𝟐 𝑰𝒄𝟐 𝟐 + 𝑰𝒛𝒛𝟐

25.01.2018 J.Nassour 82
 𝑔
Example RR 𝒍𝟐
Work out the gravity vector G.
𝑚1 𝙜 𝒚𝟎 𝒎𝟐
𝑇 𝑚2 𝙜 𝒍𝒄𝟐
𝐺 = − 𝒥𝑣1 𝒥𝑣2 𝑇 ⋯ 𝒥𝑣𝑛 𝑇 𝒍𝟏
𝑰𝒄𝟐
⋮ 𝒒𝟐
𝑚𝑛 𝙜 𝒍𝒄𝟏
𝑇 𝑇
𝐺 = −𝒥𝑣1 𝑚1 𝙜 − 𝒥𝑣2 𝑚2 𝙜
𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
0
In frame (o0,x0,y0,z0): 𝙜 = −𝑔 𝒙𝟎
0
The gravity vector is:

𝑇 𝑇
−𝑙𝑐1 𝑠1 0 0 −𝑙𝑐2 𝑠12 − 𝑙1 𝑠1 −𝑙𝑐2 𝑠12 0
𝐺 = − 𝑙𝑐1 𝑐1 0 −𝑚1 𝑔 − 𝑙𝑐2 𝑐12 + 𝑙1 𝑐1 𝑙𝑐2 𝑐12 −𝑚2 𝑔
0 0 0 0 0 0

25.01.2018 J.Nassour 83
 𝑔
Example RR 𝒍𝟐
Work out the gravity vector G.
𝑚1 𝙜 𝒚𝟎 𝒎𝟐
𝑇 𝑚2 𝙜 𝒍𝒄𝟐
𝐺 = − 𝒥𝑣1 𝒥𝑣2 𝑇 ⋯ 𝒥𝑣𝑛 𝑇 𝒍𝟏
𝑰𝒄𝟐
⋮ 𝒒𝟐
𝑚𝑛 𝙜 𝒍𝒄𝟏
𝑇 𝑇
𝐺 = −𝒥𝑣1 𝑚1 𝙜 − 𝒥𝑣2 𝑚2 𝙜
𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
0
In frame (o0,x0,y0,z0): 𝙜 = −𝑔 𝒙𝟎
0
The gravity vector is:

𝒈𝒎𝟐 𝒍𝒄𝟐 𝒄𝟏𝟐 + 𝒍𝟏 𝒄𝟏 + 𝒈𝒎𝟏 𝒍𝒄𝟏 𝒄𝟏

𝐺=
𝒈𝒎𝟐 𝒍𝒄𝟐 𝒄𝟏𝟐

25.01.2018 J.Nassour 84
 Example RR
Work out the Centrifugal and Coriolis
Vector V.
𝑔
𝒃𝟏𝟏𝟏 𝒃𝟏𝟐𝟐 𝒒𝟐𝟏 𝟐𝒃𝟏𝟏𝟐
𝑽 𝒒, 𝒒 = + 𝒒𝟏 𝒒𝟐
𝒃𝟐𝟏𝟏 𝒃𝟐𝟐𝟐 𝒒𝟐𝟐 𝟐𝒃𝟐𝟏𝟐 𝒍𝟐
1
𝑏𝑖𝑗𝑘 = (𝑚𝑖𝑗𝑘 + 𝑚𝑖𝑘𝑗 − 𝑚𝑗𝑘𝑖 ) 𝒚𝟎 𝒎𝟐
2
𝜕𝑚𝑖𝑗 𝒍𝟏 𝒍𝒄𝟐
𝑚𝑖𝑗𝑘 = 𝑰𝒄𝟐
𝜕𝑞𝑘 𝒒𝟐
𝒍𝒄𝟏
−𝟐𝒍𝟏 𝒍𝒄𝟐 𝒎𝟐 𝒔𝟐
𝑩= 𝑰𝒄𝟏
𝟎 𝒎𝟏 𝒒𝟏
𝟎 −𝒍𝟏 𝒍𝒄𝟐 𝒎𝟐 𝒔𝟐
𝑪= 𝒙𝟎
𝒍𝟏 𝒍𝒄𝟐 𝒎𝟐 𝒔𝟐 𝟎

−𝟐𝒍𝟏 𝒍𝒄𝟐 𝒎𝟐 𝒔𝟐 𝟎 −𝒍𝟏 𝒍𝒄𝟐 𝒎𝟐 𝒔𝟐 𝒒𝟐𝟏

𝑽 𝒒, 𝒒 = 𝒒𝟏 𝒒𝟐 +
𝟎 𝒍𝟏 𝒍𝒄𝟐 𝒎𝟐 𝒔𝟐 𝟎 𝒒𝟐𝟐

25.01.2018 J.Nassour 85
 Example RR
Work out the equation of motion.

𝑚2 𝑙1 2 + 2𝑚2 𝑙1 𝑙𝑐2 𝑐2 + 𝑚1 𝑙𝑐1 2 + 𝑚2 𝑙𝑐2 2 + 𝐼𝑧𝑧1 𝑚2 𝐼𝑐2 2 + 𝑙1 𝑚2 𝑙𝑐2 𝑐2 + 𝐼𝑧𝑧2 𝑞1

𝑚2 𝐼𝑐2 2 + 𝑙1 𝑚2 𝐼𝑐2 𝑐2 + 𝐼𝑧𝑧2 𝑚2 𝐼𝑐2 2 + 𝐼𝑧𝑧2 𝑞2

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

+ 𝑞1 𝑞2 +
0 𝑙1 𝑙𝑐2 𝑚2 𝑠2 0 𝑞22
𝑔
𝑔𝑚2 𝑙𝑐2 𝑐12 + 𝑙1 𝑐1 + 𝑔𝑚1 𝑙𝑐1 𝑐1 𝜏1
+ = 𝜏 𝒍𝟐
𝑔𝑚2 𝑙𝑐2 𝑐12 2

𝒚𝟎 𝒎𝟐

𝒍𝟏 𝒍𝒄𝟐
𝑰𝒄𝟐
𝒒𝟐
𝒍𝒄𝟏

𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
25.01.2018 J.Nassour 86
𝒙𝟎
 Example RR
Work out the equation of motion.

𝑚2 𝑙1 2 + 2𝑚2 𝑙1 𝑙𝑐2 𝑐2 + 𝑚1 𝑙𝑐1 2 + 𝑚2 𝑙𝑐2 2 + 𝐼𝑧𝑧1 𝑚2 𝐼𝑐2 2 + 𝑙1 𝑚2 𝑙𝑐2 𝑐2 + 𝐼𝑧𝑧2 𝑞1

𝑚2 𝐼𝑐2 2 + 𝑙1 𝑚2 𝐼𝑐2 𝑐2 + 𝐼𝑧𝑧2 𝑚2 𝐼𝑐2 2 + 𝐼𝑧𝑧2 𝑞2

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

+ 𝑞1 𝑞2 +
0 𝑙1 𝑙𝑐2 𝑚2 𝑠2 0 𝑞22
𝑔
𝑔𝑚2 𝑙𝑐2 𝑐12 + 𝑙1 𝑐1 + 𝑔𝑚1 𝑙𝑐1 𝑐1 𝜏1
+ = 𝜏 𝒍𝟐
𝑔𝑚2 𝑙𝑐2 𝑐12 2

𝒚𝟎 𝒎𝟐

The centrifugal terms on one joint are 𝒍𝟏 𝒍𝒄𝟐

𝑰𝒄𝟐
proportional to the square of the velocity 𝒒𝟐
of the other joint, and are at a maximum 𝒍𝒄𝟏
when the two links are perpendicular. 𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
25.01.2018 J.Nassour 87
𝒙𝟎
 Example RR
Work out the equation of motion.

𝑚2 𝑙1 2 + 2𝑚2 𝑙1 𝑙𝑐2 𝑐2 + 𝑚1 𝑙𝑐1 2 + 𝑚2 𝑙𝑐2 2 + 𝐼𝑧𝑧1 𝑚2 𝐼𝑐2 2 + 𝑙1 𝑚2 𝑙𝑐2 𝑐2 + 𝐼𝑧𝑧2 𝑞1

𝑚2 𝐼𝑐2 2 + 𝑙1 𝑚2 𝐼𝑐2 𝑐2 + 𝐼𝑧𝑧2 𝑚2 𝐼𝑐2 2 + 𝐼𝑧𝑧2 𝑞2

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

+ 𝑞1 𝑞2 +
0 𝑙1 𝑙𝑐2 𝑚2 𝑠2 0 𝑞22
𝑔
𝑔𝑚2 𝑙𝑐2 𝑐12 + 𝑙1 𝑐1 + 𝑔𝑚1 𝑙𝑐1 𝑐1 𝜏1
+ = 𝜏 𝒍𝟐
𝑔𝑚2 𝑙𝑐2 𝑐12 2

𝒚𝟎 𝒎𝟐

The Coriolis term is proportional to the 𝒍𝟏 𝒍𝒄𝟐

𝑰𝒄𝟐
product of the two joint velocities, and is 𝒒𝟐
also at a maximum when the two links 𝒍𝒄𝟏
are perpendicular. 𝑰𝒄𝟏
𝒎𝟏 𝒒𝟏
25.01.2018 J.Nassour 88
𝒙𝟎