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
"(q, q˙ , q
˙˙) = u
Robotics 2 2
 Direct dynamics
!  direct relation
" u1 % " q1 %
u(t) = $ ! ' q(t) = $ ! '
$$ '' $$ ''
# uN & # qN &

input for t ∈ [0,T] 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 )

!  solution by simulation "(q, q˙ , q
˙˙) = u
!  use dynamic model and integrate numerically the differential

equations (with simulation step Ts ! Tc )

Robotics 2 3
!
 Inverse dynamics
!  inverse relation

qd (t), q˙ d (t), ˙q˙ d (t) ud(t)

desired motion required input

! for t ∈ [0,T] 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)
!  analytic solution "(q, q˙ , q
˙˙) = u
!  use dynamic model and compute algebraically the values ud(t)

at every time instant t

Robotics 2 4

!
 Approaches to dynamic modeling
Euler-Lagrange method Newton-Euler method
(energy-based approach) (balance of forces/torques)
!  dynamic equations in !  dynamic equations in
symbolic/closed form numeric/recursive form
!  best for study of dynamic !  best for implementation of
properties and analysis of control schemes (inverse
control schemes 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 d’Alembert, of Hamilton, of virtual works, …

Robotics 2 5
 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 d " L "L

# = ui i = 1, ..., N
equations dt "q˙ i "qi
non-conservative (external or dissipative)
generalized forces performing work on qi
Robotics 2 6
!
 Dynamics of actuated pendulum
a first example

bm "˙ m = n "˙ " m = n" + " m0

viscous friction
" = n"m =0
n"1 !m, "m
b!
motor q = " (or q = "m )
transmission! !
d (with reduction gear) T = Tm + T!
!, " !
! 1 ˙2 1
Tm = I m " m T! = (I ! + md2 )"˙ 2
Fx ! 2 ! 2
m g0 motor inertia link inertia
! (around its (around the z-axis
spinning axis) through its center of mass)
!
! 1! 2 2 2 1 ˙2
kinetic energy T = I ! + md + n I m " = I "
( ˙ )
2 2
Robotics 2 7
 Dynamics of actuated pendulum (cont)
!m, "m U = U0 " mg0 dcos # potential energy
- d cos "

1 ˙2
g0 L = T " U = I # + mg0 dcos # " U0
2
n!m, " Fx
!
"L d "L "L
p x = ! sin" = I #˙ = I #˙˙ = $mg0 dsin#
˙p x = ! cos " # "˙ = J x!
"˙ "#˙ dt "#˙ "#

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

( )
! ! ! “sum” of
applied or dissipated torques
!
equivalent joint torque non-conservative
on motor side are multiplied by n due to force Fx applied to torques
when moved to the link side the tip at point px
!
Robotics 2 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
!

I ˙˙ m "m % b! (
˙ ! "m
"
2 m + g0 d sin = #m $ ' 2 + b "
m* m + cos + Fx
n n n &n ) n n

dynamic model in q = "m

!
Robotics 2 9
 Kinetic energy of a rigid body
mass density
vc body B
mass m = # "(x,y,z)dx dy dz = # dm
" B B

position of 1
rc = " r dm
center of mass (CoM) mB
RFc when!all vectors are referred to a body frame
RFc attached to the CoM, then
rc !
rc = 0 ⇒ " B r dm = 0
RF0 kinetic energy T=
1
v T
(x,y,z) v(x,y,z)dm
2
" B

(fundamental) !
kinematic relation v = v c + " # r = v c + S(")r
for a rigid body
! skew-symmetric matrix

Robotics 2 10
!
 Kinetic energy of a rigid body (cont)
1 T
T= # [ v c + S(")r ] [v c + S(")r ] dm
2B sum of elements
on the diagonal
1 T T 1 T T of a matrix
= # v c v c dm + # v c S(")r dm + # r S (")S(")r dm
2B B
2B
aTb=trace{abT}
1 T T 1
= m vc vc = v c S(") # r dm = 0 = $ trace{ S(")r# r T
S T (")} dm
2 B 2 B
!
1 % T
( T
translational = trace{ S(")' $ r# r dm* S (")}
2 &B )
! kinetic energy
!
(point mass 1
in CoM) + = trace{ S(") Jc S T (")}
2
rotational 1
König theorem kinetic energy = " T Ic " Euler matrix
(of the whole body)
2 Ex #1: provide the expressions
of the elements of Euler matrix Jc
Ex #2: prove last equality and
body inertia matrix provide the expressions of the
elements of inertia matrix Ic
(around the CoM)
Robotics 2 ! 11
 Examples of body inertia matrices
homogeneous bodies of mass m, with axes of symmetry

parallelepiped with sides

y
a (length/height), b, c (base)
a
c 1
% " 12 m(b + c ) %
2 2
x "I
b $ xx
' =$ 1
'
Ic = Iyy m(a2 + c2 )
$ ' $ 12 '
z # Izz & $ 1
m(a2 + b2 )'&
# 12

empty cylinder with length h,

y and external/internal radius a, b
h !
b " 1 m(a2 + b2 ) %
x $2 '
1
a Ic = $
$
12 (
m 3(a2 + b2 ) + h2 ) '
z’ z Izz ' Izz = Iyy
# &
Izz' = Izz + m(h 2)2 (parallel) axis translation theorem
Steiner theorem
its generalization:
!
T ! T T
I = Ic + m (r r" E3#3 $ rr )=I c + m S (r)S(r) changes on body inertia matrix
due to a pure translation r of
!
body inertia matrix identity Ex #3: prove the
the reference frame
relative to the CoM matrix last equality
Robotics 2 12
!
 Robot kinetic energy
N
T = " Ti N rigid bodies (+ fixed base)
i=1

Ti = Ti (q j , q˙ j , j " i) open kinematic chain

!
vci
"i König theorem
!
1 T 1 T
Ti = mi v ci v ci + " i Ici " i
2 2
RFci
absolute velocity of absolute
the center of mass (CoM) angular velocity
i-th link (body) ! of whole body
of the robot

Robotics 2 13
 Kinetic energy of a robot link
1 T 1 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 # " x y dm # " x zdm '
i
& 2 2
)
Ici = & "( x + z )dm # " y zdm )
& symm x 2
y 2
dm)
% "( + ) (
constant!

Robotics 2 14
!
 .
Dependence of T from q and q
"i vci
link i-1 qN
link 1
qi+1
q2 qi-1 link i link N
q1 qi
" ! 0 ! 0%
v ci = JLi (q) q˙ = $ 1 ! i 0 ! 0 ' q˙ 3 rows
$$ ''
# ! 0 ! 0&
(partial) Jacobians
typically expressed in RF0 # ! 0 ! 0&
" i = JAi(q) q˙ = % 1 ! i 0 ! 0 ( q˙ 3 rows
! %% ((
$ ! 0 ! 0'
Robotics 2 15
 Final expression of T
1 N
T = # (mi v ciT v ci + " iT Ici " i)
2 i=1
N
1 T# T T
&
NOTE: = q˙ % "mi JLi (q)JLi(q) + JAi(q)Ici JAi(q)( q˙
in practice, NEVER 2 $ i=1 '
!
use this formula constant if ! i
(or partial Jacobians) is expressed in RFci
for computing T; else
a better method
! 1 T 0
Ici(q)= 0 R i (q) i Ici 0 R iT (q)
is available...
T(q, q˙ ) = q˙ B(q)q˙
2
robot (generalized) inertia ! matrix
"  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
! typically
gravity acceleration position of the
expressed
vector center of mass of link i
in RF0
dependence
! on q
"r0,ci % 0 "ri,ci % constant
1 i(1
$ 1 ' = A1 (q1 ) A2 (q2 )! A i (qi ) $ 1 '
in RFi
# & # &
NOTE: need to work with homogeneous coordinates
Robotics 2 17
 Summarizing ...
positive definite
kinetic 1 T 1
T = q˙ B(q)q˙ = " bij (q)q˙ i q˙ j # 0 quadratic form
energy 2 2 i,j T = 0 q˙ = 0
potential U = U(q)
energy
Lagrangian L(q, q˙ ) = T(q, q˙ ) " U(q) !
!
Euler-Lagrange ! d "L "L
# = uk k = 1,...,N
equations dt "q˙ k "qk
!
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)

1
L(q, q˙ ) = " bij (q)q˙ iq˙ j # U(q)
2 i,j
"L d "L "b kj
= # b kj q˙ j = # b kjq
˙˙ j + # q˙ i q˙ j
"q˙ k j dt "q˙ k j i,j "qi
!
(dependences on q
are not shown) "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 + !
i, j 2 % " qi " qj " qk (

ckij = ckji Christoffel symbols

of the first kind
!
Robotics 2 20
 … and interpretation of dynamic terms
#U
" bkj (q)˙q˙ j +" ckij (q) q˙ iq˙ j + # qk
= uk k = 1,…,N
j i, j

INERTIAL CENTRIFUGAL (i=j) GRAVITY

terms and CORIOLIS (i#j) terms 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. B(q)q
˙˙ + c(q, q˙ ) + g(q) = u
c k (q, q˙ ) = q˙ T C k (q)q˙ k-th component
T
of vector c
k-th column #
1 " bk # " bk & "B &
of matrix B(q) Ck (q) = % +% ( ) ( symmetric
2 % " q $ " q ' " qk ( matrix
$ '
! !
2. B(q)q
˙˙ + S(q, q˙ )q˙ + g(q) = u
NOTE: !
these models NOT a
are in the form
symmetric s kj (q, q˙ ) = " c kij (q)q˙ i factorization of c
"(q, q˙ , q
˙˙) = u by S is not unique!
matrix i
as expected
Robotics 2 22
 Dynamic model of a PR robot
T = T1 + T2 U = constant
y (on horizontal plane)
d pc2 # q1 " dc1 &
2 T
p c1 = % 0 ( v c1 = p˙ c1 p˙ c1 = q˙ 12
dc1 % 0 (
q2 $ '
! 1
pc1 x T1 = m1q˙ 12
! 2
q1 !
1 T 1 T
T2 = m2 v c2 v c2 + "2 Ic2 "2
2 2
" q1 + d cos q2 % # q˙ 1 " d sin
! q2 q˙ 2 & #0&
p c2 = $ d sinq2 ' v c2 = % d cos q2 q˙ 2 ( "2 = % 0 (
$ ' % ( %% ((
0 % 0 (
# & ! $ ' $ q˙ 2 '
1 1
T2 = m2 (q1 + d q2 " 2d sinq2 q1 q2 ) + Ic2,zz q˙ 22
˙ 2 2 2
˙ ˙ ˙
2 2
! Robotics 2 ! ! 23
 Dynamic model of a PR robot (cont)
# m1 + m2 "m2d sinq2 & " c1 (q, q˙ ) %
B(q) = % c(q, q˙ ) = $ '
$ "m2d sinq2 Ic2,zz + m2d2 (' c q,
# 2( )& q˙
b1 b2 c k (q, q˙ ) = q˙ T C k (q)q˙
T
#
1 " bk # " bk & "B &
where Ck (q) = % +% ( ) (
! 2 " q $ " q ' " qk (
% !
$ '
!
1 ##0 0 & #0 0 & #0 0&&
C1 (q) = % % +% "% (
2 $ $ 0 "m2d cosq2 ' $ 0 "m2d cosq2 ' $ 0 0 (' '
( (
! c1 (q, q˙ ) = "m2d cos q2 q˙ 22
1 # # 0 "m2d cos q2 & # 0 0& # 0 "m2d cos q2 & &
C2 (q) = % % ( +% ( "% ((
! 2 $$0 0 ' $ "m2d cos q2 0 ' $ "m2d cos q2 0 ''
=0 ! c2 (q, q˙ ) = 0
Robotics 2 24
 Dynamic model of a PR robot (cont)

B(q)q
˙˙ + c(q, q˙ ) = u

# m1 + m2 "m2d sinq2 & # ˙q˙1 & # "m2d cos q2 q˙ 22 & #u1 &
% "m d sin q 2 ( % ˙˙ ( + % ( = %u (
$ 2 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 25
 A structural property
matrix B˙ " 2S is skew-symmetric
(when using Christoffel symbols to define matrix S)

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

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

kj kj *i & # q # q ) i kj
% k j (
! !
˙ $ # b ik # b ji ' because of the
njk = b jk " 2s jk = * & " q
˙
) i = "nkj symmetry of B
i % # qj # qk (
!

x T B˙ " 2S x = 0, #x
( )
Robotics !
2 26
 Energy conservation
!  total robot energy
1
E = T + U = q˙ TB(q)q˙ + U(q)
2
!  its evolution over time (using the dynamic model)
E˙ = q˙ TB(q)q˙˙ + 1 q˙ TB˙ (q)q˙ + " U q˙
! 2 "q here, any
1 factorization
= q˙ T (u # S(q, q˙ )q˙ # g(q)) + q˙ TB˙ (q)q˙ + q˙ T g(q) of vector c
2 by a matrix S
1 can be used
= q˙ Tu + q˙ T B˙ (q) # 2S(q, q˙ ) q˙
( )
2
!  if u≡0, total energy is constant (no dissipation or increase)
E˙ = 0 q˙ T B˙ " 2S q˙ = 0, #q, q˙
( ) E˙ = q˙ Tu
!
in general, the variation
weaker than skew-symmetry, of the total energy is
as the external velocity is the same equal to the work of
that appears in the internal matrices non-conservative forces
!Robotics 2 ! ! 27
 Appendix:
Vector format derivation of dynamic model
T T
d #" L & #" L & 1
% ( )% ( = u L = q˙ TB(q)q˙ " U(q)
dt $ " q˙ ' $ " q ' 2 [0 ... 1 ... 0]
N
B(q) = [b1 (q) ... bi (q) ... bN (q)] = "bi(q)e iT i-th
position
!
i=1

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

% ( = %% q % )
˙ e i (q * (( = %% q %)
˙ ˙ qi ( * (( = )% ( qi q * % (
˙ ˙ ˙
$ " q' $ 2 $ i=1 " q ' " q ' $ 2 $ i=1 " q ' " q ' 2 i=1 $ " q ' $ " q'
! !
+N $ T' . T
"b 1 "b $ ' $ "U '
B(q)q ˙˙ + -*& i # & i ) )q˙ i 0 q˙ + & ) = u
-, i=1 &% " q 2 % " q ( )( 0/ % " q (
! k-th row of matrix S
s kT (q, q˙ ) = q˙ T Ck (q) S(q, q˙ ) g(q)
Robotics 2 28