Dyn PDF

Download as pdf or txt
Download as pdf or txt
You are on page 1of 88

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
𝒙𝟎

You might also like