Robotics 2
Prof. Alessandro De Luca
Dynamic model of robots: Lagrangian approach
Dynamic model ! provides the relation between
generalized forces u(t) acting on the robot
robot motion, i.e., assumed configurations q(t) over time
!
"(q, ˙ q ,˙ ̇ q ) = u
ujoints(t) uCartesian(t)
q(t)
a system of 2nd order differential equations
Robotics 2 2
Direct dynamics
! direct relation
!
u(t) =
u1
!uN
"
#
$ $ $
%
&
' ' '
!
q(t) =
q1
!qN
"
#
$ $ $
%
&
' ' '
!
q(0), ˙ q (0)input for t ∈ [0,T] 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 ! use dynamic model and integrate numerically the differential
equations (with simulation step Ts ! Tc )
!
"(q, ˙ q ,˙ ̇ q ) = u
Robotics 2 3
Inverse dynamics
! inverse relation
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)
! analytic solution ! use dynamic model and compute algebraically the values ud(t)
at every time instant t
!
qd(t), ˙ q d(t),˙ ̇ q d(t) ud(t)
required input for t ∈ [0,T]
!
"(q, ˙ q ,˙ ̇ q ) = u
Robotics 2 4
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 d’Alembert, of Hamilton, of virtual works, …
Robotics 2 5
Euler-Lagrange method (energy-based approach)
!
q" IRN
!
ddt
"L" ˙ q i
#"L"qi
= ui
basic assumption: the N links in motion are considered as rigid bodies (+ possibly, concentrated elasticity at the joints)
" least action principle of Hamilton " virtual works principle
i = 1, ..., N
non-conservative (external or dissipative) generalized forces performing work on qi
generalized coordinates (e.g., joint variables, but not only!)
Lagrangian
!
L(q, ˙ q ) = T(q, ˙ q ) "U(q)
kinetic energy – potential energy
Euler-Lagrange equations
Robotics 2 6
Dynamics of actuated pendulum a first example
!m, "m
!, "
m g0
d
motor transmission (with reduction gear)
Fx
viscous friction bm
b!
!
!
!
T =12
I ! + md2 + n2Im( ) ˙ " 2 =12
I ˙ " 2kinetic energy
!
˙ " m = n ˙ "
!
"m = n"+ "m0= 0
!
q = "
!
T = Tm + T!
!
Tm =12
Im˙ " m
2
!
T! =12
I ! + md2( ) ˙ " 2
link inertia (around the z-axis
through its center of mass)
motor inertia (around its
spinning axis)
(or )
!
q = "m
!
" = n"mn " 1
Robotics 2 7
Dynamics of actuated pendulum (cont)
!
U = U0 "mg0 dcos#
n!m, " Fx
- d co
s "
!
L = T "U =12
I ˙ # 2 +mg0 dcos# "U0
!
"L" ˙ #
= I ˙ #
!
ddt"L" ˙ #
= I ˙ ̇ #
!
"L"#
= $mg0 dsin#
!
u = n"m #b! ˙ $ #nbm˙ $ m + Jx
TFx = n"m # b! + n2bm( ) ˙ $ + !cos$ %Fx
!m, "m
!
px = !sin"
!
˙ p x = !cos" # ˙ " = Jx˙ "
potential energy
“sum” of non-conservative
torques applied or dissipated torques
on motor side are multiplied by n when moved to the link side
equivalent joint torque due to force Fx applied to
the tip at point px
Robotics 2 8
g0
Dynamics of actuated pendulum (cont)
!
In2
˙ ̇ " m +mn
g0 d sin"m
n=#m $
b!n2 +bm
%
& '
(
) * ˙ " m +
!n
cos"m
n+Fx
n!m, "
!m, "m
!
I˙ ̇ " +mg0d sin" = n#m $ b! +n2bm( ) ˙ " + !cos"%Fx
dividing by n and substituting " = "m/n
dynamic model in q = "m
dynamic model in q = "
Robotics 2 9
Kinetic energy of a rigid body
RFc
vc
RF0
body B mass
!
m = "(x,y,z)dxdydzB# = dm
B#
mass density
rc
position of center of mass (CoM)
!
rc =1m
r dmB"
when all vectors are referred to a body frame RFc attached to the CoM, then
rc = 0
!
r dmB" = 0⇒
kinetic energy
!
T =12
v T (x,y,z)v(x,y,z)dmB"
(fundamental) kinematic relation for a rigid body
!
v = vc+" # r = vc + S(")r
skew-symmetric matrix
"
Robotics 2 10
Kinetic energy of a rigid body (cont)
König theorem
!
T =12
vc + S(")r[ ]T vc + S(")r[ ]dmB#
=12
vcTvc dm
B# + vc
TS(")r dmB# +
12
r TST (")S(")r dmB#
!
= vcTS(") r dm
B# = 0
!
=12
m vcTvc
!
=12
trace{S(")r# r TST (")}dmB$
=12
trace{S(") r# r T dmB$
%
& '
(
) * ST (")}
=12
trace{S(") Jc ST (")}
=12" T Ic "
aTb=trace{abT}
sum of elements on the diagonal
of a matrix
Euler matrix
translational kinetic energy (point mass
in CoM) rotational
kinetic energy (of the whole body)
body inertia matrix (around the CoM)
+
Robotics 2 11
Ex #1: provide the expressions of the elements of Euler matrix Jc
Ex #2: prove last equality and provide the expressions of the
elements of inertia matrix Ic
Examples of body inertia matrices homogeneous bodies of mass m, with axes of symmetry
parallelepiped with sides a (length/height), b, c (base)
!
Ic =Ixx
Iyy
Izz
"
#
$ $
%
&
' ' =
112 m b2 + c2( )
112 m a2 + c2( )
112 m a2 +b2( )
"
#
$ $ $
%
&
' ' '
x
y
z
b
c a
!
Ic =
12 m a2 +b2( )
112 m 3 a2 +b2( ) +h2( )
Izz
"
#
$ $ $
%
&
' ' '
empty cylinder with length h, and external/internal radius a, b
x
y
z a
h b
z’
!
Izz = Iyy
!
Izz' = Izz + m(h 2)2 (parallel) axis translation theorem
!
I = Ic +m r Tr" E3#3 $ rr T( ) = Ic +m ST(r)S(r)its generalization:
changes on body inertia matrix due to a pure translation r of
the reference frame body inertia matrix relative to the CoM
identity matrix
Steiner theorem
Robotics 2 12
Ex #3: prove the last equality
Robot kinetic energy
!
T = Tii=1
N
"
!
Ti = Ti qj , ˙ q j , j " i( )
!
Ti =12
mivciTvci +
12" i
T Ici" i
N rigid bodies (+ fixed base)
open kinematic chain
König theorem
absolute velocity of the center of mass (CoM)
absolute angular velocity of whole body
RFci
vci
i-th link (body) of the robot
i "
Robotics 2 13
Kinetic energy of a robot link
!
iIci =
y2 + z2( )dm" # x ydm" # xzdm"x2 + z2( )dm" # yzdm"
symm x2 + y2( )dm"
$
%
& & &
'
(
) ) )
!i, Ici should be expressed in the same reference frame, but the product !i
TIci!i is invariant w.r.t. any chosen frame
in frame RFci attached to (the center of mass of) link i
constant!
Robotics 2 14
!
Ti =12
mivciTvci +
12" i
T Ici" i
Dependence of T from q and q
q1
link 1
qi-1 qi
qN link i-1
link N link i q2
vci
qi+1
!
vci = JLi(q) ˙ q =! 0 ! 0! 0 ! 0! 0 ! 0
"
#
$ $ $
%
&
' ' '
˙ q i 1
!
" i = JAi(q) ˙ q =! 0 ! 0! 0 ! 0! 0 ! 0
#
$
% % %
&
'
( ( (
˙ q i 1
3 rows
3 rows
.
(partial) Jacobians typically expressed in RF0
" i
Robotics 2 15
Final expression of T
!
T =12
mi vciTvci +" i
TIci" i( )i=1
N
#
!
=12
˙ q T mi JLiT (q)JLi(q) + JAi
T (q)Ici JAi(q)i=1
N
"#
$ %
&
' ( ˙ q
robot (generalized) inertia matrix " symmetric " positive definite,∀q ⇒ always invertible
constant if ! i is expressed in RFci
!
T(q, ˙ q ) =12
˙ q TB(q) ˙ q else
!
0Ici(q)=0Ri(q)
iIci0R i
T (q)
NOTE: in practice, NEVER use this formula
(or partial Jacobians) for computing T; a better method
is available...
Robotics 2 16
Robot potential energy
!
U = Uii=1
N
"
!
Ui = Ui qj , j " i( )
assumption: GRAVITY contribution only
!
Ui = "migTr0,ci
gravity acceleration vector
position of the center of mass of link i
typically expressed
in RF0
!
r0,ci
1"
# $
%
& ' =
0A1 q1( )1 A2 q2( )!i(1 Ai qi( )ri,ci
1"
# $
%
& '
dependence on q constant
in RFi
NOTE: need to work with homogeneous coordinates
N rigid bodies (+ fixed base)
open kinematic chain
Robotics 2 17
Summarizing ...
!
T =12
˙ q TB(q) ˙ q = 12
bij (q) ˙ q i ˙ q ji,j" # 0
!
L(q, ˙ q ) = T(q, ˙ q ) "U(q)
!
U = U(q)
!
ddt
"L" ˙ q k
#"L"qk
= uk k = 1,...,N
T = 0
!
˙ q = 0
positive definite quadratic form kinetic
energy
potential energy
Lagrangian
Euler-Lagrange equations
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 k
= bkj ˙ q jj#
!
ddt
"L" ˙ q k
= bkj˙ ̇ q jj# +
"bkj
"qi
˙ q i ˙ q ji,j#
!
"L"qk
=12
"bij
"qk
˙ q i ˙ q ji,j# $
"U"qk
NONLINEAR terms in CONFIGURATION q
LINEAR terms in ACCELERATION q ..
QUADRATIC terms in VELOCITY q .
(dependences on q are not shown)
!
L(q, ˙ q ) =12
bij (q) ˙ q i ˙ q ji,j" #U(q)
Robotics 2 19
k-th dynamic equation ...
!
bkj(q)˙ ̇ q j +j"
#bkj
#qi
$12# bij
#qk
%
& '
(
) * ˙ q i˙ q j
i, j" +
#U#qk
= uk
!
! +12"bkj
"qi
+"bki
"qj
#"bij
"qk
$
% &
'
( ) ˙ q i˙ q j
i, j* +!
ckij = ckji Christoffel symbols
of the first kind
!
ddt
"L" ˙ q k
#"L"qk
= uk
exchanging indices i,j
Robotics 2 20
… and interpretation of dynamic terms
!
bkj(q)˙ ̇ q j +j" ckij(q) ˙ q i˙ q j
i, j" +
#U# qk
= uk
INERTIAL terms
CENTRIFUGAL (i=j) and CORIOLIS (i#j) terms
GRAVITY terms gk(q)
k = 1,…,N
bkj(q) = inertia “seen” at joint k when joint j accelerates
ckij(q) = coefficient of the Coriolis force at joint k when both joint i and joint j are moving
bkk(q) = inertia at joint k when joint k accelerates (bkk > 0!!)
ckii(q) = coefficient of the centrifugal force at joint k when joint i is moving (ciii = 0, ∀i)
Robotics 2 21
Robot dynamic model in vector formats
!
B q( )˙ ̇ q + c q, ˙ q ( ) + g q( ) = u
!
ck q, ˙ q ( ) = ˙ q TCk q( ) ˙ q
!
Ck q( ) =12
"bk
"q+
"bk
"q#
$ %
&
' (
T
)"B"qk
#
$ % %
&
' ( (
!
B q( )˙ ̇ q + S q, ˙ q ( )˙ q + g q( ) = u
!
skj q, ˙ q ( ) = ckij q( )˙ q ii"
k-th component of vector c
k-th column of matrix B(q) symmetric
matrix
NOT a symmetric
matrix
1.
2.
factorization of c by S is not unique!
!
"(q, ˙ q ,˙ ̇ q ) = u
NOTE: these models are in the form
as expected Robotics 2 22
Dynamic model of a PR robot
q1
q2
x
y
!
T1 =12
m1 ˙ q 12
!
T2 =12
m2vc2T vc2 +
12"2
T Ic2"2
pc2
!
pc2 =q1 + d cosq2
d sinq20
"
#
$ $
%
&
' '
!
vc2 =
˙ q 1 "d sinq2˙ q 2
d cos q2˙ q 2
0
#
$
% % %
&
'
( ( (
!
"2 =
00˙ q 2
#
$
% % %
&
'
( ( (
!
T2 =12
m2˙ q 1
2 + d2˙ q 22 "2d sinq2
˙ q 1 ˙ q 2( ) +12
Ic2,zz˙ q 2
2
!
T = T1 + T2 U = constant (on horizontal plane)
Robotics 2 23
d dc1
!
pc1 =q1 "dc1
00
#
$
% %
&
'
( (
!
vc12
= ˙ p c1T ˙ p c1 = ˙ q 1
2
pc1
Dynamic model of a PR robot (cont)
!
B(q) =m1 +m2 "m2d sinq2
"m2d sinq2 Ic2,zz +m2d2
#
$ %
&
' (
b1 b2
!
ck q, ˙ q ( ) = ˙ q TCk q( ) ˙ q
!
c q, ˙ q ( ) =c1 q, ˙ q ( )c2 q, ˙ q ( )"
# $
%
& '
where
!
C1 q( ) =12
0 00 "m2d cosq2
#
$ %
&
' ( +
0 00 "m2d cosq2
#
$ %
&
' ( "
0 00 0#
$ %
&
' (
#
$ %
&
' (
!
c1 q, ˙ q ( ) = "m2d cosq2˙ q 2
2
!
Ck q( ) =12
"bk
"q+
"bk
"q#
$ %
&
' (
T
)"B"qk
#
$ % %
&
' ( (
!
C2 q( ) =12
0 "m2d cosq2
0 0#
$ %
&
' ( +
0 0"m2d cosq2 0#
$ %
&
' ( "
0 "m2d cosq2
"m2d cosq2 0#
$ %
&
' (
#
$ %
&
' (
= 0
!
c2 q, ˙ q ( ) = 0
Robotics 2 24
Dynamic model of a PR robot (cont)
!
m1 +m2 "m2d sinq2
"m2d sinq2 Ic2,zz +m2d2
#
$ %
&
' (
˙ ̇ q 1˙ ̇ q 2# $ %
& ' ( + "m2d cosq2
˙ q 22
0#
$ %
&
' ( =
u1u2
# $ %
& ' (
!
B(q)˙ ̇ q + c(q, ˙ q ) = u
NOTE: the bNN element (here, for N=2) is always a constant!
Q4: which is the configuration with “maximum inertia”?
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?
Robotics 2 25
A structural property
matrix is skew-symmetric (when using Christoffel symbols to define matrix S)
!
˙ B " 2S
!
˙ b kj ="bkj
"qi
˙ q ii#
!
2skj = 2 ckji˙ q i
i" = 2
12#bkj
#qi
+#bki
#qj
$#bij
#qk
%
& '
(
) * ̇ q i
i"
!
˙ b kj "2skj =#bij
#qk
"#bki
#qj
$
% &
'
( ) ˙ q i = nkj
i*
!
njk = ˙ b jk "2s jk =#bik
#qj
"#bji
#qk
$
% &
'
( ) ˙ q i = "nkj
i*
Proof
because of the symmetry of B
!
x T ˙ B "2S( )x = 0, #xRobotics 2 26
! total robot energy
! its evolution over time (using the dynamic model)
! if u≡0, total energy is constant (no dissipation or increase)
Energy conservation
!
E = T + U =12
˙ q TB(q)˙ q + U(q)
!
˙ q T ˙ B "2S( )˙ q = 0, #q, ˙ q
here, any factorization of vector c
by a matrix S can be used
!
˙ E = ˙ q TB(q)˙ ̇ q + 12
˙ q T ˙ B (q)˙ q + "U"q
˙ q
= ˙ q T u#S(q, ˙ q )˙ q #g(q)( ) +12
˙ q T ˙ B (q)˙ q + ˙ q Tg(q)
= ˙ q Tu+12
˙ q T ˙ B (q) #2S(q, ˙ q )( ) ˙ q
!
˙ E = ˙ q Tu
!
˙ E = 0
weaker than skew-symmetry, as the external velocity is the same that appears in the internal matrices
in general, the variation of the total energy is equal to the work of
non-conservative forces Robotics 2 27
Appendix: Vector format derivation of dynamic model
Robotics 2 28
!
L =12
˙ q TB(q)˙ q "U(q)
!
ddt
"L" ˙ q
#
$ %
&
' (
T
)"L"q
#
$ %
&
' (
T
= u
!
B(q) = b1(q) ... bi(q) ... bN(q)[ ] = bi(q)eiT
i=1
N
"
!
0 ... 1 ... 0[ ]
i-th position
dyadic expansion
!
"L" ˙ q
#
$ %
&
' (
T
= ˙ q T B(q)( )T= B(q)˙ q
!
"L"q
#
$ %
&
' (
T
=12
˙ q T"bi
"qei
T
i=1
N
)#
$ %
&
' ( ̇ q *
"U"q
#
$ % %
&
' ( (
T
=12
˙ q T"bi
"q˙ q i
i=1
N
)#
$ %
&
' ( *
"U"q
#
$ % %
&
' ( (
T
=12
"bi
"q
#
$ %
&
' (
T
˙ q ii=1
N
) ˙ q *"U"q
#
$ %
&
' (
T
!
ddt
"L" ˙ q
#
$ %
&
' (
T
= B(q)˙ ̇ q + ˙ B (q)˙ q = B(q)˙ ̇ q +"bi
"q
#
$ %
&
' ( ̇ q
i=1
N
) ˙ q i
!
B(q)˙ ̇ q +"bi
"q#
12"bi
"q
$
% &
'
( )
T$
% & &
'
( ) ) ̇ q i
i=1
N
*+
, - -
.
/ 0 0
˙ q +"U"q
$
% &
'
( )
T
= u
!
skT (q, ˙ q ) = ˙ q TCk(q)k-th row of matrix S
!
g(q)
!
S(q, ˙ q )