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) q(t) a system of 2nd order differential equations uCartesian(t)

,q ) = u (q, q
2

Robotics 2

Direct dynamics

direct relation
u1 u(t ) = u N q1 q(t ) = q N

input for t [0,T]

(0) q(0),q

initial state at t =0
experimental solution apply torques/forces with motors and measure joint variables with encoders (with sampling time Tc )

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

Robotics 2

Inverse dynamics

inverse relation
ud(t)

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

desired motion for t [0,T]

required input 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)

,q ) = u (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)

Newton-Euler method (balance of forces/torques)

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

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

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

Euler-Lagrange equations

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

d L L = ui i qi dt q

i = 1, ..., N

Robotics 2

non-conservative (external or dissipative) generalized forces performing work on qi

Dynamics of actuated pendulum

a first example viscous friction n1

bm

m, m

m = n = n m

m = n + m0
=0

q = (or q = m ) (with reduction gear) T = Tm + T d 1 2 1 2 Tm = I m m T = (I + md2 ) 2 2 Fx motor inertia link inertia m g0 (around its (around the z-axis
motor transmission
spinning axis) through its center of mass)

Robotics 2

1 1 2 2 2 2 kinetic energy T = I + md + n I m = I 2 2

Dynamics of actuated pendulum (cont) m, m

- d cos
U = U0 mg0 dcos

potential energy

nm,

Fx

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

p x = sin p x = cos = J x

L = I

d L = I dt

L = mg0 dsin

T nbm m + Jx + cos Fx u = n m b Fx = n m b + n2bm

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 nm,

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 =
+ mg d sin = n (b + n2b ) + cos F I 0 m m x

dividing by n and substituting = m/n

dynamic model in q = m
Robotics 2

Kinetic energy of a rigid body

vc body B RFc rc RF0
mass density

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

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

kinetic energy (fundamental) kinematic relation for a rigid body

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

Robotics 2

Kinetic energy of a rigid body (cont)

T= 1 T v + S ( ) r [ ] [v c + S() r ] dm c 2B
sum of elements on the diagonal of a matrix

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 center of mass)

aTb=trace{abT}

= v c S() r dm = 0
B

1 trace{ S() r r T S T ()} dm 2B

T 1 T = trace{ S() r r dm S ()} 2 B

+
rotational kinetic energy (of the whole body)

Knig theorem

1 = trace{ S() J S T ()} 2 1 Euler matrix = TI 2

body inertia matrix

Robotics 2

11

Examples of body inertia matrices

homogeneous bodies of mass m, with axes of symmetry
y c b z y h
b

a x

parallelepiped with sides a (length/height), b, c (base)

Ixx I = Iyy
1 2 2 12 m b + c = Izz

1 12

m a2 + c2

1 2 2 m a + b 12

empty cylinder with length h, and external/internal radius a, b

x a
1 m a2 + b2 2 I =

1 12

m 3 a2 + b2 + h2

((

Izz

Izz = Iyy

Izz' = Izz + m (h 2)2

single axis translation theorem its generalization: changes on body inertia matrix due to a pure translation r of the reference frame of computation
12

Steiner theorem
Robotics 2

I = Ic + m r r E33 rr

identity matrix

body inertia matrix relative to the center of mass

Robot kinetic energy

T = Ti
i=1 N

N rigid bodies (+ fixed base) open kinematic chain Knig theorem

1 1 T Ti = m i v T v + i I i i ci ci 2 2

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

vci

i RFci

i-th link (body) of the robot

Robotics 2

absolute velocity of center of mass

absolute angular velocity of whole body

13

Kinetic energy of a robot link

1 1 T Ti = m i v T v + i I i i ci ci 2 2
i, Ii should be expressed in the same reference frame, but the product iTIii 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 Ii = symm constant!

x y dm

x 2 + z2 dm

x zdm y zdm x 2 + y 2 dm

Robotics 2

14

. Dependence of T from q and q

i
link i-1 link 1

vci qN
link i

q1

q2

qi-1

qi+1

link N

qi
0 0 0 0 q 0 0
0 0 0 0 q 0 0
3 rows

= 1 i v ci = JLi (q) q (partial) Jacobians typically expressed in RF0 = 1 i i = JAi(q) q

Robotics 2

3 rows

15

Final expression of T
1 N T T = mi v ci v ci + iT Ii i 2 i=1

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

NOTE: in practice, NEVER use this formula (or partial Jacobians) for computing T; a better method is available...

1 T N T T = q mi JLi (q) JLi(q) + JAi(q) Ii JAi(q) q 2 i=1

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

matrix robot (generalized) inertia symmetric positive definite,q always invertible

Robotics 2

16

Robot potential energy

assumption: GRAVITY contribution only
U = Ui
i=1 N

N rigid bodies (+ fixed base) open kinematic chain

Ui = Ui (q j , j i)

Ui = mi g Tr0,ci
gravity acceleration position of the vector center of mass of link i typically expressed in RF0 constant in RFi

dependence on q r0,ci 0 ri,ci 1 i1 1 = A1 (q1 ) A2 (q2 ) A i (qi ) 1

Robotics 2

NOTE: need to work with homogeneous coordinates

17

Summarizing ...
kinetic energy potential energy Lagrangian

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

positive definite

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

d L L = uk k q k dt q

Euler-Lagrange equations

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

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

L j = b kj q k q j

(dependences on q are not shown)

b kj d L j + iq j = b kjq q k dt q j i, j qi L 1 bij U iq j = q qk 2 i, j qk q k

.. 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 k q k dt q

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

j + c kij (q) q iq j + q bkj (q)
j i, j

U = uk qk

k = 1,,N

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

INERTIAL terms

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

GRAVITY terms gk(q)

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

) = q T C k (q)q c k (q, q
T 1 b k b k B Ck (q) = + 2 q q qk

k-th column of matrix B(q)

k-th component of vector c symmetric matrix

2.
NOTE: these models are in the form as expected
Robotics 2

+ S(q, q )q + g(q) = u B(q)q

NOT a symmetric matrix

,q ) = u (q, q

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

i

factorization of c by S is not unique!

22

A structural property
(when using Christoffel symbols to define matrix S)

2S is skew-symmetric matrix B

Proof b kj i b kj = q i qi

b kj b ki b ij 1 i = 2 i 2s kj = 2 c kjiq + q qj qk i i 2 qi

b ij b ki i = nkj b kj 2s kj = q qj i qk

Robotics 2

b ik b ji because of the njk = b jk 2s jk = q = n i kj symmetry of B qk i q j

2S x = 0, x xT B

23

Energy conservation

total robot energy its evolution over time (using the dynamic model)
=q (q)q TB(q)q + 1 q TB + U q E 2 q 1 T T (u S(q, q )q g(q)) + q B(q)q +q T g(q) =q 2 (q) 2S(q, q Tu + 1 q T B ) q =q 2
here, any factorization of vector c by a matrix S can be used

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

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

=0 E

2S q T B = 0, q, q q
weaker than skew-symmetry, as the external velocity is the same that appears in the internal matrices

=q Tu E
in general, the variation of the total energy is equal to the work of non-conservative forces
24

Robotics 2

Dynamic model of a PR robot

y d
dc1

T = T1 + T2
pc2 q2

U = constant
(on horizontal plane)

1 2 1 T1 = m1q 2

q1

1 1 T T T2 = m 2 v c2 v c2 + 2 I 2 2 2 2
v c2 1 d sin q2 q 2 q 2 = d cos q2 q 0

q1 dc1 + d cos q2 p c2 = d sinq2 0

0 2 = 0 q 2

Robotics 2

1 1 2 2 2 2 T2 = m2 q1 + d q2 2d sinq2 q1 q2 + I2,zz q 2 2 2

25

Dynamic model of a PR robot (cont)

m1 + m2 B(q) = m2d sinq2
b1

m2d sinq2 I2,zz + m2d2

b2

) c1 (q, q ) = c(q, q c q , q 2 ( )
) = q T C k (q)q c k (q, q

T 1 b k b k 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 ) = m2d cos q2 q 2 c1 (q, q 2

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 ) = 0 c2 (q, q

26

Dynamic model of a PR robot (cont)

+ c(q, q ) = u B(q)q
m1 + m2 m d sin q 2 2 1 m2d cos q2 q 2 m2d sinq2 q u1 2 + = 2 2 I2,zz + m2d q 0 u2

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 q dt q

1 T B(q)q U(q) L= q [0 2
N

... 1 ... 0]
i-th position

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

i =1

dyadic expansion T T N L T d L bi T + B(q)q = B(q)q + q iq B(q)) = B(q)q = B(q)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 q e i q = q qi = qi q = 2 i =1 q q 2 i =1 q q 2 i =1 q q q T T N bi 1 bi U B(q)q + qi q + = u i =1 q 2 q q

Robotics 2

) S(q, q

g(q)

28