This action might not be possible to undo. Are you sure you want to continue?
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]
Lagrange’s 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 Lagrange’s 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
&
Lagrange’s equations:
mg
ϑ
τ
l
[ ] ϑ =
(
¸
(
¸
ϑ
ϑ
− − = − = sin
sin
cos
0
0
mgl
l
l
g m m U
l
T
p g
An example
Lagrange’s 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
(skewsymmetric 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
König 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 Lagrange’s 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 n×n 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]
Skewsymmetry 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 skewsymmetric:
( ) 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 − =
(skewsymmetric 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: n×p matrix, function of joint positions, velocities and accelerations (regression
matrix)
( )π ππ π τ ττ τ q q q Y
& & &
, , =
Control of industrial robots – Robot dynamics − −− − P. Rocco [23]
Twolink Cartesian manipulator
Consider a twolink 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
Twolink 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 endeffector:
( ) ( )
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
Twolink Cartesian manipulator
Control of industrial robots – Robot dynamics − −− − P. Rocco [26]
Twolink planar manipulator
Let us consider a twolink 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
ϑ
1
ϑ
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
Twolink 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 − =
Twolink 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 skewsymmetric:
( ) ( ) ( )
( )
q q C q B q q N
&
& & &
&
& &
&
&
&
0
2
0
2
, 2 ,
2 1 2 2 2
h
h h
h
h h
=
(
¸
(
¸
ϑ −
ϑ + ϑ ϑ
−
(
¸
(
¸
ϑ
ϑ ϑ
= − =
Twolink 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 endeffector, the motion equations
are:
Twolink 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 = π
Twolink 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
& & &
Twolink 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 noncausal) 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 leastsquares technique:
−
Identification of dynamical parameters
( ) τ ττ τ π ππ π
T T
Y Y Y
1 −
= Left pseudoinverse 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]
NewtonEuler formulation
An alternative way to formulate the dynamic model of the manipulator is the
NewtonEuler 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 NewtonEuler
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
i1,Ci
vector from the origin of frame (i1) 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
i1, i
vector from the origin of frame (i1) 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 i1 on link i
f
i+1
force exerted by link i+1 on link i
µ µµ µ
i
moment exerted by link i1 on link i with respect to the origin of frame i1
µ µµ µ
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]
NewtonEuler formulation
Newton’s equation (translational motion of the center of mass)
i
C i i i i
m m p g f f
& &
= + −
+ 0 1
Euler’s 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
i1,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]
Twolink planar manipulator
Let us consider again a twolink planar manipulator
with rotational joints, whose model has been
already derived with the EulerLagrange 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
ϑ
1
ϑ
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 EulerLagrange 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 EulerLagrange method).
Control of industrial robots – Robot dynamics − −− − P. Rocco [48]
EulerLagrange vs. NewtonEuler
EulerLagrange 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)
NewtonEuler 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 EulerLagrange and with NewtonEuler 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 EulerLagrange and with NewtonEuler approaches
Problem whose solution is useful for trajectory planning and model based
control
It can be efficiently solved with the NewtonEuler 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 NewtonEuler 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
EulerLagrange 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 NewtonEuler method?
.
..
Computation of direct and inverse dynamics
NewtonEuler 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, j≠i. 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]
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)
q(t)
Control of industrial robots – Robot dynamics − P. Rocco [2]
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:
z P
T =
1 &T & mp p 2
O x
y
Similarly, for a system of points: 1 T = 2
∑
i =1
n
&i & m i pT p i
z P O x
Consider now a rigid body, with mass m, volume V and density ρ. The kinetic energy is defined with the integral:
dV y
1 T = 2
∫
V
& & pT pρdV
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:
dW = −dU
An example of a system of conservative forces is the gravitational force. For a point mass we have the potential energy:
U = −mg T p 0
where g0 is the gravity acceleration vector. For a rigid body:
U=−
∫
V
T g T pρdV = − mg 0 pl 0
where pl 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:
h (x ) = 0
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 h (x ) = 0 If the constraints h are composed of s scalar equations and they are all continuously differentiable. For example in a robot with 6 joints. or natural. or generalized coordinates. n is the number of degrees of freedom of the mechanical system. it is possible. to eliminate s coordinates from the system equations. Control of industrial robots – Robot dynamics − P. out of the 36 original coordinates. or Lagrangian. 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). The remaining n = 6r – s coordinates are called free. Rocco [6] . by means of the constraints.
The elementary work performed by the forces acting on the system can be expressed as: dW = ∑ ξ dq i i =1 n i It can be proven that the dynamics of the system of bodies is expressed by the following Lagrange’s equations: d ∂L ∂L − = ξi . q ) − U (q ) where T and U are the kinetic and the potential energies. & dt ∂q i ∂q i i = 1. Let then ξi be the generalized forces associated with the generalized coordinates qi. q ) = T (q. we define Lagrangian of the mechanical system the quantity: & & L(q. n Control of industrial robots – Robot dynamics − P. whose positions and orientations can be expressed by means of n generalized coordinates qi.Lagrange’s equations Given a system of rigid bodies. Rocco [7] . respectively.K.
Kinetic energy of the motor y C l τ mg ϑ x Tm = 1 &2 Im ϑ 2 Kinetic energy of the load Tc = 1 &2 Iϑ 2 Control of industrial robots – Robot dynamics − P. subjected to the gravitational force. Let: Im and Ii 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 load from the axis of the motor.An example Let us consider a system composed of a motor rigidly connected to a load. Rocco [8] .
An example Gravitational potential energy: l τ mg ϑ l cos ϑ T U = −mg 0 pl = −m[0 − g ] = mgl sin ϑ l sin ϑ Lagrangian: L = Tm + Tc − U = 1 & (I m + I )ϑ2 − mgl sin ϑ 2 Lagrange’s equations: d ∂L ∂L − =τ & dt ∂ϑ ∂ϑ Then: ⇒ d & (I + I m )ϑ + mgl cos ϑ = τ dt ( ) && (I + I m )ϑ + mgl cos ϑ = τ This equation can be easily interpreted as the equilibrium of moments around the rotation axis. Control of industrial robots – Robot dynamics − P. Rocco [9] .
Rocco [10] .Kinetic energy of a link The contribution of kinetic energy of a single link can be computed with the following integral: Ti = pi* Position of the center of mass: 1 2 ∫ Vi & & pi* pi* ρdV T generic point along the link pl i = 1 mi ∫ Vi pi* ρdV &* & & Velocity of the generic point: pi = pl i + ω i × ri = pl i + S (ω i )ri 0 S (ω i ) = ωiz − ωiy − ωiz 0 ωix ωiy − ωix 0 where: (skewsymmetric matrix) Control of industrial robots – Robot dynamics − P.
Kinetic energy of a link Translational contribution: 1 2 ∫ Vi 1 & T& & T& pl i pl i ρdV = mi pl i pl i 2 Mutual contribution: ∫ Vi & T & T pl i S (ω i )ri ρdV = pl i S (ω i ) ∫ (p Vi * i − pl i ρdV = 0 ) Rotational contribution: 1 2 ∫ Vi riT S T (ω i )S (ω i )ri ρdV = = 1 T ωi 2 ∫ S T (ri )S (ri )ρdV ω i = Vi Note: S (ω i )ri = −S (ri )ω i 1 T ωi I i ωi 2 Then: Ti = 1 1 & T& mi pl i pl i + ωT I i ω i i 2 2 König Theorem velocity of center of mass angular velocity of the link Control of industrial robots – Robot dynamics − P. Rocco [11] .
if expressed in the base frame. Rocco [12] .Inertia tensor We define inertia tensor the matrix: r 2 + r 2 ρdV iz iy Ii = * * ∫( ) ∫ (r − rix riy ρdV 2 ix 2 + riz ρdV ∫ ) * ∫( − rix riz ρdV I ixx = * − riy riz ρdV 2 2 rix + riy ρdV * ∫ ∫ ) − I ixy I iyy * − I ixz − I iyz I izz (symmetric matrix) The inertia tensor. Moreover it results: I i = Ri I ii RiT Control of industrial robots – Robot dynamics − P. 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): ω ii = RiT ω i the inertia tensor referred to this frame is a constant matrix.
Sum of the contributions Let us sum the translational and rotational contributions: Ti = 1 1 & T& mi pl i pl i + ωT Ri I ii RiT ω i i 2 2 Linear velocity: (i & (l & ( & & pl i = j Pl 1)q1 + K + j Pii )q i = J Pl i )q ( (i J Pl i ) = j Pl 1) L [ (l j Pii ) 0 L 0 ] ] Angular velocity: (l i & (l & (l & ω i = jO1)q1 + K + jOii )q i = JOi )q ( (l i JOl i ) = jO1) L [ (l jOii ) 0 L 0 Columns of the Jacobian: j (l i ) Pj ) (l jOji z j −1 0 = z j −1 × pl − p j −1 i z j −1 prismatic joint ( ) rotational joint Control of industrial robots – Robot dynamics − P. Rocco [13] .
symmetric positive definite depends on q Control of industrial robots – Robot dynamics − P.Inertia matrix Substituting expressions for the linear and angular velocities: Ti = 1 ( & 1 & (l ( & & ( mi q T J Pl i )T J Pl i )q + q T JOi )T Ri I ii RiT JOl i )q 2 2 By summing the contributions of all the links we obtain the kinetic energy of the whole arm: 1 T = 2 where: ∑∑ i =1 j =1 n n & & bij (q )q i q j = 1 &T & q B(q )q 2 B(q ) = (m J ( ) J ( ) + J ( ) R I R ∑ n i li T P li P li T O i i i i =1 T (l i ) i JO ) is the inertia matrix of the manipulator. Rocco [14] .
Rocco [15] .Potential energy The potential energy of a rigid link is related just to the gravitational force: Ui = − ∫ V T T g 0 pi* ρdV = − mi g 0 pl i where g0 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: U= ∑ i =1 n Ui = − ∑ i =1 n mi g T pl i 0 Control of industrial robots – Robot dynamics − P.
Motion equations The Lagrangian of the manipulator is: 1 & & L(q. q ) − U (q ) = 2 ∑∑ i =1 j =1 n n & & bij (q )q i q j + ∑ i =1 n mi g T pli (q ) 0 If we differentiate the Lagrangian: d dt ∂L ∂q &i d ∂T d = dt ∂q = dt &i ∑ & bij (q )q j = j =1 n ∑ j =1 n && bij (q )q j + ∑ j =1 n dbij (q ) dt & qj = n ∂bij (q ) && & & = bij (q )q j + q k q j ∂q k j =1 j =1 k =1 ∑ n n ∑∑ n Furthermore: ∂T 1 = ∂q i 2 ∑∑ j =1 k =1 n ∂b jk (q ) ∂q i & & qk q j n n ∂U (l ) T ∂plj = − m j g0 = − m j g T j P j (q ) = g i (q ) 0 i ∂q i ∂q i j =1 j =1 ∑ ∑ Control of industrial robots – Robot dynamics − P. Rocco [16] . q ) = T (q.
n where: hijk = ∂bij ∂q k − 1 ∂b jk 2 ∂q i gravitational term.K. depends only on the joint positions Acceleration terms: bii : inertia moment as “seen” from the axis of joint i bij : effect of the acceleration of joint j on the joint i Centrifugal and Coriolis terms: . Rocco [17] .. hijkqjqk : Coriolis effect induced at joint i by the velocities of joints j e k hiii = 0 Control of industrial robots – Robot dynamics − P. hijjqj2 : centrifugal effect induced at joint i by the velocity of joint j .Motion equations From Lagrange’s equations we obtain: & ∑ b (q )q& + ∑∑ h ij j j =1 j =1 k =1 n n n ijk & & (q )q k q j + g i (q ) = ξ i i = 1.
q ) diagonal matrix of viscous friction coefficients function that models the static friction at the joint Control of industrial robots – Robot dynamics − P. other forces act on the manipulator: actuation torques viscous friction torques static friction torques τ & −Fv q & − fs (q. Rocco [18] .q ) Fv & fs (q.Non conservative forces Besides the gravitational conservative forces.
Complete dynamic model In vector form the dynamic model can be expressed as follows: && & & & & B (q )q + C (q. Rocco [19] . q ) + g (q ) = τ where C is a suitable n×n matrix.q )q + Fv q + fs (q. whose elements satisfy the equation: ∑ c q& = ∑∑ h ij j j =1 j =1 k =1 n n n ijk q k q j & & C is not symmetric in general Control of industrial robots – Robot dynamics − P.
Rocco [20] . One possible choice is the following one: ∑ j =1 n & c ij q j = ∑∑ j =1 k =1 n n & & hijk q k q j = n ∑∑ n ∂bij 1 ∂b jk ∂q − 2 ∂q k i j =1 k =1 n n & & q k q j = 1 = 2 ∑∑ ∑c k =1 n 1 & & qk q j + ∂q k 2 j =1 k =1 n n ∂bij ∑∑ ∂bik ∂b jk − ∂q j ∂q i j =1 k =1 q k q j & & The generic element of C is: c ij = ijk q k & where: c ijk 1 ∂bij ∂bik ∂b jk = + − ∂q i 2 ∂q k ∂q j Christoffel symbols of the first kind Control of industrial robots – Robot dynamics − P.Computation of the elements of C The choice of matrix C is not unique.
Skewsymmetry of matrix B−2C − The previous choice of matrix C allows to prove an important property of the dynamic model of the manipulator. Matrix: & & & N (q. Rocco [21] . q )w = 0. q ) is skewsymmetric: & w T N (q. ∀w In fact: n n ∂bij ∂b 1 1 ∂bik − jk &k + c ij = q 2 k =1 ∂q k 2 k =1 ∂q j ∂q i . q ) = B (q ) − 2C (q. ∑ ∑ n ∂b 1& 1 q k = bij + ∂bik − jk & 2 2 k =1 ∂q j ∂q i ∑ q k & & nij = bij − 2c ij = ∑ ∂b jk ∂bik − ∂q i ∂q j k =1 n q k & nij = −n ji (skewsymmetric matrix) Control of industrial robots – Robot dynamics − P.
Energy conservation The equation: & & & q T N (q.q ) − g (q )) from which the equation follows. From the energy conservation principle. q )q = 0 (particular case of the previous one) is valid whatever the choice of matrix C is. the derivative of the kinetic energy equals the power generated by all the forces acting at the joint of the manipulator: 1 d &T & & & & q B(q )q = q T (τ − Fv q − fs (q. Control of industrial robots – Robot dynamics − P. Rocco [22] ( ) ( ) ( ) . q ) − g (q )) 2 dt Taking the derivative at the left hand side and using the equation of the model: 1 d &T 1& & & & & && 1 & & & & q B(q )q = q T B (q )q + q T B(q )q = q T B (q ) − 2C (q. q ) q 2 dt 2 2 & & & + q T (τ − Fv q − fs (q.
velocities and accelerations (regression matrix) Control of industrial robots – Robot dynamics − P. function of joint positions. q )π π: vector of p constant parameters Y: n×p matrix. q.Linearity in the dynamical parameters If we assume a simplified expression for the static friction function: & & fs (q. moments of inertia) We can then write: & && τ = Y (q. Rocco [23] . q ) = Fs sgn(q ) it is possible to prove that the dynamic model of the manipulator is linear with respect to a suitable set of dynamic parameters (masses.
characterized by link masses m1 e m2.Twolink Cartesian manipulator d2 m2 z1 m1 z0 d1 x0 Consider a twolink Cartesian manipulator. Control of industrial robots – Robot dynamics − P. Rocco [24] . The vector of generalized coordinates is: d q = 1 d 2 The Jacobians needed for the computation of the inertia matrix are the following ones: 0 0 ( J Pl1 ) = 0 0 1 0 0 1 ( J Pl 2 ) = 0 0 1 0 while there are no contributions to the angular velocities.
i. there are no centrifugal and Coriolis terms. Then. we obtain: m + m 2 ( ( ( ( B = m1J Pl1 )T J Pl1 ) + m2 J Pl 2 )T J Pl 2 ) = 1 0 0 m2 As B is constant. C=0. since: 0 g0 = 0 − g the vector of the gravitational terms is: (m + m2 )g g= 1 0 Control of industrial robots – Robot dynamics − P.e.Twolink Cartesian manipulator Computing the inertia matrix with the general formula. Rocco [25] .
Twolink Cartesian manipulator If there are no friction torques and no forces at the endeffector: & (m1 + m2 )d&1 + (m1 + m2 )g = f1 && m2 d 2 = f2 f1 e f2: forces which act along the generalized coordinates Control of industrial robots – Robot dynamics − P. Rocco [26] .
Twolink planar manipulator a2 y0 m2. I1 l1 ϑ1 a1 x0 ϑ2 Let us consider a twolink planar manipulator: masses: m1 and m2 lengths: a1 and a2 distances of the centers of mass from the joint axes: l1 and l2 moments of inertia around axes passing through the centers of mass and parallel to z0: I1 and I2 ϑ q = 1 Generalized coordinates: ϑ 2 − l 2s12 l 2c12 0 The Jacobians needed for the computation of the inertia matrix are the following ones: − l1s1 0 ( J Pl1 ) = l1c1 0 0 0 0 0 ( JOl1 ) = 0 0 1 0 − a1s1 − l 2s12 ( J Pl 2 ) = a1c1 + l 2c12 0 0 0 ( JOl 2 ) = 0 0 1 1 Control of industrial robots – Robot dynamics − P. Rocco [27] . I2 l2 m1.
so that the computation of the inertia matrix gives: (l1 )T (l1 ) (l 2 )T (l 2 ) (l1 )T (l1 ) (l 2 )T (l 2 ) b11 (ϑ) b12 (ϑ) B(q ) = m1J P J P + m2 J P J P + I1JO JO + I 2 JO JO = b21 (ϑ) b22 2 2 2 b11 = m1l1 + I1 + m2 a1 + l 2 + 2a1l 2 c 2 + I 2 2 b12 = b21 = m2 l 2 + a1l 2c 2 + I 2 2 b22 = m2 l 2 + I 2 ( ( ) ) depends on ϑ2 depends on ϑ2 constant! Control of industrial robots – Robot dynamics − P.Twolink planar manipulator Taking into account that the angular velocity vectors ω1 e ω2 are aligned with z0 it is not necessary to compute rotation matrices Ri . Rocco [28] .
Rocco [29] .Twolink planar manipulator From the inertia matrix it is simple to compute the Christoffel symbols: c111 = 1 ∂b11 =0 2 ∂q1 1 ∂b11 = −m2a1l 2s 2 = h 2 ∂q 2 h = −m2a1l 2s 2 c112 = c121 = c122 = c 211 = ∂b12 1 ∂b22 − = −m2a1l 2 s2 = h ∂q 2 2 ∂q1 ∂b21 1 ∂b11 − = m2a1l 2s 2 = −h ∂q1 2 ∂q 2 1 ∂b22 =0 2 ∂q1 c 212 = c 221 = c 222 = 1 ∂b22 =0 2 ∂q 2 Control of industrial robots – Robot dynamics − P.
the vector of the gravitational terms is: ( ) (m l + m2a1 )gc1 + m2 gl 2c12 g = 11 m2 gl 2c12 Control of industrial robots – Robot dynamics − P. q ) = B & & hϑ 2 0 − hϑ1 0 & & 0 − 2hϑ1 − hϑ2 T & = = −N (q. Rocco [30] . q ) = = & & 0 − hϑ1 m2 a1l 2s 2 ϑ1 ( ) & & − m2 a1l 2s 2 ϑ1 + ϑ2 0 ( ) We can verify that matrix N is skewsymmetric: & & & & & & (q ) − 2C (q.q ) = 2hϑ2 hϑ2 − 2 hϑ2 h ϑ1 + ϑ2 = & & N (q. q ) & + hϑ & 0 2 2hϑ1 Furthermore.Twolink planar manipulator The expression of matrix C is then: & & & & hϑ2 h ϑ1 + ϑ2 − m2a1l 2 s 2 ϑ2 & C (q. since g0 = [0 −g 0]T.
Rocco [31] . the motion equations are: (m l 2 2 2 && && + I1 + m2 a1 + l 2 + 2a1l 2c 2 + I 2 ϑ1 + m2 l 2 + a1l 2c 2 + I 2 ϑ2 + & & & − 2m2a1l 2 s2 ϑ1ϑ2 − m2 a1l 2s 2 ϑ2 + 2 2 11 ( ) ) ( ( ) ) ) + (m1l1 + m2a1 )gc1 + m2 gl 2c12 = τ1 (m (l 2 && && + a1l 2c 2 + I 2 ϑ1 + m2 l 2 + I 2 ϑ2 + & 2 + m2a1l 2s2 ϑ1 + m2 l 2 gc12 = τ 2 2 2 2 ) ) ( τi : torques applied at the joints Control of industrial robots – Robot dynamics − P.Twolink planar manipulator Without friction at the joints and forces at the endeffector.
q )π We have: π = [π1 π 2 π3 π4 π5 ] T π1 = m1l1 2 π 2 = I1 + m1l1 π 3 = m2 π 4 = m2 l 2 2 π 5 = I 2 + m2 l 2 Control of industrial robots – Robot dynamics − P. Rocco [32] .e. we can obtain the dynamical parameters with respect to which the model is linear. i. q. those parameters for which we can write: & && τ = Y (q.Twolink planar manipulator By simple inspection.
Twolink planar manipulator y 12 y & . their first and second derivatives. q ) = 11 && Y (q. ϑ2. Rocco [33] . Control of industrial robots – Robot dynamics − P. q 0 0 y 11 = gc1 && y =ϑ 12 y 13 0 y 14 y 24 y 15 y 25 y 13 = y 14 y 15 + a1gc1 && && & & & = 2a1c 2 ϑ1 + a1c 2 ϑ2 − 2a1s 2 ϑ1ϑ2 − a1s 2 ϑ2 + gc12 2 && && =ϑ +ϑ 1 2 1 2 && a1 ϑ1 && &2 y 24 = a1c 2 ϑ1 + a1s 2 ϑ1 + gc12 && && y =ϑ +ϑ 25 1 2 The coefficients of Y depend on ϑ1. g and a1.
Control of industrial robots – Robot dynamics − P. directly (with suitable sensors) or indirectly. from the measurements of currents in the motors.Identification of dynamical parameters The linearity of the dynamic model with respect to the dynamical parameters: & && τ = Y (q. tN. along which the joint positions q are . Suitable motion trajectories must be executed. q. …. Suppose to have the measurements (direct or indirect ones) of all the variables for the time instants t1. recorded. . the velocities q are measured or obtained by numerical differentiation. Also the torques τ are measured. which are usually unknown or uncertain. and the accelerations q are obtained with filtered (also noncausal) differentiation.. Rocco [34] . q )π allows to setup a procedure for the experimental identification of the same parameters.
The trajectories to be used must be sufficiently rich (good conditioning of matrix − − YTY): they explore the robot workspace and involve all components in the dynamic model Control of industrial robots – Robot dynamics − P.Identification of dynamical parameters With N measurement sets: τ (t1 ) Y (t1 ) τ = M = M π = Y π τ (t N ) Y (t N ) Solving with a leastsquares technique: π= Y Y T ( ) −1 Y τ T − Left pseudoinverse matrix of Y Only the elements of π for which the corresponding column is different from zero can be identified. Rocco [35] . Some parameters are identifiable only in combination with other ones.
We obtain a system of equations that might be solved in a recursive way.NewtonEuler formulation An alternative way to formulate the dynamic model of the manipulator is the NewtonEuler method. Rocco [36] . propagating the velocities and accelerations from the base to the end effector. Control of industrial robots – Robot dynamics − P. while the forces and moments in the opposite way: v acc eloci ele ties rat io n s fo m o r c es me nts Recursion makes NewtonEuler algorithm computationally efficient. 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.
Springer. L. Oriolo: Robotics: Modelling.Ci ri. Planning and Control. G. L. Siciliano. Villani.Ci ri1. Rocco [37] . 3rd Ed. Sciavicco.Definition of the parameters Let us consider the generic link i of the kinematic chain: This picture is taken from the textbook: B. i vector from the origin of frame (i1) to the center of mass Ci vector from the origin of frame i to the center of mass Ci vector from the origin of frame (i1) to the origin of frame i Control of industrial robots – Robot dynamics − P. 2009 We define the following parameters: mi and Ii mass and inertia tensor of the link ri1.
Rocco [38] . pCi linear acceleration of the center of mass Ci ... ωi angular acceleration of the link g0 gravity acceleration fi fi+1 µi force exerted by link i1 on link i force exerted by link i+1 on link i moment exerted by link i1 on link i with respect to the origin of frame i1 µ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. pCi linear velocity of the center of mass Ci . pi linear acceleration of the origin of frame i . pi linear velocity of the origin of frame i ωi angular velocity of the link . Control of industrial robots – Robot dynamics − P.Definition of the variables .
Rocco [39] .Ci − µ i +1 − fi +1 × ri .Ci = Generalized force at joint i: d & (I i ω i ) = I i ω i + ωi × (I i ω i ) dt fiT z i −1 prismatic joint τi = T µ i z i −1 rotational joint Control of industrial robots – Robot dynamics − P.NewtonEuler formulation Newton’s equation (translational motion of the center of mass) && fi − fi +1 + mi g 0 = mi pCi it can be proven Euler’s equation (rotational motion) gyroscopic effect µ i + fi × ri −1.
i ) && && & pCi = pi + ω i × ri .i & & pi = & pi −1 + ω i × ri −1.i prismatic joint rotational joint Note: derivative of a vector ai attached to the moving frame i d d ai (t ) = Ri (t )aii = dt dt i = S ( i (t ))Ri (t )ai = i (t ) × ai (t ) Propagation of the accelerations: & prismatic joint ω i −1 & ωi = && & & ω i −1 + ϑi z i −1 + ϑi ω i −1 × z i −1 rotational joint && & pi −1 + d i z i −1 + 2d i ω i × z i −1 + ω i × ri −1.i + ω i × (ω i × ri −1.i + ω i × (ω i × ri −1.i ) prismatic joint & && &&i = p && & rotational joint pi −1 + ω i × ri −1. Rocco [40] .Ci + ω i × ω i × ri .Ci ( ) center of mass Control of industrial robots – Robot dynamics − P.Accelerations of a link Propagation of the velocities: prismatic joint ω i −1 ωi = & ω i −1 + ϑi z i −1 rotational joint & pi −1 + d i z i −1 + ω i × ri −1.
computation of ωi. pi.. . p0g0.i + ri . pCi A backward recursion of forces and moments is made: terminal conditions on fn+1 and µn+1 computations: & µ i = −fi × ri −1. initial conditions on ω0..Recursive algorithm v acc eloci el e t i es rat io n s A forward recursion of velocities and .. Rocco [41] fo m o r ces me nts && fi = fi +1 + mi pCi .Ci + µ i +1 + fi +1 × ri .Ci + I i ω i + ω i × (I i ω i ) The generalized force at joint i is computed: & fiT z i −1 + Fvi d i + fsi τi = T & µ i z i −1 + Fvi ϑi + fsi friction contributions ( ) prismatic joint rotational joint Control of industrial robots – Robot dynamics − P.accelerations is made: . ωi. . ω0 .
It is more convenient to express the vectors with respect to the current frame on link i. Rocco [42] . 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.Local reference frames Up to now we have supposed that all the vectors are referred to the base frame.Ci and the inertia tensor Ii are constant. Control of industrial robots – Robot dynamics − P. which makes the algorithm computationally more effcient. vectors ri1. In this way.i e ri.
µ3 = 0 3 Control of industrial robots – Robot dynamics − P. Rocco [43] . T 0 && 0 p0 − g 0 = [0 g 0] .Twolink planar manipulator a2 y0 m2. whose model has been already derived with the EulerLagrange method: masses: m1 and m2 lengths: a1 and a2 distances of the centers of mass from the joint axes: l1 and l2 moments of inertia around axes passing through the centers of mass and parallel to z0: I1 and I2 Initial conditions for the forward recursion of velocities and accelerations. I2 l2 m1. I1 l1 ϑ1 a1 x0 ϑ2 Let us consider again a twolink planar manipulator with rotational joints. &0 ω0 = ω0 = 0 0 Initial conditions for the backward recursion of forces and moments: f33 = 0.
Rocco [44] . r0. We derive these constant vectors: l1 − a1 a1 1 = 0 . R3 = 0 1 0 0 0 1 1 Control of industrial robots – Robot dynamics − P. The rotation matrices are: c1 − s1 0 c 2 0 1 R1 = s1 c1 0.C2 r122 .Definition of vectors and matrices Let us refer all the quantities to the current fram on the link. 0 0 l 2 − a 2 = 0 .C1 2 r2. R2 = s 2 0 0 0 1 − s2 c2 0 0 1 0 0 2 0. 0 a2 =0 0 1 r1.1 = 0 .
C1 1 1 ( ) &2 − l1ϑ1 + gs1 && = l1ϑ1 + gc1 0 Control of industrial robots – Robot dynamics − P. Rocco [45] .1 + ω1 × ω1 × r0.C1 + ω1 × ω1 × r1.Forward recursion: link 1 0 & ω1 = R1 T ω 0 + ϑ1z 0 1 ( ) 0 = 0 & ϑ1 0 && & &1 & ω1 = R1 T ω 0 + ϑ1z 0 + ϑ1ω 0 × z 0 ( ) 0 = 0 && ϑ1 0 && 1 &&1 &1 1 p1 = R1 T p0 + ω1 × r0.1 1 1 ( ) &2 − a1ϑ1 + gs1 && = a1ϑ1 + gc1 0 1 &&1 &&1 & 1 1 pC1 = p1 + ω1 × r1.
Forward recursion: link 2 1 & ω 2 = R2T ω1 + ϑ2 z 0 2 1 ( ) 0 = 0 & & ϑ1 + ϑ2 1 & &2 & 1 && ω 2 = R2T ω1 + ϑ2 z 0 + ϑ2 ω1 × z 0 1 ( ) 0 = 0 && && ϑ1 + ϑ2 1 &&1 && 2 &2 p2 = R2T p1 + ω 2 × r122 + ω 2 × ω 2 × r122 .C2 2 2 ( ) a s ϑ − a c ϑ2 − l ϑ + ϑ 2 + gs && & & & 1 2 1 1 2 1 2 1 2 12 && && && &2 = a1c 2 ϑ1 + l 2 ϑ1 + ϑ2 + a1s2 ϑ1 + gc12 0 ( ( ) ) Control of industrial robots – Robot dynamics − P.C2 + ω 2 × ω 2 × r2. ( ) a s ϑ − a c ϑ2 − a ϑ + ϑ 2 + gs && & & & 1 2 1 1 2 1 2 1 2 12 && && && &2 = a1c 2 ϑ1 + a2 ϑ1 + ϑ2 + a1s 2 ϑ1 + gc12 0 ( ( ) ) 2 2 && 2 && 2 & 2 pC 2 = p2 + ω 2 × r2. 2 2 . Rocco [46] .
C2 + R3 µ 3 + R3 f33 × r2.C2 + I 2 ω 2 + ω 2 × I 2 ω 2 = 2 .Backward recursion: link 2 2 && 2 && 2 f22 = R3 f33 + m2 pC2 = m2 pC2 m a s ϑ − a c ϑ2 − l ϑ + ϑ 2 + gs && & & & 2 12 2 1 2 1 1 2 1 2 1 && && && &2 = m2 a1c 2 ϑ1 + l 2 ϑ1 + ϑ2 + a1s 2 ϑ1 + gc12 0 ( ( ( ) ) ) 2 2 2 2 2 2 &2 µ 2 = −f22 × r122 + r2. 3 2 2 ( ) ( ) ∗ = ∗ 2 && && + m a l c ϑ + m a l s ϑ2 + m l gc && & I 2 + m2 l 2 ϑ1 + ϑ2 2 12 2 1 2 12 2 1 2 2 12 ( )( ) 1 τ 2 = µ 2T R2T z 0 = 2 2 2 && && &2 = I 2 + m2 l 2 + a1l 2c 2 ϑ1 + I 2 + m2 l 2 ϑ2 + m2a1l 2s 2 ϑ1 + m2 l 2 gc12 ( ( )) ( ) (identical to the equation obtained with EulerLagrange method). Control of industrial robots – Robot dynamics − P. Rocco [47] .
Rocco [48] .1 + r1.C1 + I1 ω1 + ω1 × I1 ω1 = 1 2 1 1 ( − m l s ϑ + ϑ − m l ϑ2 − m a ϑ2 − m l c ϑ + ϑ 2 + (m + m )gs && && & & & & 2 2 2 1 2 11 1 2 1 1 2 2 2 1 2 1 2 1 2 && && && & & = (m1l1 + m2a1 )ϑ1 + m2 l 2c 2 ϑ1 + ϑ2 − m2 l 2 s2 ϑ1 + ϑ2 + (m1 + m2 )gc1 0 ( ) ( ) ( ( ) ) ) ( ) ∗ = ∗ I + m a 2 + m l 2 + m a l c ϑ + I + m a l c + m l 2 ϑ + ϑ + && && && 1 2 1 11 2 12 2 1 2 2 12 2 2 2 1 2 2 &2 & & + m2 a1l 2s 2 ϑ1 − m2 a1l 2s 2 ϑ1 + ϑ2 + (m1l1 + m2a1 )gc1 + m2 l 2 gc12 ( ( ) ( ) )( ) 0 τ1 = µ1T R1 T z 0 = 1 2 2 2 2 && && = I1 + m1l1 + I 2 + m2 a1 + l 2 + 2a1l 2c 2 ϑ1 + I 2 + m2 l 2 + a1l 2 c 2 ϑ2 & & & − 2m2a1l 2s 2 ϑ1ϑ2 − m2a1l 2s 2 ϑ2 + (m1l1 + m2a1 )gc1 + m2 l 2 gc12 2 ( ( )) ( ( )) (identical to the equation obtained with EulerLagrange method).C1 + R2µ 2 + R2 f22 × r1.Backward recursion: link 1 1 &&1 f11 = R2f22 + m2 pC1 1 1 1 1 1 1 1 &1 µ1 = −f11 × r0. Control of industrial robots – Robot dynamics − P.
NewtonEuler EulerLagrange formulation it is systematic and easy to understand it returns the equations of motion in an analytic and compact form. 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) NewtonEuler formulation it is a computationally efficient recursive method Control of industrial robots – Robot dynamics − P. the Coriolis and centrifugal terms. Rocco [49] .EulerLagrange vs. separating the inertia matrix.
Problem whose solution is useful in order to compute the simulation model of the robot manipulator It can be solved both with EulerLagrange and with NewtonEuler approaches Inverse dynamics . positions q(t0) and velocities q(t0) are known. Rocco [50] .q ) = τ Direct dynamics .. For given joint torques τ(t). if initial . . the positions q(t) and the velocites .. determine the joint accelerations q(t) and. Problem whose solution is useful for trajectory planning and model based control It can be efficiently solved with the NewtonEuler formulation Control of industrial robots – Robot dynamics − P. For given accelerations q(t). q )q + g (q ) + Fv q + fs (q. q(t). velocities q(t) and positions q(t) determine the joint torques τ(t) needed for motion generation.Direct and inverse dynamics && & & & & B(q )q + C (q.
q )q + g (q ) + Fv q + fs (q. q ) = τ where: & & & & & n (q. As for the computation of the direct dynamics. Control of industrial robots – Robot dynamics − P. let us rewrite the dynamic model of the manipulator in these terms: && & B(q )q + n (q. Rocco [51] .Computation of direct and inverse dynamics Computation of the inverse dynamics can be easily done both with the EulerLagrange method and with the NewtonEuler one. q ) We does have to numerically integrate the explicit system of differential equations: −1 && & q = B(q ) (τ − n (q. q )) where all the elements needed to build the system are directly computed by the EulerLagrange method. q ) = C (q.
centrifugal and friction effects). With the.. a first iteration of the script is performed. we set g0 = 0 inside the script (in order to eliminate the gravitational effects) and q = 0 (in order to eliminate Coriolis. with qi = 1 and qj = 0. n . q. iterations of the script are performed. . Then. setting q = 0. This way matrix B is formed column by column and all elements to form the system of equations are available. Rocco [52] .current values of q and q. Control of industrial robots – Robot dynamics − P. In this way the torques τ computed by the method directly return the vector n. …): & && τ = ΝΕ(q.Computation of direct and inverse dynamics How to compute the direct dynamics with the NewtonEuler method? NewtonEuler script (Matlab. j≠i.. C. q ) ..
This action might not be possible to undo. Are you sure you want to continue?
We've moved you to where you read on your other device.
Get the full title to continue listening from where you left off, or restart the preview.