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

generalized coordinates (e.g., joint variables, but not only!)

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

Dynamics of actuated pendulum


a first example
viscous friction
n"1

b!

"

" m = n "
" = n"m

bm

!m, "m

" m = n" + " m0


=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!

spinning axis) through its center of mass)

1!
1 2
2
2
2

kinetic energy T = I ! + md + n I m " = I "


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#
"#

u = n"m # b ! $ # nbm$ m + JxTFx = n"m # b ! + n2bm $ + ! cos $ %Fx

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

Dynamics of actuated pendulum (cont)


!m, "m

dynamic model in q = "


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

n!m, "

dividing by n and substituting " = "m/n


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

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

dynamic model in q = "m


!

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

body inertia matrix


(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

Robot kinetic energy


N

T = " Ti

N rigid bodies (+ fixed base)

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

NONLINEAR terms in CONFIGURATION q


Robotics 2

19

k-th dynamic equation ...


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

% # b kj 1 # b ij (
#U

" bkj (q)qj +" ' # q $ 2 # q * qiqj + # q = uk


j
i, j &
i
k )
k

exchanging
indices i,j

1 $ " b kj " b ki " b ij '


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

ckij = ckji Christoffel symbols


of the first kind

Robotics 2

20

and interpretation of dynamic terms

" 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

T2 = m2 (q1 + d q2 " 2d sinq2 q1 q2 ) + Ic2,zz q 22


2
2
!
!

25

Dynamic model of a PR robot (cont)


# 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

Dynamic model of a PR robot (cont)

+ 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