robotics 2 - università di romadeluca/rob2_en/05_lagrangiandynamics_3.pdf · robotics 2 18 !...

Robotics 2

Prof. Alessandro De Luca

Dynamic model of robots: Analysis, properties, extensions,

parametrization, identification, uses

Analysis of inertial couplings

!  Cartesian robot

!  Cartesian “skew” robot

!  PR robot

!  2R robot

!  3R articulated robot (under simplifying assumptions on the CoM)

!  dynamic model turns out to be linear if !  g ≡ 0 !  B constant ⇒ c ≡ 0

!

B =b11 00 b22

"

# $

%

& '

!

B(q) =b11 b12(q2)

b21(q2) b22

"

# $

%

& '

!

B =b11 b12

b21 b22

"

# $

%

& '

d = 0 in PR d2 = 0 in 2R

center of mass of link 2

on joint 2 axis

!

B(q) =b11 q2,q3( ) 0 0

0 b22(q3) b23(q3)0 b23(q3) b33

"

#

$ $ $

%

&

' ' '

Robotics 2 2

!

B(q) =b11(q2) b12(q2)b21(q2) b22

"

# $

%

& '

Analysis of gravity term

!  static balancing !  distribution of masses (including motors)

!  mechanical compensation !  articulated system of springs !  closed kinematic chains

!  absence of gravity !  applications in space !  constant U (motion on horizontal plane)

!

g(q) " 0

Robotics 2 3

Bounds on dynamic terms

!  for an open-chain (serial) manipulator, there always exist positive real constants k0 to k7 such that, for any value of q and q

!

k 0 " B(q) " k1 + k2 q + k3 q2

Robotics 2 4

!  if the robot has only revolute joints, these simplify to

.

!

g(q) " k 6 + k7 q

!

S(q, ˙ q ) " ˙ q k 4 + k5 q( )inertia matrix

factorization matrix of Coriolis/centrifugal terms

gravity vector

!

k 0 " B(q) " k1

!

g(q) " k 6

!

S(q, ˙ q ) " k 4˙ q

NOTE: norms are either for vectors or for matrices (induced norms)

Robots with closed kinematic chains - 1

Comau Smart NJ130 MIT Direct Drive Mark II and Mark III

Robotics 2 5

Robots with closed kinematic chains - 2

MIT Direct Drive Mark IV (planar five-bar linkage)

UMinnesota Direct Drive Arm (spatial five-bar linkage)

Robotics 2 6

Robot with parallelogram structure (planar) kinematics and dynamics

q1 q2

q2 - !

1

2

3 4

!

! c1

!

! c2

!

! c3

!

! c4

center of mass:

parallelogram:

!

!1 = ! 3

!

! 2 = ! 4

!

! ciarbitrary

!

pc1 =! c1c1

! c1s1

"

# $

%

& '

!

pc2 =! c2c2

! c2s2

"

# $

%

& '

!

pc3 =! 2c2

! 2s2

"

# $

%

& ' +! c3c1

! c3s1

"

# $

%

& '

!

pc4 =!1c1

!1s1

"

# $

%

& ' (! c4c2

! c4s2

"

# $

%

& '

position of center of masses

5

!

pEE =!1c1

!1s1

"

# $

%

& ' +! 5 cos(q2 ())! 5 sin(q2 ())

"

# $

%

& ' =!1c1

!1s1

"

# $

%

& ' (! 5 c2

! 5 s2

"

# $

%

& '

!

! 5

E-E

x

y

direct kinematics

Robotics 2 7

!

vc1 ="! c1s1

! c1c1

#

$ %

&

' ( ̇ q 1

!

vc3 ="! c3s1

! c3c1

#

$ %

&

' ( ̇ q 1 +

"! 2s2

! 2s2

#

$ %

&

' ( ̇ q 2

!

vc4 ="!1s1

!1c1

#

$ %

&

' ( ̇ q 1 "

"! c4s2

! c4c2

#

$ %

&

' ( ̇ q 2

Kinetic energy

linear/angular velocities

!

vc2 ="! c2s2

! c2c2

#

$ %

&

' ( ̇ q 2

!

"1 ="3 = ˙ q 1

!

"2 ="4 = ˙ q 2

!

T1 =12

m1! c12 ˙ q 1

2 +12

Ic1,zz˙ q 12

!

T2 =12

m2! c22 ˙ q 2

2 +12

Ic2,zz ˙ q 22

!

T3 =12

Ic3,zz ˙ q 12 +

12

m3 ! 22 ˙ q 2

2 + ! c32 ˙ q 1

2 + 2!2! c3c2"1˙ q 1 ˙ q 2( )

!

T4 =12

Ic4,zz ˙ q 22 +

12

m4 !12 ˙ q 1

2 + ! c42 ˙ q 2

2 "2!1! c4c2"1˙ q 1 ˙ q 2( )

Ti

Robotics 2 8

Note: a 2D notation is used here!

Robot inertia matrix

!

T = Ti =12

˙ q TB(q) ˙ q i=1

4

"

!

B(q) =Ic1,zz +m1! c1

2 + Ic3,zz +m3! c32 +m4!1

2 symmm3! 2! c3 "m4!1! c4( )c2"1 Ic2,zz +m2! c2

2 + Ic4,zz +m4! c42 +m3! 2

2

#

$ %

&

' (

!

m3! 2! c3 = m4!1! c4

B(q) diagonal and constant ⇒ centrifugal and Coriolis terms ≡ 0

mechanically DECOUPLED and LINEAR dynamic model (up to the gravity term g(q))

big advantage for the design of a motion control law!

(*) structural condition in mechanical design

Robotics 2 9

Potential energy and gravity terms from the y-components of vectors pci

!

U1 = m1g0! c1s1

!

U2 = m2g0! c2s2

!

U3 = m3g0 ! 2s2 + ! c3s1( )

!

U4 = m4g0 !1s1 " ! c4s2( )

Ui

!

U = Uii=1

4

"

!

g(q) ="U"q

#

$ %

&

' (

T

=g0 m1! c1 + m3! c3 + m4!1( )c1

g0 m2! c2 + m3! 2 )m4! c4( )c2

#

$ %

&

' ( =

g1(q1)g2(q2)#

$ %

&

' (

!

b11˙ ̇ q 1 + g1(q1) = u1

b22˙ ̇ q 2 + g2(q2) = u2

in addition, when (*) holds

gravity components are always “decoupled”

ui (non-conservative) torque

producing work on qi

further structural conditions in the mechanical design lead to g(q) ≡ 0!!

Robotics 2 10

Adding dynamic terms ... !  dissipative phenomena due to friction at the joints/transmissions

!  viscous, dry, Coulomb, ... !  local effects at the joints !  difficult to model in general, except for:

!  inclusion of electrical motors (as additional rigid bodies) !  motor i mounted on link i-1 (or before), typically with its motion

(spinning) axis aligned with joint i axis !  (balanced) mass of motor included in total mass of carrying link !  (rotor) inertia has to be added to robot kinetic energy !  transmissions with reduction gears (often, large reduction ratios) !  in some cases, multiple motors cooperate in moving multiple links:

use a transmission coupling matrix T (with off-diagonal elements)

!

uV,i = "FV,i ˙ q i

!

uS,i = "FS,isgn(˙ q i)

Robotics 2 11

Placement of motors along the chain

RF0 RF1

RFN-1

link 0 (base)

link 1

joint1

link N - 1

link 2 link N joint 2

RFw (world frame)

joint N

RFN

motor 1

motor 2

motor N

!m2 .

!m1 .

!mN .

!mi = nri !i

. .

"i = nri "mi

Robotics 2 12

Resulting dynamic model !  simplifying assumption: in the rotational part of kinetic

energy, only the “spinning” rotor velocity is considered

!  including all added terms, the robot dynamics becomes

!

B(q) + Bm( )˙ ̇ q + c(q, ˙ q ) + g(q) + FV˙ q + FS sgn(˙ q ) = "

constant does NOT contribute to c

Fv > 0, FS > 0 diagonal

motor torques (after

reduction gears)

moved to the left ...

Robotics 2 13

!

Tmi =12

Imi˙ " mi

2 =12

Iminri2 ˙ q i

2 =12

Bmi˙ q i

2

!

Tm = Tmii=1

N

" =12

˙ q T Bm˙ q

diagonal, >0

!  scaling by the reduction gears, looking from the motor side

motor torques (before

reduction gears)

!

Im + diagbii(q)nri

2

" # $

% & '

(

) *

+

, - ̇ ̇ . m + diag

1nri

" # $

% & '

b j(q)˙ ̇ q jj=1,..,n/ + f(q, ˙ q )

(

) * *

+

, - - = 0m

diagonal except the terms bjj

Including joint elasticity

!  in industrial robots, use of motion transmissions based on !  belts !  harmonic drives !  long shafts

introduces flexibility between actuating motors (input) and driven links (output)

!  in research robots for human cooperation, compliance in the transmissions is introduced on purpose for safety !  actuator relocation by means of (compliant) cables and pulleys !  harmonic drives and lightweight (but rigid) link design !  redundant (macro-mini or parallel) actuation, with elastic couplings

!  in both cases, flexibility is modeled as concentrated at the joints !  most of the times, assuming small joint deformation (elastic domain)

Robotics 2 14

Robots with joint elasticity

Dexter with cable transmissions

DLR LWR-III with harmonic drives

Stanford DECMMA with micro-macro actuation

Quanser Flexible Joint (1-dof linear, educational)

motor load/link elastic spring

(stiffness K)

Robotics 2 15

video

Dynamic model of robots with elastic joints

!  introduce 2N generalized coordinates !  q = N link positions !  ! = N motor positions (after reduction, !i = !mi/nri)

!  add motor kinetic energy Tm to that of the links

!  add elastic potential energy Ue to that due to gravity Ug(q)

!  K = matrix of joint stiffness (diagonal, >0)

!  apply Euler-Lagrange equations w.r.t. (q, !)

!

B(q) ˙ ̇ q + c(q, ˙ q ) +g(q) +K(q"#) = 0

Bm˙ ̇ # + K(# "q) = $

2N 2nd-order differential equations

!

Tmi =12

Imi˙ " mi

2 =12

Iminri2 ˙ " i

2 =12

Bmi˙ " i2

!

Tq =12

˙ q T B(q) ˙ q

!

Tm = Tmii=1

N

" =12

˙ # T Bm˙ #

diagonal, >0

!

Uei =12

Ki qi" #mi /nri( )( )2=

12

Ki qi"# i( )2

!

Ue = Ueii=1

N

" =12

q-#( )T K q-#( )

no external torques performing work on q

Robotics 2 16

Dynamic parameters of robots - 1

Robotics 2 17

!  consider a generic link of a fully rigid robot

joint i

link i

CoM Ci

xi

yi

zi

kinematic frame i (DH or modified DH)

Center of Mass (CoM) frame i

base frame 0

mig0

rCi

ICi

!  however, the robot dynamics depends in a nonlinear way on some of these parameters (e.g., through the combination Ici,zz+mirxi

2)

!  each link is characterized by 10 dynamic parameters

mi rCi

!

=rxiryi

rzi

"

#

$ $

%

&

' '

ICi

!

=Ici,xx Ici,xy Ici,xz

Ici,yy Ici,yz

symm Ici,zz

"

#

$ $

%

&

' '

ri

Oi

Dynamic parameters of robots - 2

Robotics 2 18

!  kinetic energy and gravity potential energy can both be rewritten so that a new set of dynamic parameters appears only in a linear way "  need to re-express link inertia and CoM position in (any) known kinematic

frame attached to the link (same orientation as the barycentric frame)

!  kinetic energy of link i

!

Ti = 12 mi vci

Tvci +12" i

TIci " i = 12 mi vi #S(rci)" i( )T

v i #S(rci)" i( ) + 12" i

TIci " i

= 12 mi vi

Tv i +12" i

T Ici +miST(rci)S(rci)( ) " i # vi

TS(mirci)" i

!

vci = vi +" i # rci = vi + S(" i)rci = vi $S(rci)" i

!  fundamental kinematic relation

!

= Ici +mi rci2E - rci

T rci( ) = Ii =Ii,xx Ii,xy Ii,xz

Ii,yy Ii,yz

symm Ii,zz

"

#

$ $

%

&

' '

Steiner theorem 3!3 identity matrix

Dynamic parameters of robots - 3

Robotics 2 19

!  gravitational potential energy of link i

!

Ui = "mig0Tr0,ci = "mig0

T ri +rci( ) = "mig0Tri " g0

T#mirci

!  since the E-L equations involve only linear operations on T and U, also the robot dynamic model is linear in the standard parameters " # R10N$

mass of link i

(0-th order moment)

mass ! CoM position of link i

(1-st order moment)

inertia of link i

(2-nd order moment)

!

" i =mi

miirci

vect iIi{ }

#

$

% % %

&

'

( ( (

= mi miirci,x mi

irci,y miirci,z

iIi,xxiIi,xy

iIi,xziIi,yy

iIi,yziIi,zz( )

T

!

Ti = 12 mi

iv iT iv i +mi

irciT S(iv i)

i" i +12

i" iT iIi

i" i

!  by expressing vectors and matrices in frame i, both Ti and Ui are linear in the set of 10 (constant) standard parameters "i

!

Ui = "mig0Tri"

ig0T #mi

irci

Dynamic parameters of robots - 4

Robotics 2 20

!  using a N!10N regression matrix Y" that depends only on kinematic quantities, the robot dynamic equations can always be rewritten linearly in the standard dynamic parameters as

!

" T = "1T "2

T ! " NT( )

!

B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = Y"(q, ˙ q ,˙ ̇ q ) " = u

!  the open kinematic chain structure of the manipulator implies that the i-th dynamic equation may depend only on the standard dynamic parameters of links i to N ⇒ Y" has a block upper triangular structure

!

Y"(q, ˙ q ,˙ ̇ q ) =

Y1,1 Y1,2 … Y1,N

0 Y2,2 … Y2,N

! " !0 0 YN,N

#

$

% % %

&

'

( ( (

with row vectors Yi,j of size 1!10

A nice property: element bij of B(q) is function of at most (qk+1, ... ,qN), with k=min{i,j}, and of at most the inertial parameters of links r to N, with r=max{i,j}

Dynamic parameters of robots - 5

Robotics 2 21

!  many standard parameters do not appear (“play no role”) in the dynamic model of the given robot ⇒ the associated columns of Y" are 0!

!  some standard parameters may appear only in fixed combinations with others ⇒ the associated columns of Y" are linearly dependent!

!  one can isolate p << 10N independent groups of parameters " (associated to p functionally independent columns Yindep of Y") and partition matrix Y" in two blocks, the second containing dependent (or zero) columns as Ydep = YindepT, for a suitable constant p!(10N-p) matrix T

!  these grouped parameters are called dynamic coefficients a # Rp, “the only that matter” in robot dynamics (= base parameters by W. Khalil)

!  the minimal number p of dynamic coefficients that is needed can also be checked numerically (see later → Identification)

!

Y"(q, ˙ q ,˙ ̇ q ) " = Yindep Ydep( ) " indep

"dep

#

$ %

&

' ( = Yindep YindepT( ) " indep

"dep

#

$ %

&

' (

= Yindep " indep + T"dep( ) = Y(q, ˙ q ,˙ ̇ q ) a

Linear parameterization of robot dynamics

it is always possible to rewrite the dynamic model in the form

!

B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = Y(q, ˙ q ,˙ ̇ q ) a = u

N ! p p ! 1

a = vector of dynamic coefficients

regression matrix

NOTE: 4 more coefficients will be present when including the coefficients Fv,i and Fs,i of viscous and dry friction at the joints (“decoupled” terms, each appearing only in the respective equation i=1,2)

Robotics 2 22

e.g., the heuristic grouping (found by inspection) on a 2R planar robot

!

a1 = Ic1,zz +m1d12 + Ic2,zz +m2d2

2 +m2!12

!

a2 = m2!1d2

!

a3 = Ic2,zz +m2d22

!

a4 = g0 m1d1 + m2!1( )

!

˙ ̇ q 1 c2 2˙ ̇ q 1 + ˙ ̇ q 2( ) " s2˙ q 2

2 + 2˙ q 1˙ q 2( ) ˙ ̇ q 2 c1 c12

0 c2˙ ̇ q 1 + s2

˙ q 12 ˙ ̇ q 1+ ˙ ̇ q 2 0 c12

#

$ %

&

' (

a1

a2

a3

a4

a5

#

$

% % % %

&

'

( ( ( (

=u1

u2

#

$ %

&

' (

!

a5 = g0m2d2

Linear parametrization of a 2R planar robot (N=2)

Robotics 2 23

!  being the kinematics known (i.e., and g0), the number of dynamic coefficients can be reduced since we can merge the two coefficients

!  therefore, after regrouping, p = 4 dynamic coefficients are sufficient

!  this (minimal) linear parametrization of robot dynamics is not unique, both in terms of the chosen set of dynamic coefficients a and for the associated regression matrix Y !  a systematic procedure for its derivation would be preferable

!

a2 = m2!1d2

!

a5 = g0 m2d2

!

!1

⇒

!

a2 = m2d2

!

a1 = Ic1,zz +m1d12 + Ic2,zz +m2d2

2 +m2!12

!

a2 = m2d2

!

a3 = Ic2,zz +m2d22

!

a4 = m1d1 +m2!1

!

˙ ̇ q 1 !1c2 2˙ ̇ q 1 + ˙ ̇ q 2( ) " !1s2˙ q 2

2 + 2˙ q 1˙ q 2( ) + g0c12˙ ̇ q 2 g0c1

0 !1c2˙ ̇ q 1 + !1s2

˙ q 12 + g0c12

˙ ̇ q 1+ ˙ ̇ q 2 0

#

$ %

&

' (

a1

a2

a3

a4

#

$

% % %

&

'

( ( (

= Ya = u =u1

u2

#

$ %

&

' (

(factoring out and g0)

!

!1

Linear parametrization of a 2R planar robot (N=2)

Robotics 2 24

!  as alternative, consider the following general procedure !  10N = 20 standard parameters are defined for the two links !  from the assumptions made on CoM locations, only 5 such parameters actually

appear, namely (with di = rci,x)

!  in the 2!5 matrix Y" , the 3rd column (associated to m2) is !  after regrouping/reordering, again p = 4 dynamic coefficients are sufficient

!

m1d1 I1,zz = Ic1,zz +m1d12 (link 1) m2 m2d2 I2,zz = Ic2,zz +m2d2

2 (link 2)

!  determining a minimal parameterization (i.e., minimizing p) is important for !  experimental identification of dynamic coefficients !  adaptive/robust control design in the presence of uncertain parameters

!

Y"3 = Y"1!1 + Y"2!12

!

a2 = I1,zz +m2!12 = Ic1,zz +m1d1

2( ) +m2!12

!

a3 = m2d2

!

a4 = I2,zz = Ic2,zz +m2d22

!

a1 = m1d1 +m2!1

!

g0c1˙ ̇ q 1 !1c2 2˙ ̇ q 1 + ˙ ̇ q 2( ) " !1s2

˙ q 22 + 2˙ q 1˙ q 2( ) + g0c12

˙ ̇ q 1+ ˙ ̇ q 20 0 !1c2

˙ ̇ q 1 + !1s2˙ q 1

2 + g0c12˙ ̇ q 1+ ˙ ̇ q 2

#

$ %

&

' (

a1

a2

a3

a4

#

$

% % %

&

'

( ( (

= Ya = u =u1

u2

#

$ %

&

' (

Identification of dynamic coefficients

!  in order to “use” the model, one needs to know the numeric values of the robot dynamic coefficients

!  robot manufacturers provide at most only a few principal dynamic parameters (e.g., link masses)

!  estimates can be found with CAD tools (e.g., assuming uniform mass) !  friction coefficients are (slowly) varying over time

!  lubrication of joints/transmissions !  for an added payload (attached to the E-E)

!  a change in the 10 dynamic parameters of last link !  this implies a variation of (almost) all robot dynamic coefficients

!  preliminary identification experiments are needed !  robot in motion (dynamic issues, not just static or geometric ones!) !  only the robot dynamic coefficients can be identified (not all link

standard parameters!)

Robotics 2 25

Identification experiments 1.  choose a motion trajectory that is sufficiently “exciting”, i.e.,

"  explores the robot workspace and involves all components in the robot dynamic model

"  is periodic, with multiple frequency components 2.  execute this motion (approximately) by means of a control law

"  taking advantage of any available information on the robot model "  usually, u = KP(qd-q)+KD(qd-q) (PD, no model information used)

3.  measure q (encoder) and, if available, q in nc time instants "  joint velocity and acceleration can be later estimated off line by

numerical differentiation (use of non-causal filters is feasible) 4.  with such measures/estimates, evaluate the regression matrix Y (on

the left) and use the applied commands (on the right) in the expression

!

Y(q(tk), ˙ q (tk),˙ ̇ q (tk)) a = u(tk) k = 1,…,nc

.

. .

Robotics 2 26

Least Squares (LS) identification

!  set up the system of linear equations

!  sufficiently “exciting” trajectories, large enough number of samples (nc ! N ≫ p), and their suitable selection/position, guarantee rank(Y) = p (full column rank)

!  solution by pseudoinversion of matrix Y

!  one can also use a weighted pseudoinverse, to take into account different levels of noise in the collected measures

!

Y(q(t1), ˙ q (t1),˙ ̇ q (t1))!

Y(q(tnc), ˙ q (tnc

),˙ ̇ q (tnc))

"

#

$ $ $

%

&

' ' '

a =

u(t1)

u(tnc)

"

#

$ $ $

%

&

' ' '

!

Y a = u

_ _

!

a = Y # u = Y TY ( )-1Y T u

nc ! N

Robotics 2 27

Additional remarks on LS identification !  it is convenient to preserve the block (upper) triangular structure of

the regression matrix, by “stacking” all time evaluations sequenced by the rows of the original Y matrix

!

Y1(q(t1), ˙ q (t1),˙ ̇ q (t1))!

Y1(q(tnc), ˙ q (tnc

),˙ ̇ q (tnc))

Y2(q(t1), ˙ q (t1),˙ ̇ q (t1))!

YN(q(t1), ˙ q (t1),˙ ̇ q (t1))!

YN(q(tnc), ˙ q (tnc

),˙ ̇ q (tnc))

"

#

$ $ $ $ $ $ $ $

%

&

' ' ' ' ' ' ' '

a =

u1(t1)!

u1(tnc)

u2(t1)!

uN(t1)!

uN(tnc)

"

#

$ $ $ $ $ $ $ $

%

&

' ' ' ' ' ' ' '

!

Y a = u N!

Robotics 2 28

!  further practical hints !  outlier data can be eliminated in advance (when building Y) !  if complex friction models are not included in Y a, discard the data

collected at joint velocities that are close to zero

_

nc

nc

"  numerical check of full column rank is more robust ⇔ rank = p (# of col’s)

!

Y =

nc

nc

Summary on dynamic identification

Robotics 2 29

J. Swevers, W. Verdonck, and J. De Schutter: “Dynamic model identification for industrial robots”

IEEE Control Systems Mag., Oct 2007

KUKA IR 361 robot and optimal

excitation trajectory

results after identification (first three joints only)

Dynamic identification of KUKA LWR4

Robotics 2 30

C. Gaz, F. Flacco, A. De Luca: “Identifying the dynamic model used by the KUKA LWR:

A reverse engineering approach” IEEE ICRA 2014, Hong Kong, June 2014

validation after identification (for all 7 joints): on new desired trajectories, compare

torques computed with the identified model and torques measured by joint torque sensors

data acquisition for identification dynamic coefficients: 30 inertial, 12 for gravity

video

Dynamic identification of KUKA LWR4

Robotics 2 31

J. Hollerbach, W. Khalil, M. Gautier: “Ch. 6: Model Identification”, Springer Handbook of Robotics (2nd Ed), 2016 free access to multimedia extension: http://handbookofrobotics.org

using more dynamic robot motions for model identification

video

Adding a payload to the robot

Robotics 2 32

!  in several industrial applications, changes in the robot payload are often needed !  using different tools for various technological operations

such as polishing, welding, grinding, ... !  pick-and-place tasks of objects having unknown mass

!  what is the rule of change for dynamic parameters when there is an additional payload? !  do we obtain again a linearly parameterized problem? !  does this property rely on some specific choice of

reference frames (e.g., conventional or modified D-H)?

Rule of change in dynamic parameters

Robotics 2 33

!  only the dynamic parameters of the link where a load is added will change (typically, added to the last one –link n– as payload)

!  last link dynamic parameters: mn (mass), cn = (cnx cny cnz)T (center of

mass), In (inertia tensor w.r.t. frame n)

!  payload dynamic parameters: mL (mass), cL = (cLx cLy cLz)T (center of

mass), IL (inertia tensor w.r.t. frame n)

!  mass

!  center of mass

!  inertia tensor

(weighted average) where i = x, y, z

valid only if tensors are expressed w.r.t. the same reference frame (i.e., frame n)!

"  linear parametrization is preserved with any kinematic convention (the parameters of the last link will always appear in the form shown above)

Example: 2R planar robot with payload

Robotics 2 34

gravity acceleration

robot dynamics robot dynamics

g0

Note1: the positions of the center of mass of the two links and payload may also be asymmetric Note2: link inertias & centers of mass are expressed in the DH kinematic frame attached to the link;

thus, e.g., I2zz + m2a22 is the inertia of the second link w.r.t. z1 (parallel axis theorem)

Validation on the KUKA LWR4 robot

Robotics 2 35

video

C. Gaz, A. De Luca: “Payload estimation based on identified coefficients of robot dynamics – with an application to collision detection” IEEE IROS 2017, Vancouver, September 2017

see block of slides!

Use of the dynamic model Inverse dynamics

!  given a desired trajectory qd(t) of motion !  twice differentiable (∃ qd(t)) !  possibly obtained from a task/Cartesian trajectory rd(t), by

(differential) kinematic inversion the input torque needed to execute it is

..

!  useful also for control (e.g., nominal feedforward) !  however, this algebraic computation (∀t) is not efficient when

performed using the above Lagrangian approach !  symbolic terms grow much longer, quite rapidly for larger n !  in real time, better a numerical computation based on

Newton-Euler method

!

"d = B(qd) + Bm( )˙ ̇ q d + c(qd, ˙ q d) + g(qd) + FV˙ q d + FS sgn(˙ q d)

Robotics 2 36

Dynamic scaling of trajectories uniform time scaling of motion

Robotics 2 37

!  given a smooth original trajectory qd(t) of motion for t ∈ [0,T] !  suppose to rescale time as t → r(t) (a strictly increasing function of t) !  in the new time scale, the scaled trajectory qs(r) satisfies

!  uniform scaling of the trajectory occurs when r(t) = k t

Q: what is the new input torque needed to execute the scaled trajectory? (suppose dissipative terms can be neglected)

!

qd(t) = qs(r(t))

!

˙ q d(t) =dqd

dt=

dqs

drdrdt

= qs' (r) ˙ r

!

˙ ̇ q d(t) =d˙ q ddt

=dqs

'

drdrdt

"

# $

%

& ' ̇ r +qs

' (r)˙ ̇ r = qs'' (r) ˙ r 2 + qs

' (r)˙ ̇ r

same path executed (at different instants of time)

!

˙ q d(t) = k qs' (kt)

!

˙ ̇ q d(t) =k2qs'' (kt)

Dynamic scaling of trajectories inverse dynamics under uniform time scaling

Robotics 2 38

!  the new torque could be recomputed through the inverse dynamics, for every r = k t ∈[0,T’]=[0,kT] along the scaled trajectory, as

!  however, being the dynamic model linear in the acceleration and quadratic in the velocity, it is

!  thus, saving separately the total torque %d(t) and gravity torque gd(t) in the inverse dynamics computation along the original trajectory, the new input torque is obtained directly as

!

"d(t) = B(qd)˙ ̇ q d + c(qd, ˙ q d) + g(qd) = B(qs)k2qs

'' + c(qs,kqs' ) + g(qs)

= k2 B(qs)qs'' + c(qs,qs

' )( ) + g(qs) = k2 "s(kt) #g(qs)( ) + g(qs)

!

"s(kt) = B(qs)qs'' + c(qs,qs

' ) + g(qs)

!

"s(kt) =1k2 "d(t) #g(qd(t))( ) + g(qd(t))

k>1: slow down ⇒ reduce torque

k<1: speed up ⇒ increase torque

gravity term (only position-dependent): does NOT scale!

Dynamic scaling of trajectories numerical example

Robotics 2 39

!  rest-to-rest motion with cubic polynomials for planar 2R robot under gravity (from downward equilibrium to horizontal link 1 & upward vertical link 2)

!  original trajectory lasts T=0.5 s (but maybe violates the torque limit at joint 1) only joint 1 torque is shown

total torque

at equilibrium = zero gravity

torque

gravity torque component

torque only due to non-zero initial

acceleration

for both joints

Dynamic scaling of trajectories numerical example

Robotics 2 40

!  scaling with k=2 (slower) → T’ = 1 s original total

torque

scaled total

torque

gravity torque component

does not scale

gravity torque to sustain the link

at steady state

T=0.5 s

T’=1 s

*

*

!

"d(0.1) #g(qd(0.1)) = 20 Nm

!

"s(2# 0.1) $g(qs(2# 0.1)) =2022 = 5 Nm

T’=1 s T=0.5 s

!

k = 2

* *

0 Nm

!

14

!

˜ x =q

B(q)˙ q "

# $

%

& '

State equations Direct dynamics

!

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

state equations

N differential 2nd-order equations

defining the vector of state variables as

!

x =x1

x2

"

# $

%

& ' =

q˙ q "

# $ %

& ' ( IR2N

!

˙ x =˙ x 1˙ x 2

"

# $

%

& ' =

x2

(B(1(x1) c(x1,x2) + g(x1)[ ]"

# $

%

& ' +

0B(1(x1)"

# $

%

& ' u

= f(x) + G(x)u

2N!1 2N!N

another choice... x = ... (do it as exercise) ~ .

Lagrangian dynamic model

generalized momentum

2N differential 1st-order equations

Robotics 2 41

Dynamic simulation

u

!

˙ q 1(0)

!

q1(0)

q1

!

˙ ̇ q 1

!

˙ q 1

g(q)

!

˙ q 2 (0)

!

q2 (0)

q2

!

˙ ̇ q 2

!

˙ q 2

!

q, ˙ q

!

q

!

c(q, ˙ q )

B-1(q)

!

"

!

"

!

"

!

"

here, a generic 2-dof robot

!

q

+

_

_

"  initialization (dynamic coefficients and initial state) "  calls to (user-defined) Matlab functions for the evaluation of model terms "  choice of a numerical integration method (and of its parameters)

input torque command (open-loop

or in feedback)

including “inv(B)”

Simulink block

scheme

Robotics 2 42

… an incorrect realization

u1

c1+ g1

!

1b11

!

"

!

˙ q 1(0)

!

q1(0)

q1

!

˙ ̇ q 1

!

˙ q 1

u2

c2 + g2

!

1b22

!

˙ q 2 (0)

!

q2 (0)

q2

!

˙ ̇ q 2

!

˙ q 2

b12

b12 algebraic loop!

causality principle

is violated!!

_

_

_

_

!

"

!

"

!

"

inversion of the robot inertia matrix in toto is needed (and not only of its elements on the diagonal...)

Robotics 2 43

Approximate linearization

!

B(qd)˙ ̇ q d + c(qd, ˙ q d) + g(qd) = ud

!  we can derive a linear dynamic model of the robot, which is valid locally around a given operative condition !  useful for analysis, design, and gain tuning of linear (or, the linear

part of) control laws !  approximation by Taylor series expansion, up to the first order !  linearization around a (constant) equilibrium state or along a

(nominal, time-varying) equilibrium trajectory !  usually, we work with (nonlinear) state equations; for mechanical

systems, it is more convenient to directly use the 2nd-order model !  same result, but easier derivation

!

g(qe) = ueequilibrium state (q,q) = (qe,0) [ q=0 ] . ..

equilibrium trajectory (q,q) = (qd (t),qd(t)) [ q=qd(t) ] . .

Robotics 2 44

.. ..

Linearization at an equilibrium state

!  variations around an equilibrium state

!  keeping into account the quadratic dependence of c terms on velocity (thus, neglected around the zero velocity)

!  in state-space format

!

q = qe + "q ˙ q = ˙ q e + "˙ q = "˙ q ˙ ̇ q = ˙ ̇ q e + "˙ ̇ q = "˙ ̇ q u = ue + "u

!

B qe( )"˙ ̇ q + g(qe) +# g# q

q=qe

"q + o "q , "˙ q ( ) = ue + "u

!

G(qe)infinitesimal terms

of second or higher order

!

"˙ x =0 I

-B-1 qe( )G(qe) 0#

$ %

&

' ( "x +

0B-1 qe( )#

$ %

&

' ( "u = A "x + B "u

!

"x ="q"˙ q #

$ %

&

' (

Robotics 2 45

Linearization along a trajectory

!  variations around an equilibrium trajectory

!  developing terms in the dynamic model ...

!

q = qd + "q ˙ q = ˙ q d + "˙ q ˙ ̇ q = ˙ ̇ q d + "˙ ̇ q u = ud + "u

!

B qd + "q( )(˙ ̇ q d + "˙ ̇ q ) + c(qd + "q, ˙ q d + "˙ q ) + g(qd + "q) = ud + "u

!

B(qd + "q) #B(qd) +$ bi

$ qi=1

N

%q=qd

eiT"q

!

c(qd + "q, ˙ q d + "˙ q ) # c(qd, ˙ q d) +$ c$ q q=qd˙ q = ˙ q d

"q +$ c$ ˙ q q=qd˙ q = ˙ q d

"˙ q

!

g(qd + "q) # g(qd) + G(qd)"q

!

C1(qd, ˙ q d)

!

C2(qd, ˙ q d)

i-th row of the identity matrix

Robotics 2 46

Linearization along a trajectory (cont)

!  after simplifications …

with

!  in state-space format

!

B qd( )"˙ ̇ q + C2(qd, ˙ q d)"˙ q + D(qd, ˙ q d,˙ ̇ q d)"q = "u

!

"˙ x =0 I

-B-1 qd( )D(qd, ˙ q d,˙ ̇ q d) -B-1 qd( )C2(qd, ˙ q d)#

$ %

&

' ( "x +

0B-1 qd( )#

$ %

&

' ( "u

= A t( )"x + B t( )"u

!

D(qd, ˙ q d,˙ ̇ q d) = G(qd) + C1(qd, ˙ q d) +" bi

" qi=1

N

#q=qd

˙ ̇ q d eiT

a linear, but time-varying system!!

Robotics 2 47

Coordinate transformation

!

B(q)˙ ̇ q + c(q, ˙ q ) + g(q) = B(q)˙ ̇ q + n(q, ˙ q ) = uq

!

q" IRN

if we wish/need to use a new set of generalized coordinates p

!

p" IRN

!

p = f(q)

!

q = f "1(p)

!

˙ ̇ p = J(q)˙ ̇ q + ˙ J (q) ˙ q

!

˙ ̇ q = J"1(q) ˙ ̇ p " ˙ J (q)J"1(q)˙ p [ ]

1

1

!

B(q)J"1(q)˙ ̇ p "B(q)J"1(q)˙ J (q)J"1(q) ˙ p + n(q, ˙ q ) = JT (q)up

!

˙ p = "f(q)"q

˙ q = J(q) ˙ q

!

˙ q = J"1(q) ˙ p

!

uq = JT (q)up,

!

J"T (q) # pre-multiplying the whole equation... Robotics 2 48

Robot dynamic model after coordinate transformation

!

J"T (q)B(q)J"1(q)˙ ̇ p + J"T (q) n(q, ˙ q ) "B(q)J"1(q)˙ J (q)J"1(q)˙ p ( ) = up

!

Bp(p)˙ ̇ p + cp(p, ˙ p ) + gp(p) = up

!

Bp = J"TBJ"1

!

cp = J"T c "BJ"1˙ J J"1 ˙ p ( ) = J"Tc "Bp˙ J J"1 ˙ p

!

gp = J"Tg

!

cp p, ˙ p ( ) = Sp p, ˙ p ( )˙ p

!

˙ B p "2Sp skew - symmetric

!

q"p

!

q, ˙ q "p, ˙ p for actual computation, these inner substitutions

are not necessary

symmetric, positive definite

(out of singularities)

quadratic dependence on p

.

when p = E-E pose, this is the robot dynamic model in Cartesian coordinates

non-conservative generalized forces

performing work on p

Robotics 2 49

Q: What if the robot is redundant with respect to the Cartesian task?

Optimal point-to-point robot motion considering the dynamic model

Robotics 2 50

"  given the initial and final robot configurations (at rest) and actuator torque bounds, find !  the minimum-time Tmin motion !  the (global/integral) minimum-energy Emin motion

and the associated command torques needed to execute them "  a complex nonlinear optimization problem solved numerically

Tmin= 1.32 s, E = 306 T = 1.60 s, Emin = 6.14

video video