Control of industrial robots

Robot dynamics
Prof. Paolo Rocco (paolo.rocco@polimi.it)
Politecnico di Milano
Dipartimento di Elettronica e Informazione
Introduction
With these slides we will derive the dynamic model of the manipulator
The dynamic model accounts for the relation between the sources of motion (forces
and moments) and the resulting motion (positions and velocities)
Systematic methods exist to derive the dynamic model of the manipulators, which will
be reviewed here, along with the main properties of such model
u(t)
Control of industrial robots Robot dynamics P. Rocco [2]
q(t)
Kinetic energy
Consider a point with mass m, whose position is
described by vector p with respect to a xyz frame.
We define kinetic energy of the point the quantity:
P
x
y
z
O
p p
& &
T
m T
2
1
=
Similarly, for a system of points:
Consider now a rigid body, with mass m, volume
V and density . The kinetic energy is defined
with the integral:

=
V
T
dV T p p
& &
2
1
P
x
y
z
O
dV

=
=
n
i
i
T
i i
m T
1
2
1
p p
& &
Control of industrial robots Robot dynamics P. Rocco [3]
Potential energy
A system of position forces (i.e. depending only on the positions of the
points of application) is said to be conservative is the work computed by
each force does not depend on the trajectory followed by the point of
application, but only on the initial and final positions.
In this case the elementary work coincides with the differential, with
changed sign, of a function called potential energy:
dU dW =
An example of a system of conservative forces is the gravitational force. An example of a system of conservative forces is the gravitational force.
For a point mass we have the potential energy:
p g
T
m U
0
=
For a rigid body:
l
T
V
T
m dV U p g p g
0 0
= =

where g
0
is the gravity acceleration vector.
where p
l
is the position of the center of mass.
Control of industrial robots Robot dynamics P. Rocco [4]
Systems of rigid bodies
Let us consider a system of r rigid bodies (as for example, the links of a robot). If
all these bodies are free to move in space, the motion of the system is, at each
time instant, described by means of 6r coordinates x.
Suppose now that limitations exist in the motion of the bodies of the system (as
for example the presence of a joint, which eliminates five out of the six relative
degrees of freedom between two consecutive links).
A constraint thus exists on the motion of the bodies, which we will express with
the relation: the relation:
( ) 0 = x h
Such a constraint is said to be holonomic (it depends only on position coordinates,
not velocities) and stationary (it does not change with time).
Control of industrial robots Robot dynamics P. Rocco [5]
Free coordinates
( ) 0 = x h
If the constraints h are composed of s scalar equations and they are all
continuously differentiable, it is possible, by means of the constraints, to
eliminate s coordinates from the system equations.
The remaining n = 6r s coordinates are called free, or Lagrangian, or natural,
or generalized coordinates. n is the number of degrees of freedom of the
mechanical system. mechanical system.
For example in a robot with 6 joints, out of the 36 original coordinates, 30 are
eliminated by virtue of the constraints imposed by the 6 joints. The remaining 6
are the Lagrangian coordinates (typically the joint variables used in the
kinematic model).
Control of industrial robots Robot dynamics P. Rocco [6]
Lagranges equations
( ) ( ) ( ) q q q q q U T L =
& &
, ,
Given a system of rigid bodies, whose positions and orientations can be
expressed by means of n generalized coordinates q
i
, we define
Lagrangian of the mechanical system the quantity:
where T and U are the kinetic and the potential energies, respectively. Let then

i
be the generalized forces associated with the generalized coordinates q
i
. The
elementary work performed by the forces acting on the system can be
expressed as:

=
=
n
i
i i
dq dW
1
expressed as:
It can be proven that the dynamics of the system of bodies is expressed by the
following Lagranges equations:
n i
q
L
q
L
dt
d
i
i i
, , 1 , K
&
= =

Control of industrial robots Robot dynamics P. Rocco [7]

An example
Let us consider a system composed of a
motor rigidly connected to a load, subjected
to the gravitational force.
Let:
I
m
and I
i
the moments of inertia of the
motor and the load with respect to the
motor spinning axis
m the mass of the load
l the distance of the center of mass of the
mg

l
x
y
C
l the distance of the center of mass of the
load from the axis of the motor.
Kinetic energy of the motor
2
2
1
=
&
m m
I T
Kinetic energy of the load
2
2
1
=
&
I T
c
Control of industrial robots Robot dynamics P. Rocco [8]
Gravitational potential energy:
Lagrangian:
( ) + = + = sin
2
1
2
mgl I I U T T L
m c m
&
Lagranges equations:
mg

l
[ ] =
(

= = sin
sin
cos
0
0
mgl
l
l
g m m U
l
T
p g
An example
Lagranges equations:
( ) ( ) = + + =

cos mgl I I
dt
d L L
dt
d
m
&
&
Then:
( ) = + + cos mgl I I
m
& &
This equation can be easily interpreted as the equilibrium of moments around
the rotation axis.
Control of industrial robots Robot dynamics P. Rocco [9]
Kinetic energy of a link
The contribution of kinetic energy
of a single link can be computed
with the following integral:

=
i
V
i
T
i i
dV T
* *
2
1
p p
& &
generic point along the link
*
i
p
Position of the center of
mass:

=
i
i
V
i
i
l
dV
m
*
1
p p
Velocity of the generic point:
( )
i i l i i l i
i i
r S p r p p + = + =
& & &
*
where:
( )
(
(
(

=
0
0
0
ix iy
ix iz
iy iz
i
S
(skew-symmetric matrix)
Control of industrial robots Robot dynamics P. Rocco [10]
Translational contribution:
i i
i
i i
l
T
l i
V
l
T
l
m dV p p p p
& & & &
2
1
2
1
=

Mutual contribution:
( ) ( ) ( ) 0
*
= =

i
i i
i
i
V
l i i
T
l
V
i i
T
l
dV dV p p S p r S p
& &
Kinetic energy of a link
Rotational contribution:
( ) ( ) ( ) ( )
i i
T
i
i
V
i i
T T
i
V
i i i
T T
i
i i
dV dV


I
r S r S r S S r
2
1
2
1
2
1
=
=
|

\
|
=

( ) ( )
i i i i
r S r S =
Note:
Control of industrial robots Robot dynamics P. Rocco [11]
Then:
i i
T
i l
T
l i i
i i
m T I p p
2
1
2
1
+ =
& &
velocity of center of mass
angular velocity of the link
Knig Theorem
Inertia tensor
( )
( )
( )
(
(
(

=
(
(
(
(
(

+
+
+
=

izz
iyz iyy
ixz ixy ixx
iy ix
iz iy iz ix
iz ix iy ix iz iy
i
I
I I
I I I
dV r r
dV r r dV r r
dV r r dV r r dV r r
* *
*
* *
*
2 2
2 2
2 2
I
We define inertia tensor the matrix:
i
T
i
i
i
R =
(symmetric matrix)
The inertia tensor, if expressed in the base frame, depends on the robot
configuration. If the angular velocity is expressed with reference to a frame
rigidly attached to the link (for example the DH frame):
the inertia tensor referred to this frame is a constant matrix. Moreover it
results:
T
i
i
i i i
R I R I =
Control of industrial robots Robot dynamics P. Rocco [12]
Sum of the contributions
Let us sum the translational and rotational contributions:
i
T
i
i
i i
T
i l
T
l i i
i i
m T R I R p p
2
1
2
1
+ =
& &
Linear velocity:
( ) ( ) ( )
q J j j p
& &
K
& &
i i i
i
l
P i
l
Pi
l
P l
q q = + + =
1 1
( ) ( ) ( )
[ ] 0 0 L L
i i i
l
Pi
l
P
l
P
j j J
1
=
Angular velocity: Angular velocity:
( ) ( ) ( )
q J j j
& &
K
&
i i i
l
O
i
l
Oi
l
O
i
q q = + + =
1
1

( ) ( ) ( )
[ ] 0 0 L L
i i i
l
Oi
l
O
l
O
j j J
1
=
( )
( )
( )

=
(
(

joint rotational
joint prismatic
1
1 1
1
j
j l j
j
l
Oj
l
Pj
i
i
i
z
p p z
z
j
j
0
Columns of the Jacobian:
Control of industrial robots Robot dynamics P. Rocco [13]
Inertia matrix
Substituting expressions for the linear and angular velocities:
( ) ( ) ( ) ( )
q J R I R J q q J J q
& & & &
i i i i
l
O
T
i
i
i i
T l
O
T l
P
T l
P
T
i i
m T
2
1
2
1
+ =
By summing the contributions of all the links we obtain the kinetic energy of
the whole arm:
( ) ( )q q B q q
& & & &
T
n n
j i ij
q q b T
1 1
= =

( ) ( )q q B q q
& & & &
i j
j i ij
q q b T
2 2
1 1
= =

= =
where:
( )
( ) ( ) ( ) ( )
( )

=
+ =
n
i
l
O
T
i
i
i i
T l
O
l
P
T l
P
i
i i i i
m
1
J R I R J J J q B
is the inertia matrix of the manipulator.

symmetric
positive definite
depends on q
Control of industrial robots Robot dynamics P. Rocco [14]
Potential energy
The potential energy of a rigid link is related just to the gravitational force:
i
l
T
i
V
i
T
i
m dV U p g p g
0
*
0
= =

where g
0
is the gravity acceleration vector expressed in the base frame.
The potential energy of the whole manipulator is then the sum of the
single contributions:

= =
= =
n
i
l
T
i
n
i
i
i
m U U
1
0
1
p g
Control of industrial robots Robot dynamics P. Rocco [15]
Motion equations
The Lagrangian of the manipulator is:
( ) ( ) ( ) ( ) ( )

= = =
+ = =
n
i
li
T
i
n
i
n
j
j i ij
m q q b U T L
1
0
1 1
2
1
, , q p g q q q q q q
& & & &
If we differentiate the Lagrangian:
( ) ( )
( )

= + =
|
|
|

|
=
|
|

\
|

=
|
|

\
|

n
j
ij
n
j ij
n
j ij
q
dt
db
q b q b
dt
d
q
T
dt
d
q
L
dt
d
& & & &
& &
q
q q
Furthermore:
( )
( ) ( ) q q j g
p
g
i
n
j
l
P
T
j
n
j
i
lj
T
j
i
g m
q
m
q
U
j
i
= =

= = 1
0
1
0
( ) ( )
( )
( )


= = =
= = =
(
(

+ =
|

\
|

n
j
j
n
k
k
k
ij
n
j
j ij
j
j
j
j ij
j
j ij
i i
q q
q
b
q b
dt dt q dt q dt
1 1 1
1 1 1
& & & &
& &
q
q
( )

= =

n
j
n
k
j k
i
jk
i
q q
q
b
q
T
1 1
2
1
& &
q
Control of industrial robots Robot dynamics P. Rocco [16]
From Lagranges equations we obtain:
where:
( ) ( ) ( ) n i g q q h q b
i i
n
j
n
k
j k ijk
n
j
j ij
, , 1
1 1 1
K
& & & &
= = + +

= = =
q q q
i
jk
k
ij
ijk
q
b
q
b
h

=
2
1
Acceleration terms:
gravitational term, depends only
on the joint positions
Motion equations
Acceleration terms:
b
ii
: inertia moment as seen from the axis of joint i
b
ij
: effect of the acceleration of joint j on the joint i
Centrifugal and Coriolis terms:
h
ijj
q
j
2
: centrifugal effect induced at joint i by the velocity of joint j
h
ijk
q
j
q
k
: Coriolis effect induced at joint i by the velocities of joints j e k
.
. .
h
iii
= 0
Control of industrial robots Robot dynamics P. Rocco [17]
Non conservative forces
Besides the gravitational conservative forces, other forces act on the manipulator:
actuation torques
viscous friction torques
static friction torques

q F
&
v

( ) q q f
&
,
s

diagonal matrix of viscous friction coefficients

v
F
( ) q q f
&
,
s
function that models the static friction at the joint
Control of industrial robots Robot dynamics P. Rocco [18]
Complete dynamic model
In vector form the dynamic model can be expressed as follows:
( ) ( ) ( ) ( ) = + + + + q g q q f q F q q q C q q B
& & & & & &
, ,
s v
where C is a suitable nn matrix, whose elements satisfy the equation:
n n n

= = =
=
n
j
n
k
j k ijk
n
j
j ij
q q h q c
1 1 1
& & &
Control of industrial robots Robot dynamics P. Rocco [19]
C is not symmetric in general
Computation of the elements of C
The choice of matrix C is not unique. One possible choice is the following
one:


= = = =
= = = = =
|
|

\
|

=
=
|
|

\
|

= =
n
j
n
k
j k
i
jk
j
ik
n
j
n
k
j k
k
ij
n
j
n
k
j k
i
jk
k
ij
n
j
n
k
j k ijk
n
j
j ij
q q
q
b
q
b
q q
q
b
q q
q
b
q
b
q q h q c
1 1 1 1
1 1 1 1 1
2
1
2
1
2
1
& & & &
& & & & &
The generic element of C is:

=
=
n
k
k ijk ij
q c c
1
&
|
|

\
|

=
i
jk
j
ik
k
ij
ijk
q
b
q
b
q
b
c
2
1
Christoffel symbols of the
first kind
where:
Control of industrial robots Robot dynamics P. Rocco [20]
Skew-symmetry of matrix B 2C
The previous choice of matrix C allows to prove an important property of the
dynamic model of the manipulator. Matrix:
.
( ) ( ) ( ) q q C q B q q N
&
&
&
, 2 , =
is skew-symmetric:
( ) w w q q N w = , 0 ,
&
T

= = =
|
|

\
|

+ =
|
|

\
|

=
n
k
k
i
jk
j
ik
ij
n
k
k
i
jk
j
ik
n
k
k
k
ij
ij
q
q
b
q
b
b q
q
b
q
b
q
q
b
c
1 1 1
2
1
2
1
2
1
2
1
&
&
& &
In fact:
Control of industrial robots Robot dynamics P. Rocco [21]

=
|
|

\
|

= =
n
k
k
j
ik
i
jk
ij ij ij
q
q
b
q
b
c b n
1
2
&
&
ji ij
n n =
(skew-symmetric matrix)
Energy conservation
The equation:
( ) 0 , = q q q N q
& & &
T
(particular case of the previous one) is valid whatever the choice of matrix C is.
From the energy conservation principle, the derivative of the kinetic energy
equals the power generated by all the forces acting at the joint of the
manipulator:
( ) ( ) ( ) ( ) ( )
1 d
( ) ( ) ( ) ( ) ( ) q g q q f q F q q q B q =
& & & & &
,
2
1
s v
T T
dt
d

Taking the derivative at the left hand side and using the equation of the model:
( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( ) ( ) q g q q f q F q
q q q C q B q q q B q q q B q q q B q
+
= + =
& & &
& &
&
& & & & &
&
& & &
,
, 2
2
1
2
1
2
1
s v
T
T T T T
dt
d

from which the equation follows.
Control of industrial robots Robot dynamics P. Rocco [22]
Linearity in the dynamical parameters
If we assume a simplified expression for the static friction function:
( ) ( ) q F q q f
& &
sgn ,
s s
=
( ) q q q Y
& & &
, , =
it is possible to prove that the dynamic model of the manipulator is linear with
respect to a suitable set of dynamic parameters (masses, moments of inertia)
We can then write:
: vector of p constant parameters
Y: np matrix, function of joint positions, velocities and accelerations (regression
matrix)
( ) q q q Y
& & &
, , =
Control of industrial robots Robot dynamics P. Rocco [23]
Two-link Cartesian manipulator
Consider a two-link Cartesian manipulator, characterized
by link masses m
1
e m
2
.
The vector of generalized coordinates is:
(

=
2
1
d
d
q
x
0
z
0
z
1
m
1
m
2
d
1
d
2
The Jacobians needed for the computation of the inertia matrix are the following
ones:
( ) ( )
(
(
(

=
(
(
(

=
0 1
0 0
1 0
0 1
0 0
0 0
2 1
l
P
l
P
J J
while there are no contributions to the angular velocities.
Control of industrial robots Robot dynamics P. Rocco [24]
Computing the inertia matrix with the general formula, we obtain:
As B is constant, C=0, i.e. there are no centrifugal and Coriolis terms.
Then, since:
( 0
( ) ( ) ( ) ( )
(

+
= + =
2
2 1
2 1
0
0
2 2 1 1
m
m m
m m
l
P
T l
P
l
P
T l
P
J J J J B
Two-link Cartesian manipulator
the vector of the gravitational terms is:
(
(
(

=
g
0
0
0
g
( )
(

+
=
0
2 1
g m m
g
Control of industrial robots Robot dynamics P. Rocco [25]
If there are no friction torques and no forces at the end-effector:
( ) ( )
2 2 2
1 2 1 1 2 1
f d m
f g m m d m m
=
= + + +
& &
& &
f
1
e f
2
: forces which act along the generalized coordinates
Two-link Cartesian manipulator
Control of industrial robots Robot dynamics P. Rocco [26]
Two-link planar manipulator
Let us consider a two-link planar manipulator:
masses: m
1
and m
2
lengths: a
1
and a
2
distances of the centers of mass from the joint axes: l
1
and l
2
moments of inertia around axes passing through the
centers of mass and parallel to z
0
: I
1
and I
2
(

=
1
q
m
1
, I
1
m
2
, I
2
a
1
a
2

2
l
1
l
2
x
0
y
0
Generalized coordinates: (

=
2
q
The Jacobians needed for the computation of the inertia matrix are the following ones:
( ) ( )
(
(
(

+

=
(
(
(

=
0 0 0 0
0
0
12 2 12 2 1 1
12 2 12 2 1 1
1 1
1 1
2 1
c l c l c a
s l s l s a
c l
s l
l
P
l
P
J J
( ) ( )
(
(
(

=
(
(
(

=
1 1
0 0
0 0
0 1
0 0
0 0
2 1
l
O
l
O
J J
Generalized coordinates:
Control of industrial robots Robot dynamics P. Rocco [27]
Taking into account that the angular velocity vectors
1
e
2
are aligned with z
0
it
is not necessary to compute rotation matrices R
i
, so that the computation of the
inertia matrix gives:
( )
( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )
( ) ( )
( )
( )
22 21
12 11
2 1 2 1
2 2 1 1 2 2 1 1
b b
b b
I I m m
l
O
T l
O
l
O
T l
O
l
P
T l
P
l
P
T l
P (

= + + + = J J J J J J J J q B
Two-link planar manipulator
( )
( )
2
2
2 2 22
2 2 2 1
2
2 2 21 12
2 2 2 1
2
2
2
1 2 1
2
1 1 11
2
I l m b
I c l a l m b b
I c l a l a m I l m b
+ =
+ + = =
+ + + + + =
Control of industrial robots Robot dynamics P. Rocco [28]
depends on
2
depends on
2
constant!
From the inertia matrix it is simple to compute the Christoffel symbols:
2
1
2
1
0
2
1
2 2 1 2
1
22
2
12
122
2 2 1 2
2
11
121 112
1
11
111
= =

=
= =

= =
=

=
h s l a m
q
b
q
b
c
h s l a m
q
b
c c
q
b
c
2 2 1 2
s l a m h =
Two-link planar manipulator
0
2
1
0
2
1
2
1
2
2
22
222
1
22
221 212
2 2 1 2
2
11
1
21
211
1 2
=

=
=

= =
= =

=

q
b
c
q
b
c c
h s l a m
q
b
q
b
c
q q
Control of industrial robots Robot dynamics P. Rocco [29]
The expression of matrix C is then:
( )
( ) ( )
(

+
=
(

+
=
0 0
,
1 2 2 1 2
2 1 2 2 1 2 2 2 2 1 2
1
2 1 2
&
& & &
&
& & &
&
s l a m
s l a m s l a m
h
h h
q q C
We can verify that matrix N is skew-symmetric:
( ) ( ) ( )
( )
q q C q B q q N
&
& & &
&
& &
&
&
&
0
2
0
2
, 2 ,
2 1 2 2 2
h
h h
h
h h
=
(

= =
Two-link planar manipulator
( ) ( ) ( )
( ) q q N
&
& &
& &
& &
,
0 2
2 0
0 0
2 1
2 1
1 2
T
h h
h h
h h
=
(

+

=
(

Furthermore, since g
0
= [0 g 0]
T
, the vector of the gravitational terms is:
( )
(

+ +
=
12 2 2
12 2 2 1 1 2 1 1
c gl m
c gl m gc a m l m
g
Control of industrial robots Robot dynamics P. Rocco [30]
( ) ( ) ( ) ( )
( )
1 12 2 2 1 1 2 1 1
2
2 2 2 1 2 2 1 2 2 1 2
2 2 2 2 1
2
2 2 1 2 2 2 1
2
2
2
1 2 1
2
1 1
2
2
= + + +
+
+ + + + + + + + +
c gl m gc a m l m
s l a m s l a m
I c l a l m I c l a l a m I l m
& & &
& & & &
( ) ( ) ( )
2 2
+ + + + + I l m I c l a l m
& & & &
Without friction at the joints and forces at the end-effector, the motion equations
are:
Two-link planar manipulator

i
: torques applied at the joints
( ) ( ) ( )
2 12 2 2
2
1 2 2 1 2
2 2
2
2 2 1 2 2 2 1
2
2 2
= + +
+ + + + +
gc l m s l a m
I l m I c l a l m
&
& & & &
Control of industrial robots Robot dynamics P. Rocco [31]
By simple inspection, we can obtain the dynamical parameters with respect to
which the model is linear, i.e. those parameters for which we can write:
( ) q q q Y
& & &
, , =
We have:
[ ]
T
5 4 3 2 1
=
1 1 1
l m =
Two-link planar manipulator
2
2 2 2 5
2 2 4
2 3
2
1 1 1 2
1 1 1
l m I
l m
m
l m I
l m
+ =
=
=
+ =
=
Control of industrial robots Robot dynamics P. Rocco [32]
12
2
2 2 1 2 1 2 1 2 2 1 1 2 1 14
1 1 1
2
1 13
1 12
1 11
2 2 + + =
+ =
=
=
& & & & & & &
& &
& &
gc s a s a c a c a y
gc a a y
y
gc y
( )
(

=
25 24
15 14 13 12 11
0 0 0
, ,
y y
y y y y y
q q q Y
& & &
Two-link planar manipulator
2 1 25
12
2
1 2 1 1 2 1 24
2 1 15
+ =
+ + =
+ =
& & & &
& & &
& & & &
y
gc s a c a y
y
The coefficients of Y depend on
1
,
2
, their first and second derivatives, g and a
1
.
Control of industrial robots Robot dynamics P. Rocco [33]
Identification of dynamical parameters
The linearity of the dynamic model with respect to the dynamical parameters:
( ) q q q Y
& & &
, , =
allows to setup a procedure for the experimental identification of the same
parameters, which are usually unknown or uncertain.
Suitable motion trajectories must be executed, along which the joint positions q are
recorded, the velocities q are measured or obtained by numerical differentiation,
.
..
recorded, the velocities q are measured or obtained by numerical differentiation,
and the accelerations q are obtained with filtered (also non-causal) differentiation.
Also the torques are measured, directly (with suitable sensors) or indirectly, from
the measurements of currents in the motors.
Suppose to have the measurements (direct or indirect ones) of all the variables for
the time instants t
1
, , t
N.
..
Control of industrial robots Robot dynamics P. Rocco [34]
With N measurement sets:
( )
( )
( )
( )



Y
Y
Y
=
(
(
(

=
(
(
(

=
N N
t
t
t
t
M M
1 1
Solving with a least-squares technique:

Identification of dynamical parameters

( )
T T
Y Y Y
1
= Left pseudo-inverse matrix of Y

Only the elements of for which the corresponding column is different from
zero can be identified.
Some parameters are identifiable only in combination with other ones.
The trajectories to be used must be sufficiently rich (good conditioning of matrix
Y
T
Y): they explore the robot workspace and involve all components in the
dynamic model

Control of industrial robots Robot dynamics P. Rocco [35]
Newton-Euler formulation
An alternative way to formulate the dynamic model of the manipulator is the
Newton-Euler method.
It is based on balances of forces and moments acting on the single link, due to
the interactions with the nearby links in the kinematic chain.
We obtain a system of equations that might be solved in a recursive way,
propagating the velocities and accelerations from the base to the end effector,
while the forces and moments in the opposite way:
Recursion makes Newton-Euler
algorithm computationally efficient.
v
e
l
o
c
i
t
i
e
s
a
c
c
e
l
e
r
a
t
i
o
n
s
f
o
r
c
e
s
m
o
m
e
n
t
s
Control of industrial robots Robot dynamics P. Rocco [36]
Definition of the parameters
Let us consider the generic link i of the kinematic chain:
This picture is taken from the textbook:
B. Siciliano, L. Sciavicco, L. Villani, G. Oriolo:
Robotics: Modelling, Planning and Control, 3rd Ed.
Springer, 2009
m
i
and I
i
mass and inertia tensor of the link
r
i-1,Ci
vector from the origin of frame (i-1) to the center of mass C
i
r
i,Ci
vector from the origin of frame i to the center of mass C
i
r
i-1, i
vector from the origin of frame (i-1) to the origin of frame i
We define the following parameters:
Control of industrial robots Robot dynamics P. Rocco [37]
Definition of the variables
p
Ci
linear velocity of the center of mass C
i
p
i
linear velocity of the origin of frame i

i
angular velocity of the link
p
Ci
linear acceleration of the center of mass C
i
p
i
linear acceleration of the origin of frame i

i
angular acceleration of the link
g
0
gravity acceleration
.
..
.
.
..
g
0
gravity acceleration
f
i
force exerted by link i-1 on link i
-f
i+1
force exerted by link i+1 on link i

i
moment exerted by link i-1 on link i with respect to the origin of frame i-1
-
i+1
moment exerted by link i+1 on link i with respect to the origin of frame i
All vectors are expressed in the base frame.
Control of industrial robots Robot dynamics P. Rocco [38]
Newton-Euler formulation
Newtons equation (translational motion of the center of mass)
i
C i i i i
m m p g f f
& &
= +
+ 0 1
Eulers equation (rotational motion)
( ) ( )
i i i i i i i C i i i C i i i
dt
d
i i
I I I r f r f + = = +
+ +
&
, 1 1 , 1
it can be proven
gyroscopic effect
Generalized force at joint i:

joint rotational
joint prismatic
1
1
i
T
i
i
T
i
i
z
z f

Control of industrial robots Robot dynamics P. Rocco [39]
Accelerations of a link
Propagation of the velocities:

+
=

joint rotational
joint prismatic
1 1
1
i i i
i
i
z
&

+
+ +
=


joint rotational
joint prismatic
, 1 1
, 1 1 1
i i i i
i i i i i i
i
d
r p
r z p
p


&
&
&
&
Propagation of the accelerations:
joint prismatic
&
Note: derivative of a vector a
i
attached
to the moving frame i
( ) ( )
( ) ( ) ( ) ( ) ( ) t t t t S
t
dt
d
t
dt
d
i i
i
i i i
i
i i i
a a R
a R a
= =
= =

+ +
=

joint rotational
joint prismatic
1 1 1 1
1
i i i i i i
i
i
z z


& & &
&
&
&
( )
( )

+ +
+ + + +
=


joint rotational
joint prismatic 2
, 1 , 1 1
, 1 , 1 1 1 1
i i i i i i i i
i i i i i i i i i i i i i
i
d d
r r p
r r z z p
p


&
& &
&
& & &
& &
& &
( ) mass of center
, ,
i i i
C i i i C i i i C
r r p p + + =
&
& & & &
Control of industrial robots Robot dynamics P. Rocco [40]
Recursive algorithm
A forward recursion of velocities and accelerations is made:
initial conditions on
0
, p
0
-g
0
,
0
computation of
i
,
i
, p
i
, p
Ci
.. .
. .. ..
A backward recursion of forces and moments is made:
terminal conditions on f
n+1
and
n+1
computations:
v
e
l
o
c
i
t
i
e
s
a
c
c
e
l
e
r
a
t
i
o
n
s
i
C i i i
m p f f
& &
+ =
+1
( ) ( )
i i i i i C i i i C i i i i i
i i
I I r f r r f + + + + + =
+ +
&
, 1 1 , , 1
The generalized force at joint i is computed:

+ +
+ +
=

joint rotational
joint prismatic
1
1
si i vi i
T
i
si i vi i
T
i
i
f F
f d F
&
&
z
z f

friction contributions
f
o
r
c
e
s
m
o
m
e
n
t
s
Control of industrial robots Robot dynamics P. Rocco [41]
Local reference frames
Up to now we have supposed that all the vectors are referred to the base frame.
It is more convenient to express the vectors with respect to the current frame on
link i. In this way, vectors r
i-1,i
e r
i,Ci
and the inertia tensor I
i
are constant, which
makes the algorithm computationally more effcient.
The equations are modified in some terms (we need to multiply the vectors by
suitable rotation matrices) but nothing changes in the nature of the method.
Control of industrial robots Robot dynamics P. Rocco [42]
Two-link planar manipulator
Let us consider again a two-link planar manipulator
with rotational joints, whose model has been
already derived with the Euler-Lagrange method:
masses: m
1
and m
2
lengths: a
1
and a
2
distances of the centers of mass from the joint
axes: l
1
and l
2
moments of inertia around axes passing through
the centers of mass and parallel to z
0
: I
1
and I
2
m
1
, I
1
m
2
, I
2
a
1
a
2

2
l
1
l
2
x
0
y
0
0 1 2
Initial conditions for the forward recursion of velocities and accelerations;
[ ] 0 = = =
0
0
0
0
0
0
0
0
, 0 0
&
& &
T
g g p
Initial conditions for the backward recursion of forces and moments:
0 0, = =
3
3
3
3
f
Control of industrial robots Robot dynamics P. Rocco [43]
Definition of vectors and matrices
Let us refer all the quantities to the current fram on the link. We derive these
constant vectors:
The rotation matrices are:
(
(
(

=
(
(
(

=
(
(
(

=
(
(
(

=
0
0 ,
0
0 ,
0
0 ,
0
0
2
2
2 , 1
2 2
2
, 2
1
1
1 , 0
1 1
1
, 1
2 1
a a l a a l
C C
r r r r
(
(
(

=
(
(
(

=
(
(
(

=
1 0 0
0 1 0
0 0 1
,
1 0 0
0
0
,
1 0 0
0
0
2
3 2 2
2 2
1
2 1 1
1 1
0
1
R R R c s
s c
c s
s c
Control of industrial robots Robot dynamics P. Rocco [44]
Forward recursion: link 1
( )
(
(
(

= + =
1
0 1 0
0
1
1
1
0
0
&
&
z R
T
( )
(
(
(

= + + =
1
0 0 1 0 1 0
0
1
1
1
0
0
& &
& & &
& &
z z R
T
(
+
2
&
( )
(
(
(

+
+
= + + =
0
1 1 1
1
2
1 1
1
1 , 0
1
1
1
1
1
1 , 0
1
1 0
0
1
1
1
gc a
gs a
T
& &
&
&
& & & &
r r p R p
( )
(
(
(

+
+
= + + =
0
1 1 1
1
2
1 1
1
, 1
1
1
1
1
1
, 1
1
1
1
1
1
1 1 1
gc l
gs l
C C C
& &
&
&
& & & &
r r p p
Control of industrial robots Robot dynamics P. Rocco [45]
( )
(
(
(

+
= + =
2 1
0 2
1
1
1
2
2
2
0
0
& &
&
z R
T
( )
(
(
(

+
= + + =
2 1
0
1
1 2 0 2
1
1
1
2
2
2
0
0
& & & &
& & &
& &
z z R
T
( )
(
2
Forward recursion: link 2
( )
( )
( )
(
(
(
(

+ + + +
+ +
= + + =
0
12
2
1 2 1 2 1 2 1 2 1
12
2
2 1 2
2
1 2 1 1 2 1
2
2 , 1
2
2
2
2
2
2 , 1
2
2
1
1
1
2
2
2
gc s a a c a
gs a c a s a
T
& & & & & & &
& & & & &
&
& & & &
r r p R p
( )
( )
( )
(
(
(
(

+ + + +
+ +
= + + =
0
12
2
1 2 1 2 1 2 1 2 1
12
2
2 1 2
2
1 2 1 1 2 1
2
, 2
2
2
2
2
2
, 2
2
2
2
2
2
2
2 2
gc s a l c a
gs l c a s a
C C C
& & & & & & &
& & & & &
&
& & & &
r r p p
Control of industrial robots Robot dynamics P. Rocco [46]
( )
( ) ( )
(
(
(
(
(

+ + + +
|

\
|
+ +
= = + =
0
12
2
1 2 1 2 1 2 1 2 1 2
12
2
2 1 2
2
1 2 1 1 2 1 2
2
2
2
2
3
3
2
3
2
2
2 2
gc s a l c a m
gs l c a s a m
m m
C C
& & & & & & &
& & & & &
& & & &
p p f R f
( ) ( )
(
= + + + + + =
2
2
2
2
2
2
2
2
2
2
2
, 2
3
3
2
3
3
3
2
3
2
, 2
2
2 , 1
2
2
2
2
2 2
C C
&
I I r f R R r r f
Backward recursion: link 2
( )( )
(
(
(

+ + + + +

=
12 2 2
2
1 2 2 1 2 1 2 2 1 2 2 1
2
2 2 2
gc l m s l a m c l a m l m I
& & & & & & &
( ) ( ) ( )
12 2 2
2
1 2 2 1 2 2
2
2 2 2 1 2 2 1
2
2 2 2
0
1
2
2
2 2
gc l m s l a m l m I c l a l m I
T T
+ + + + + + =
= =
& & & & &
z R
(identical to the equation obtained with Euler-Lagrange method).
Control of industrial robots Robot dynamics P. Rocco [47]
( ) ( ) ( )
( ) ( ) ( ) ( )
(
(
(
(

+ + + + + +
+ + + +
= + =
0
1 2 1
2
2 1 2 2 2 2 1 2 2 2 1 1 2 1 1
1 2 1
2
2 1 2 2 2
2
1 1 2
2
1 1 1 2 1 2 2 2
1
2
2
2
1
2
1
1
1
gc m m s l m c l m a m l m
gs m m c l m a m l m s l m
m
C
& & & & & & & &
& & & & & & & &
& &
p f R f
( ) ( )
(
(
(

= + + + + + =
1
1
1
1
1
1
1
1
1
1
1
, 1
2
2
1
2
2
2
1
2
1
, 1
1
1 , 0
1
1
1
1
1 1
C C
&
I I r f R R r r f
Backward recursion: link 1
( ) ( )( )
( ) ( )
(
(
(
(
(

+ + + + +
+ + + + + + + +

=
12 2 2 1 1 2 1 1
2
2 1 2 2 1 2
2
1 2 2 1 2
2 1
2
2 2 2 2 1 2 2 1 2 2 1 2
2
1 1
2
1 2 1
gc l m gc a m l m s l a m s l a m
l m c l a m I c l a m l m a m I
& & &
& & & & & &
( ) ( ) ( ) ( )
( )
12 2 2 1 1 2 1 1
2
2 2 2 1 2 2 1 2 2 1 2
2 2 2 1
2
2 2 2 1 2 2 1
2
2
2
1 2 2
2
1 1 1
0
0
1
1
1 1
2
2
gc l m gc a m l m s l a m s l a m
c l a l m I c l a l a m I l m I
T T
+ + +
+ + + + + + + + =
= =
& & &
& & & &
z R
(identical to the equation obtained with Euler-Lagrange method).
Control of industrial robots Robot dynamics P. Rocco [48]
Euler-Lagrange vs. Newton-Euler
Euler-Lagrange formulation
it is systematic and easy to understand
it returns the equations of motion in an analytic and compact form, separating
the inertia matrix, the Coriolis and centrifugal terms, the gravitational terms. All
these elements are useful for the design of a model based controller
it lends itself to the introduction into the model of more complex effects (like
joint or link deformation)
Newton-Euler formulation
it is a computationally efficient recursive method
Control of industrial robots Robot dynamics P. Rocco [49]
Direct and inverse dynamics
Direct dynamics
For given joint torques (t), determine the joint accelerations q(t) and, if initial
positions q(t
0
) and velocities q(t
0
) are known, the positions q(t) and the velocites
q(t).
( ) ( ) ( ) ( ) = + + + + q q f q F q g q q q C q q B
& & & & & &
, ,
s v
.
..
.
Problem whose solution is useful in order to compute the simulation model of
the robot manipulator
It can be solved both with Euler-Lagrange and with Newton-Euler approaches
Inverse dynamics
For given accelerations q(t), velocities q(t) and positions q(t) determine the joint
torques (t) needed for motion generation.
. ..
It can be solved both with Euler-Lagrange and with Newton-Euler approaches
Problem whose solution is useful for trajectory planning and model based
control
It can be efficiently solved with the Newton-Euler formulation
Control of industrial robots Robot dynamics P. Rocco [50]
Computation of direct and inverse dynamics
( ) ( ) = + q q n q q B
& & &
,
As for the computation of the direct dynamics, let us rewrite the dynamic model of
the manipulator in these terms:
where:
Computation of the inverse dynamics can be easily done both with the Euler-
Lagrange method and with the Newton-Euler one.
( ) ( ) ( ) ( ) q q f q F q g q q q C q q n
& & & & &
, , ,
s v
+ + + =
We does have to numerically integrate the explicit system of differential
equations:
( ) ( ) ( ) q q n q B q
& & &
,
1
=


where all the elements needed to build the system are directly computed by the
Euler-Lagrange method.
Control of industrial robots Robot dynamics P. Rocco [51]
With the current values of q and q, a first iteration of the script is performed,
setting q = 0. In this way the torques computed by the method directly return
How to compute the direct dynamics with the Newton-Euler method?
.
..
Computation of direct and inverse dynamics
Newton-Euler script (Matlab, C, ):
( ) q q q
& & &
, , =
setting q = 0. In this way the torques computed by the method directly return
the vector n.
Then we set g
0
= 0 inside the script (in order to eliminate the gravitational effects)
and q = 0 (in order to eliminate Coriolis, centrifugal and friction effects). n
iterations of the script are performed, with q
i
= 1 and q
j
= 0, ji. This way matrix B
is formed column by column and all elements to form the system of equations are
available.
..
.
..
Control of industrial robots Robot dynamics P. Rocco [52]