You are on page 1of 28

# Robotics 2

## Dynamic model of robots:

Lagrangian approach
Prof. Alessandro De Luca

Dynamic model
!

## provides the relation between

generalized forces u(t) acting on the robot
robot motion, i.e.,
assumed configurations q(t) over time
ujoints(t)

uCartesian(t)
q(t)

## a system of 2nd order

differential equations
Robotics 2

) = u
"(q, q , q
2

Direct dynamics
!

direct relation
" u1 %
u(t) = \$ ! '
\$\$ ''
# uN &

## input for t [0,T]

" q1 %
q(t) = \$ ! '
\$\$ ''
# qN &

q(0),q (0)

initial state at t != 0

!
!

experimental solution
!
! apply torques/forces with motors and measure joint variables
with encoders (with sampling time Tc )

) = u
"(q, q , q
solution by simulation
! use dynamic model and integrate numerically the differential
equations (with simulation step Ts ! Tc )

Robotics 2

Inverse dynamics
!

inverse relation

## qd (t), q d (t), q d (t)

ud(t)

required input
for t [0,T]

desired motion
for t [0,T]

!
!

experimental solution
! repeated motion trials of direct dynamics using uk(t), with
iterative learning of nominal torques updated on trial k+1 based
on the error in [0,T] measured in trial k: uk(t) ud(t)

) = u
"(q, q , q
analytic solution
! use dynamic model and compute algebraically the values ud(t)
at every time instant t

Robotics 2

## Approaches to dynamic modeling

Euler-Lagrange method
(energy-based approach)
!

dynamic equations in
symbolic/closed form
best for study of dynamic
properties and analysis of
control schemes

Newton-Euler method
(balance of forces/torques)
!

dynamic equations in
numeric/recursive form
best for implementation of
control schemes (inverse
dynamics in real time)

## " many formal methods based on basic principles in mechanics are

available for the derivation of the robot dynamic model:
" principle of dAlembert, of Hamilton, of virtual works,
Robotics 2

Euler-Lagrange method
(energy-based approach)

## basic assumption: the N links in motion are considered as rigid bodies

(+ possibly, concentrated elasticity at the joints)

q " IR N

## Lagrangian L(q, q ) = T(q, q ) " U(q)

kinetic energy potential energy
" least action principle of Hamilton
" virtual works principle

!
!
Euler-Lagrange
equations

d " L "L
#
= ui
dt "q i "qi

i = 1, ..., N

## non-conservative (external or dissipative)

generalized forces performing work on qi

Robotics 2

a first example
viscous friction
n"1

b!

"

" m = n "
" = n"m

bm

!m, "m

=0

## q = " (or q = "m )

!
(with reduction gear)
T = Tm + T!
d
!
1 2
1
!
Tm = I m " m T! = (I ! + md2 )" 2
!
2 !
2
Fx
motor inertia
link inertia
m g0
!
(around its
(around the z-axis
motor
transmission!

1!
1 2
2
2
2

2
2
!

Robotics 2

## Dynamics of actuated pendulum (cont)

- d cos "

!m, "m
n!m, "

Fx

p x = ! sin"
p x = ! cos " # " = J x!
"
!

## U = U0 " mg0 dcos #

potential energy

1 2
L = T " U = I # + mg0 dcos # " U0
2

"L
= I #
"#

d "L
= I #
dt "#

"L
= \$mg0 dsin#
"#

## applied or dissipated torques

on motor side are multiplied by n
when moved to the link side
Robotics 2

## equivalent joint torque

due to force Fx applied to
the tip at point px

sum of
non-conservative
torques
8

!m, "m

## dynamic model in q = "

I" + mg0d sin" = n#m \$ (b ! + n2bm ) " + ! cos "% Fx

n!m, "

!
% b!
(
I m
"m
!
"m

"
+
g
d
sin
=
#
\$
+
b
"
+
cos
+ Fx
' 2
0
m
m* m
2 m
n
n
n
n
n
&n
)

!

Robotics 2

## Kinetic energy of a rigid body

vc body B
!

mass m = # "(x,y,z)dx dy dz = # dm
B

rc
RF0

1
position of
rc = " r dm
mB
center of mass (CoM)
when!all vectors are referred to a body frame
RFc attached to the CoM, then
!
rc = 0 " B r dm = 0

RFc

kinetic energy
(fundamental)
kinematic relation
for a rigid body
!

Robotics 2

mass density

T=

1
2

"

T
v
(x,y,z) v(x,y,z)dm
B

v = v c + " # r = v c + S(")r
skew-symmetric matrix
10

## Kinetic energy of a rigid body (cont)

T=

1
T
v
+
S(")r
[
]
[v c + S(")r ] dm
c
#
2B

1
1 T T
T
T
= # v c v c dm + # v c S(")r dm + # r S (")S(")r dm
2B
2B
B
1
T
= m vc vc
2

!
!

translational
kinetic energy
!
(point mass
in CoM)

Knig theorem

Robotics 2

= v c S(") # r dm = 0
B

1
2

sum of elements
on the diagonal
of a matrix

aTb=trace{abT}

\$ trace{ S(")r# r

S T (")} dm

%
( T
1
T
= trace{ S(")' \$ r# r dm* S (")}
2
&B
)

+
rotational
kinetic energy
(of the whole body)

1
= trace{ S(") Jc S T (")}
2
1
= " T Ic " Euler matrix
Ex #1: provide the expressions
2

(around the CoM)

## of the elements of Euler matrix Jc

Ex #2: prove last equality and
provide the expressions of the
elements of inertia matrix Ic

11

## Examples of body inertia matrices

homogeneous bodies of mass m, with axes of symmetry
parallelepiped with sides
a (length/height), b, c (base)

y
a

"I
xx
\$
Ic =
\$
#

!
b

" 1 m(a2 + b2 )
\$2
Ic = \$
\$
#

x
a

## Izz' = Izz + m(h 2)2

Steiner theorem
!

I = Ic + m (r r" E3#3 \$ rr
!

Robotics 2

Iyy

1
12

m(a2 + c2 )

%
'
'
1
m(a2 + b2 )'&
12

## empty cylinder with length h,

and external/internal radius a, b

2
2
% " 12 m(b + c )
' =\$
' \$
Izz & \$
#

## body inertia matrix

relative to the CoM

)=I

identity
matrix

+ m S (r)S(r)
Ex #3: prove the
last equality

1
12

m 3(a2 + b2 ) + h2

%
'
'
Izz '
&

Izz = Iyy

## (parallel) axis translation theorem

its generalization:
!
changes on body inertia matrix
due to a pure translation r of
the reference frame
12

N

T = " Ti

i=1

## open kinematic chain

Ti = Ti (q j , q j , j " i)
!

vci

Knig theorem

!i
RFci
i-th link (body)
of the robot
Robotics 2

1
1 T
T
Ti = mi v ci v ci + " i Ici " i
2
2
absolute velocity of
the center of mass (CoM)

absolute
angular velocity
of whole body

13

## Kinetic energy of a robot link

1
1 T
T
Ti = mi v ci v ci + " i Ici " i
2
2
!i, Ici should be expressed in the same reference frame,
but the product !iTIci!i is invariant w.r.t. any chosen frame

## in frame RFci attached to (the center of mass of) link i

\$ " ( y 2 + z2 )dm
&
i
Ici = &
&
symm
%

constant!
Robotics 2

# " x y dm

2
2
x
+
z
)dm
"(

## # " x zdm '

)
# " y zdm )
)
2
2
x
+
y
dm
) (
"(

14

.
Dependence of T from q and q
!i

vci

link i-1

qN

link 1

q1

q2

qi-1

link i

link N

qi

" !
v ci = JLi (q) q = \$ 1 ! i
\$\$
# !
(partial) Jacobians
typically expressed in RF0
# !
" i = JAi(q) q = % 1 ! i
%%
!
\$ !
Robotics 2

qi+1

0 ! 0%
0 ! 0 ' q
''
0 ! 0&

3 rows

0 ! 0&
0 ! 0 ( q
((
0 ! 0'

3 rows

15

Final expression of T
1 N
T = # (mi v ciT v ci + " iT Ici " i)
2 i=1
NOTE:
in practice, NEVER
!
use this formula
(or partial Jacobians)
for computing T;
a better method
!
is available...

N
&
1 T#
T
T
= q % "mi JLi (q)JLi(q) + JAi(q)Ici JAi(q)( q
2 \$ i=1
'

1 T
T(q, q ) = q B(q)q
2

constant if ! i
is expressed in RFci
else
0
Ici(q)= 0 R i (q) i Ici 0 R iT (q)

! matrix
robot (generalized) inertia
" symmetric
" positive definite,q always invertible
Robotics 2

16

## Robot potential energy

assumption: GRAVITY contribution only
N

U = " Ui

## N rigid bodies (+ fixed base)

i=1

Ui = Ui (q j , j " i)

!
!

## open kinematic chain

Ui = " mi g Tr0,ci
gravity acceleration
position of the
vector
center of mass of link i

dependence
! on q
"r0,ci % 0
"ri,ci %
1
i(1
\$ 1 ' = A1 (q1 ) A2 (q2 )! A i (qi ) \$ 1 '
# &
# &
Robotics 2

## NOTE: need to work with homogeneous coordinates

typically
expressed
in RF0
constant
in RFi

17

Summarizing ...
positive definite

1 T
1
T = q B(q)q = " bij (q)q i q j # 0 quadratic form
2
2 i,j
T = 0 q = 0
U = U(q)

kinetic
energy
potential
energy

## L(q, q ) = T(q, q ) " U(q)

Lagrangian

Euler-Lagrange !
equations

d "L
"L
#
= uk
dt "q k "qk

k = 1,...,N

non-conservative (active/dissipative)
generalized forces performing work on qk coordinate

!
Robotics 2

18

## Applying Euler-Lagrange equations

(the scalar derivation; see Appendix for vector format)

L(q, q ) =

"L
= # b kj q j
"q k
j
!

(dependences on q
are not shown)

1
" bij (q)q iq j # U(q)
2 i,j

"b kj
d "L
j + #
= # b kjq
q i q j
dt "q k
j
i,j "qi
"L 1 "bij
"U
= #
q i q j \$
"qk 2 i,j "qk
"qk

..
LINEAR terms in ACCELERATION q
.
QUADRATIC terms in VELOCITY q

Robotics 2

19

## k-th dynamic equation ...

d "L
"L
#
= uk
dt "q k "qk

% # b kj 1 # b ij (
#U

j
i, j &
i
k )
k

exchanging
indices i,j

!+* &
+
#
) qiq j + !
" qj " qk (
i, j 2 % " qi

## ckij = ckji Christoffel symbols

of the first kind

Robotics 2

20

## " bkj (q)q j +" ckij (q) q iq j +

j

INERTIAL
terms

i, j

#U
= uk
# qk

CENTRIFUGAL (i=j)
and CORIOLIS (i#j) terms

k = 1,,N

GRAVITY
terms gk(q)

!
bkk(q) = inertia at joint k when joint k accelerates (bkk > 0!!)

## bkj(q) = inertia seen at joint k when joint j accelerates

ckii(q) = coefficient of the centrifugal force at joint k when
joint i is moving (ciii = 0, i)
ckij(q) = coefficient of the Coriolis force at joint k when both
joint i and joint j are moving
Robotics 2

21

## Robot dynamic model

in vector formats

1.

+ c(q, q ) + g(q) = u
B(q)q
c k (q, q ) = q T C k (q)q

k-th column
of matrix B(q)

2.
NOTE:
!
these models
are in the form

) = u
"(q, q , q

as expected
Robotics 2

T
#
1 " bk # " bk &
"B &
Ck (q) = %
+%
)
(
(
2 % " q \$ " q ' " qk (
\$
'

k-th component
of vector c
symmetric
matrix

+ S(q, q )q + g(q) = u
B(q)q
NOT a
symmetric
matrix

## s kj (q, q ) = " c kij (q)q i

i

factorization of c
by S is not unique!
22

A structural property
matrix B " 2S is skew-symmetric

## (when using Christoffel symbols to define matrix S)

Proof
kj
b = # "b
! q i
kj
i "qi

% # b kj # b ki # b ij (
1
2s kj = 2" c kjiq i = 2" '
+
\$
* q i
# qj # qk )
i
i 2 & # qi

## b " 2s = \$ # b ij " # b ki ' q = n

*i & # q # q ) i kj
kj
kj
j (
% k

\$ # b ik # b ji '
because of the

## njk = b jk " 2s jk = * &

"
q
=
"n
) i
kj
symmetry of B
# qk (
i % # qj

x T B " 2S x = 0, #x
Robotics !
2

23

Energy conservation
!

## total robot energy

1
E = T + U = q TB(q)q + U(q)
2

## its evolution over time (using the dynamic model)

+ 1 q TB (q)q + " U q
E = q TB(q)q
2
"q
!
1
= q T (u # S(q, q )q # g(q)) + q TB (q)q + q T g(q)
2
1
= q Tu + q T B (q) # 2S(q, q ) q
2

## if u0, total energy is constant (no dissipation or increase)

E = 0

E = q Tu

q T B " 2S q = 0, #q, q

## weaker than skew-symmetry,

as the external velocity is the same
that appears in the internal matrices

!Robotics 2

here, any
factorization
of vector c
by a matrix S
can be used

## in general, the variation

of the total energy is
equal to the work of
non-conservative forces
24

## Dynamic model of a PR robot

T = T1 + T2

y
d
dc1

pc2
q2

!
x

q1

" q1 + d cos q2 %
p c2 = \$ d sinq2 '
\$
'
0
#
&

Robotics 2

## # q1 " dc1 &

p c1 = % 0 (
% 0 (
\$
'

U = constant
(on horizontal plane)

v c1

T
= p c1
p c1 = q 12

1
T1 = m1q 12
2
!
1
1 T
T
T2 = m2 v c2 v c2 + "2 Ic2 "2
2
2
! q2 q 2 &
#0&
# q 1 " d sin
v c2 = % d cos q2 q 2 ( "2 = % 0 (
%
(
%% ((
%
(
0
\$
'
\$ q 2 '

1
1
2
2 2

2
2
!
!

25

# m1 + m2
B(q) = %
\$ "m2d sinq2
b1

## "m2d sinq2 &

Ic2,zz + m2d2 ('
b2

" c1 (q, q ) %
c(q, q ) = \$
'

c
q,
q
# 2( )&
c k (q, q ) = q T C k (q)q

T
#
1 " bk # " bk &
"B &
where Ck (q) = %
+%
)
(
(
!
%
2 " q \$ " q ' " qk (
\$
'
!
0
0
& #0
& #0 0&&
1 ##0
C1 (q) = % %
+%
"%
(
(
(
2 \$ \$ 0 "m2d cosq2 ' \$ 0 "m2d cosq2 ' \$ 0 0 (' '
!
c1 (q, q ) = "m2d cos q2 q 22

0
1 # # 0 "m2d cos q2 & #
C2 (q) = % %
+%
(
2 \$\$0
0
' \$ "m2d cos q2
=0
Robotics 2

0& #
0
"m2d cos q2 & &
"%
((
(
0 ' \$ "m2d cos q2
0
''
!
c2 (q, q ) = 0
26

+ c(q, q ) = u
B(q)q
# m1 + m2
% "m d sin q
\$ 2
2

## "m2d sinq2 & # q1 & # "m2d cos q2 q 22 & #u1 &

( = %u (
2 ( % ( + %
q
Ic2,zz + m2d ' \$ 2 ' \$
0
' \$ 2'

## NOTE: the bNN element (here, for N=2)

is always a constant!

## Q1: why Coriolis terms are not present?

Q2: when applying a force u1, does the second joint accelerate? always?
Q3: what is the expression of a factorization matrix S? is it unique?
Q4: which is the configuration with maximum inertia?
Robotics 2

27

Appendix:
Vector format derivation of dynamic model
T
T
d #" L & #" L &
% ( )% ( = u
dt \$ " q ' \$ " q '

1
L = q TB(q)q " U(q)
[0
2

... 1 ... 0]

## B(q) = [b1 (q) ... bi (q) ... bN (q)] = "bi(q)e iT

!

i=1

i-th
position

dyadic expansion
!
! T
T
N #
#
&
#" L &
T
d
"
L
" bi &
T

+ B(q)q = B(q)q
+ )% ( q i q
% ( = B(q)q
% ( = (q B(q)) = B(q)q
dt \$ " q '
\$ " q '
i=1 \$ " q '
!
T
T
T
T
N #
# " L & T # 1 T # N " bi T & " U & # 1 T # N " bi & " U &
&
#
&
1
" bi
"U

e i (q * (( = %% q %)
qi ( * (( = )% ( qi q * % (
% ( = %% q % )
2 i=1 \$ " q '
\$ " q' \$ 2 \$ i=1 " q ' " q ' \$ 2 \$ i=1 " q ' " q '
\$ " q'
!
T' .
T
+N \$
\$
'
\$
'
" bi 1 " bi ) 0
"U
&

B(q)q + *&
# & ) )qi q + & ) = u
-, i=1 % " q 2 % " q ( ( 0/ % " q (
Robotics 2

S(q, q )

g(q)

28