Robotics 2

Prof. Alessandro De Luca

Dynamic model of robots:Analysis, properties, extensions,

parametrization, identification, uses

Analysis of inertial couplingsn Cartesian robot

n Cartesian “skew” robot

n PR robot

n 2R robot

n 3R articulated robot(under simplifyingassumptions on the CoM)

n dynamic model turns out to be linear ifn ! ≡ 0n $ constant ⇒ & ≡ 0

'&2 = 0 in PR'2 = 0 in 2R

center of massof link 2

on joint 2 axisRobotics 2 2

$ = *++ 00 *,,

$ = *++ *+,*+, *,,

$ = *++ *+,(.,)*+,(.,) *,,

$ = *++(.,) *+,(.,)*+,(.,) *,,

$ =*++(.,, .1) 0 0

0 *,,(.1) *,1(.1)0 *,1(.1) *11

Analysis of gravity termn static balancing

n distribution of masses (including motors)n mechanical compensation

n articulated system of springsn closed kinematic chains

n absence of gravityn applications in spacen constant !" (motion on horizontal plane)

Robotics 2 3

# $ ≈ 0

Bounds on dynamic termsn for an open-chain (serial) manipulator, there always exist

positive real constants !0 to !7 such that, for any value of $ and $

Robotics 2 4

inertia matrix

factorization matrix ofCoriolis/centrifugal terms

gravity vector

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

!& ≤ ( $ ≤ !) + !+ $ + !, $ +

- $, $ ≤ !/ + !0 $ $

1 $ ≤ !2 + !3 $n if the robot has only revolute joints, these simplify to

!& ≤ ( $ ≤ !) - $, $ ≤ !/ $ 1 $ ≤ !2

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

!"

!" − $

1

2

34 center of mass:

parallelogram:

arbitrary %&'5 E-E

(

)

Robotics 2 7

%&*

%+

%&,

!,

%, = %*%" = %.

direct kinematics%&"

/00 =%,1,%,2, + %+ cos !" − $

%+ sin !" − $ = %,1,%,2, − %+1"

%+2"position of center of masses

/&, =%&,1,%&,2, /&" =

%&"1"%&"2" /&* =

%"1"%"2" + %&*1,

%&*2, /&. =%,1,%,2, − %&.1"

%&.2"

%&.

Kinetic energylinear/angular velocities

Robotics 2 8

Note: a (planar) 2D notation is used here!

!"# =−&"#'#&"#(# *#

!"+ =−&"+'+&"+(+ *+

!", =−&",'#&",(# *# +

−&+'+&+(+ *+

!". =−&#'#&#(# *# +

&".'+−&".(+ *+

/# = /, = *#

/+ = /. = *+

01 0# = #+2#&"#+ *#+ + #

+ 3"#,55*#+ 0+ = #

+2+&"++ *++ + #+ 3"+,55*+

+

0, = #+2, &++*++ + &",+ *#+ + 2&+&",(+7#*#*+ + #

+ 3",,55*#+

0. = #+2. &#+*#+ + &".+ *++ − 2&#&".(+7#*#*+ + #

+ 3".,55*++

Robot inertia matrix

mechanically DECOUPLED and LINEARdynamic model (up to the gravity term !(#))

big advantage for the design of a motion control law!

Robotics 2 9

% = '()*

+

%( =12 #

/0 # #

0 # = 12*,44 + 6*72*8 + 129,44 + 697298 + 6+7*8 symm6978729 − 6+7*72+ >8?* 128,44 + 687288 + 12+,44 + 6+72+8 + 69788

0(#) diagonal and constant ⇒ centrifugal and Coriolis terms ≡ 0

(*)structural conditionin mechanical design

6978729 = 6+7*72+

Potential energy and gravity termsfrom the !-components of vectors "#$

%$

gravitycomponentsare always

“decoupled”

Robotics 2 10

% ='$()

*

%$

%) = +),-.#)/) %0 = +0,-.#0/0%1 = +1,- .0/0 + .#1/) %* = +*,- .)/) − .#*/0

, 4 = 5%54

6= ,- +).#) + +1.#1 + +*.) 7)

,- +0.#0 + +1.0 − +*.#* 70= ,) 4)

,0 40

in addition,when (*) holds

8$ are(non-conservative) torques

performing work on 4$further structural conditions in the mechanical design lead to ,(4) ≡ 0!!

+))4) + ,) 4) = 8)+0040 + ,0 40 = 80

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

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

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

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

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

Robotics 2 11

%&,( = −*&,( ,( %-,( = −*-,( sgn ,(

Placement of motors along the chain

RF0 RF1

RFN-1

link 0(base)

link 1

joint1

link N - 1

link 2 link Njoint 2

RFW(world frame)

joint N

RFN

motor 1

motor 2

motor N

rotor 1frame

rotor N frame

Robotics 2 12

"#$

"#%

"#&

"#' = )*'"'+' = )*'+#'

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

energy, only the “spinning” rotor velocity is considered

Robotics 2 13

diagonal, > 0

#$% =1

2)$%+$%

, =1

2)$%-.%

, /%, =

1

20$%/%

, #$ = 1%23

4

#$% =1

2/50$/

n including all added terms, the robot dynamics becomes

constant does NOTcontribute to 6

78 > 0, 7: > 0diagonal

motor torques(after

reduction gears)

moved tothe left ...

; / + 0$ / + 6 /, / + > / + 7?/ + 7: sgn / = C

n scaling by the reduction gears, looking from the motor sidemotor torques

(before reduction gears)

diagonal except the terms DEE

)$ + diagD%%(/)

-.%, +$ + diag

1

-.%1E23

4

K;E(/)/E + L /, / = C$

Including joint elasticityn in industrial robots, use of motion transmissions based on

n beltsn harmonic drivesn long shafts

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

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

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

Robotics 2 14

Robots with joint elasticity

Dexter with cable transmissions

DLR LWR-IIIwith harmonic drives

Stanford DECMMAwith micro-macro actuation

Quanser Flexible Joint(1-dof linear, educational)

motor load/linkelasticspring

(stiffness K)

Robotics 2 15

video

Dynamic modelof robots with elastic joints

n introduce 2" generalized coordinatesn # = " link positionsn % = " motor positions (after reduction, %& = %'&/)*&)

n add motor kinetic energy +, to that of the links

n add elastic potential energy -. to that due to gravity -/(#)n 2 = matrix of joint stiffness (diagonal, > 0)

n apply Euler-Lagrange equations w.r.t. (#, %)

diagonal, > 0

no external torquesperforming work on #

Robotics 2 16

+'& =1

27'&%'&

9 =1

27'&)*&

9 %&9 =

1

2:'&%&

9

+; =12#<=(#)#

+' = >&?@

A

+'& =1

2%<:'%

-B& =1

22& #& −

%'&)*&

9

=1

22& #& − %&

9 -B = >&?@

A

-B& =1

2# − % <2 # − %

2" 2nd-orderdifferentialequations

= # # + F #, # + G # + 2 # − % = 0

:'% + 2 % − # = H

Dynamic parameters of robots - 1

Robotics 2 17

n consider a generic linkof a fully rigid robot

joint !

link !

CoM "!

#!

$!

%!

kinematic frame !(DH or modified DH)

Center of Mass(CoM) frame !

base frame 0

'()*

+,(

-,(

+(

.!

n each link is characterized by10 dynamic parameters

'( +,( =+1(+2(+3(

-,( =-,(,11 -,(,12 -,(,13

-,(,22 -,(,23symm -,(,33

n however, the robot dynamics depends in a nonlinear way on some of these parameters (e.g., through the combination -8(,33 + '(+1(: )

Dynamic parameters of robots - 2

Robotics 2 18

n 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)n fundamental kinematic relation

!"# = !# + &# × ()# = !# + * &# ()# = !# − * ()# &#n kinetic energy of link i

,# =12/#!)#0 !)# +

12&#

01)#&#

= 12/#!#0!# + &#0 1)# +/#*0 ()# * ()# &# − !#0* /#()# &#

= 23/# !# − * ()# &# 0 !# − * ()# &# + 2

3&#01)#&#

Steiner theorem 1# =1#,55 1#,56 1#,57

1#,66 1#,67symm 1#,77

Dynamic parameters of robots - 3

Robotics 2 19

n gravitational potential energy of link !

n since the E-L equations involve only linear operations on " and #, also the robot dynamic model is linear in the standard parameters $ ∈ ℝ'()

#* = −-*.(/0(,2* = −-*.(/ 0* + 02* = −-*.(/0* − .(/ -*02*n by expressing vectors and matrices in frame !, both "* and #* are

linear in the set of 10 (constant) standard parameters $* ∈ ℝ'(

"* =12-* !7*/!7* + -* !02*/ 8 !7* !9* + !9*/ !:* !9*

#* = −-* .(/0* − .(/ 0;* (-* !02*)mass of link !

(0-th order moment )

mass�CoMposition of link !

(1-st order moment )

inertia of link !(2-nd order moment )

$* =-*

-*!02*7>?@ !:*

= -* -*!02*,A -*!02*,B -*!02*,C !:*,AA !:*,AB !:*,AC !:*,BB !:*,BC !:*,CC /

Dynamic parameters of robots - 4

Robotics 2 20

n using a ! × 10! regression matrix %& that depends only on kinematic quantities, the robot dynamic equations can always be rewritten linearly in the standard dynamic parameters as

n the open kinematic chain structure of the manipulator implies that the '-thdynamic equation can depend only on the standard dynamic parameters of links ' to ! ⇒ %& has a block upper triangular structure

%& ), ), ) =

%.. %./0 %//

⋯ %.1⋯ %/1

⋮0 ⋯

⋱ ⋮0 %11

with row vectors%4,5 of size 1×10

Property: element 645 of 7()) is function of at most ():;.,⋯ , )1), for < = min{', A} , and of the inertial parameters of at most links C to !, with C = max{', A}

7 ) ) + G ), ) + H ) = %& ), ), ) I = J

IK = I.K I/

K ⋯ I1K

Dynamic parameters of robots - 5

Robotics 2 21

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

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

n one can isolate $ ≪ 10( independent groups of parameters ) (associated to $functionally independent columns "*+,-. of "#) and partition matrix "# in two blocks, the second containing dependent (or zero) columns as ",-. = "*+,-.0, for a suitable constant $ × (10( − $) matrix 0

n these grouped parameters are called dynamic coefficients 5 ∈ ℝ., “the only that matter” in robot dynamics (= base parameters by W. Khalil)

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

"# 8, 8, 8 ) = "*+,-. ",-.)*+,-.),-. = "*+,-. "*+,-.0

)*+,-.),-.

= "*+,-. )*+,-. + 0 ),-. = " 8, 8, 8 5

Linear parameterization of robot dynamicsit is always possible to rewrite the dynamic model in the form

! × # # × 1

% = vector ofdynamic coefficients

regressionmatrix

NOTE: 4 more coefficients are added when including the coefficients &',) and &*,) of viscous anddry friction at joints (“decoupled” terms appearing only in the respective equations + = 1,2)

Robotics 2 22

. / / + 2 /, / + 4 / = 5 /, /, / % = 6

e.g., the heuristic grouping (found by inspection) on a 2R planar robot%7 = 897,:: + ;7<7= + 89=,:: + ;=<== + ;=>7=

%? = 89=,:: + ;=<==%= = ;=>7<=

%@ = 4A;=<=%B = 4A ;7<7 + ;=>7

/7 2= 2/7 + /= − D= /== + 2/7/= /= 27 27=0 2=/7 + D=/7= /7 + /= 0 27=

%7%=%?%B%@

= 676=

Linear parametrizationof a 2R planar robot (! = 2)

Robotics 2 23

n being the kinematics known (i.e., $% and &'), the number of dynamic coefficients can be reduced since we can merge the two coefficients

n therefore, after regrouping, ( = ) dynamic coefficients are sufficient

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

⇒ (factoring out $% and &') *- = .-$%/- *0 = &'.-/-& *- = .-/-

2% $%3- 22% + 2- − $%6- 2-- + 22%2- + &'3%-0 $% 3-2% + 6-2%- + &'3%-

2- &'3%2% + 2- 0

*%*-*9*:

= + * = ; = ;%;-

*% = <=%,?? + .%/%- + <=-,?? + .-/-- + .-$%- *9 = <=-,?? + .-/--

*- = .-/- *: = .%/% + .-$%

!"#$ &$0 0

($#) 2&$ + &) − ($-) &)) + 2&$&) + !"#$) &$ + &)($ #)&$ + -)&$) + !"#$) &$ + &)

/$/)/0/1

= 3 / = 4 = 4$4)

/) = 5$,77 + 8) ($) = 59$,77 + 8$:$) + 8$($) /1 = 5),77 = 59),77 + 8):))/0 = 8):)/$ = 8$:$ + 8) ($

Linear parametrizationof a 2R planar robot (; = 2)

Robotics 2 24

n as alternative to the previous heuristic method, apply the general proceduren 10; = 20 standard parameters are defined for the two linksn from the assumptions made on CoM locations, only 5 such parameters actually

appear, namely (with :> = ?9>,@)

n in the 2×5 matrix 3B, the 3rd column (associated to 8)) is 3B0 = 3B$($ + 3B)($)n after regrouping/reordering, C = D dynamic coefficients are again sufficient

n determining a minimal parameterization (i.e., minimizing E) is important forn experimental identification of dynamic coefficientsn adaptive/robust control design in the presence of uncertain parameters

8$:$ 5$,77 = 59$,77 + 8$:$) 8):) 5),77 = 59),77 + 8):))8)link 1: link 2:

Identification of dynamic coefficients

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

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

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

n lubrication of joints/transmissionsn for an added payload (attached to the E-E)

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

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

the link standard parameters!)

Robotics 2 25

Identification experiments1. 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 components2. execute this motion (approximately) by means of a control law

§ taking advantage of any available information on the robot model § often ! = #$(&' − &) + #+(&' − &) (PD, no model information used)

3. measure & (encoder) and, if available, also & in -. 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 / (on the

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

0 = 1,⋯ , -.Robotics 2 26

/ & 45 , & 45 , & 45 7 = ! 45

Least Squares (LS) identification n set up the system of linear equations

n sufficiently “exciting” trajectories, large enough number of samples (!" × $ ≫ &), and their suitable selection/position, guarantee rank('() = & (full column rank)

n solution by pseudoinversion of matrix '(

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

!"× $

Robotics 2 27

) = '(#', = '(- '( ./ '(- ', (∈ ℝ3)

'() = ',( 5 6/ , 5 6/ , 5 6/

⋮( 5 6;< , 5 6;< , 5 6;<

) =, 6/⋮

, 6;<

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

the regression matrix, by “stacking” all time evaluations in row by row sequence of the original ! matrix

"×

Robotics 2 28

n further practical hintsn outlier data can be eliminated in advance (when building !)n when complex friction models are not included in !$, discard the

data collected at joint velocities that are close to zero

%&

%& %&

%&

'! =

!) * +) , * +) , * +)⋮

!) * +01 , * +01 , * +01!2 * +) , * +) , * +)

⋮!2 * +01 , * +01 , * +01

⋮!3 * +) , * +) , * +)

⋮!3 * +01 , * +01 , * +01

$ =

4) +)⋮

4) +0142 +)⋮

42 +01⋮

43 +)⋮

43 +01

'!$ = '4

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

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 andoptimal

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 modeland torques measured by joint torque sensors

data acquisition for identificationdynamic 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), 2016free 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

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

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

n what is the rule of change for dynamic parameters when there is an additional payload? n do we obtain again a linearly parameterized problem?n 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

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

n last link dynamic parameters: mn (mass), cn = (cnxcnycnz)T (center of mass), In (inertia tensor expressed w.r.t. frame n)

n payload dynamic parameters: mL (mass), cL = (cLxcLycLz)T (center of mass), IL (inertia tensor expressed w.r.t. frame n)

n mass

n center of mass

n 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

g0 = gravity acceleration

robot dynamics robot dynamics

Note 1: position of the center of mass of the two links and of the payload may also be asymmetricNote 2: link inertia & center of mass are expressed in the DH kinematic frame attached to the link

(e.g., I2zz is the inertia of the second link around the axis z2)

y0

q1

a1

a2

q2

y1

y2

x0

x1

x2

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 the block of slides!

Use of the dynamic modelInverse dynamics

n given a desired trajectory !"($)n twice differentiable (∃ !"($))n possibly obtained from a task/Cartesian trajectory ("($), by

(differential) kinematic inversion the input torque needed to execute this motion (in free space) is

n useful also for control (e.g., nominal feedforward)n however, this way of performing the algebraic computation (∀$) is

not efficient when using the above Lagrangian approach

n symbolic terms grow much longer, quite rapidly for larger *n in real time, numerical computation is based on Newton-Euler method

Robotics 2 36

+" = - !" + /0 !" + 1 !", !" + 4 !" + 56!" + 57 sgn !"

Dynamic scaling of trajectoriesuniform time scaling of motion

Robotics 2 37

n given a smooth original trajectory !"($) of motion for $ ∈ [0, *]n suppose to rescale time as $ → -($) (a strictly increasing function of $)n in the new time scale, the scaled trajectory !.(-) satisfies

n uniform scaling of the trajectory occurs when - $ = 0 $

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

same path executed(at different instants of time)

!" $ = !. -($) !" $ = 2!"2$ = 2!.

2-2-2$ = !.3 -

!" $ = 2!"2$ = 2!.3

2-2-2$ - + !.3 - = !.33-6 + !.3 -

!" $ = 0!.3(0$) !" $ = 06!.33(0$)

n the new torque could be recomputed through the inverse dynamics, for every ! = #$ ∈ 0, () = [0, #(] along the scaled trajectory, as

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

n thus, saving separately the total torque ,-($) and gravity torque 0-($)in the inverse dynamics computation along the original trajectory, the new input torque is obtained directly as

Dynamic scaling of trajectoriesinverse dynamics under uniform time scaling

Robotics 2 38

# > 1: slow down⇒ reduce torque

# < 1: speed up⇒ increase torque

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

,5 #$ = 6 75 75)) + 9 75, 75) + 0(75)

,- $ = 6 7- 7- + 9 7-, 7- + 0 7- = 6 75 #<75)) + 9 75, #75) + 0(75)= #< 6 75 75)) + 9 75, 75) + 0 75 = #< ,5 #$ − 0 75 + 0 75

,5 #$ = 1#< ,- $ − 0 7-($) + 0 7-($)

Dynamic scaling of trajectoriesnumerical example

Robotics 2 39

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

n original trajectory lasts ! = 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 torquecomponent

torque only due to non-zero initial

acceleration

for both joints

Dynamic scaling of trajectoriesnumerical example

Robotics 2 40

n scaling with ! = 2 (slower) → $% = 1 soriginaltotal

torque

scaledtotal

torque

gravity torquecomponent

does not scale

gravity torqueto sustain the link

at steady state

$ = 0.5 s

$% = 1 s

$ = 1 s$ = 0.5 s

* *0 Nm

! = 2

14

*

+, 0.1 − . /, 0.1 = 20 Nm

*

+0 2 1 0.1 − . /0 2 1 0.1 = 2324 = 5 Nm

State equationsDirect dynamics

! differential2nd orderequations

another choice... #$ = ... (do it as exercise)

Lagrangiandynamic model

generalizedmomentum

Robotics 2 41

& ' ' + * ', ' + , ' = -

defining the vector of state variables as $ = $.$/ = '

' ∈ ℝ/2

#$ = '&(')'

state equations

2! × 1 2! × !

2! differential1st order

equations

$ = $.$/ = $/

−&9.($.) * $., $/ + ,($.) + 0&9.($.) -

= ;($) + <($)-

Dynamic simulation

!

"($)

&'(($)

here, a generic 2-dof robot

+_

_

§ 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 torquecommand(open-loop

or in feedback)

including “inv(M)”

Simulinkblock

scheme

Robotics 2 42

)($, $)

$ $

$, $$(

$,

$(

$,

$,(0)

$((0)$((0)

$,(0)

$(

$,

… an incorrect realization

!"

#" + %"

!&

#& + %&

'&"

'"&algebraic loop!

causalityprinciple

is violated!!

_

__

_

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

Robotics 2 43

+

+

1'""

1'&&

*"

*&

*"

*&

*"

*&*&(0)

*"(0)*"(0)

*&(0)

Approximate linearizationn we can derive a linear dynamic model of the robot, which is valid

locally around a given operative conditionn useful for analysis, design, and gain tuning of linear (or, the linear

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

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

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

equilibrium state (", ") = ("', 0) [ " = 0 ]equilibrium trajectory (", ") = (",(-), ",(-)) [ " = ",(-) ]

Robotics 2 44

. ", ", + 0 ",, ", + 1 ", = 2,

1 "' = 2'

Linearization at an equilibrium staten variations around an equilibrium state

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

n in state-space format, with ∆" = ∆$∆$

Robotics 2 45

$ = $& + Δ$ $ = $& + Δ$ = Δ$ $ = $& + ∆$ = ∆$ * = *& + Δ*

+ $& ∆$ + , $& + -.,.$ /0/1

∆$ + o ∆$ , ∆$ = *& + ∆*infinitesimal terms

of second or higher order4($&)

∆" = 0 8−+:; $& 4($&) 0 ∆" + 0

+:;($&) ∆* = < ∆" + = ∆*

Linearization along a trajectoryn variations around an equilibrium trajectory

n developing to 1st order the terms in the dynamic model ...

!-th row of theidentity matrix

Robotics 2 46

" = "$ + Δ" " = "$ + Δ" " = "$ + ∆" * = *$ + Δ*

+("$ + ∆") "$ + ∆" + .("$ + ∆", "$ + ∆") + 0 "$ + ∆" = *$ + ∆*

+ "$ + ∆" ≅ + "$ +2345

678+3

8" 949:;3<∆"

0 "$ + ∆" ≅ 0 "$ + =("$)∆"

. "$ + ∆", "$ + ∆" ≅ . "$, "$ + 78.8" 949:

949:

∆" + 78.8" 949:

949:

∆"

>?("$, "$)

>5("$, "$)

Linearization along a trajectory (cont)

n after simplifications …

with

n in state-space format

a linear, but time-varying system!!

Robotics 2 47

!(#$) ∆# + )*(#$, #$) ∆# + - #$, #$, #$ ∆# = ∆/

- #$, #$, #$ = 0 #$ + )1 #$, #$ +2341

567!3

7# 8489#$:3;

∆< = 0 >−!@1 #$ - #$, #$, #$ −!@1 #$ )* #$, #$ ∆<

+ 0!@1(#$) ∆/ = A(B) ∆< + C(B) ∆/

Coordinate transformation

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

1

1

Robotics 2 48

" ∈ ℝ% & " " + ) ", " + , " = & " " + . ", " = /0

! ∈ ℝ% ! = 1(") " = 145(!)

! = 616" " = 7(")" " = 745(")! /0 = 78(")/9

! = 7 " " + 7(")" " = 745(") ! − 7(")745(")!

& " 745 " ! − &(")745(") 7(")745(")! + . ", " = 78(")/9

pre-multiplying the whole equation...748(") ;

Robot dynamic modelafter coordinate transformation

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

non-conservativegeneralized forces

performing work on !

Robotics 2 49

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

"#$ % & % "#' % ! + "#$ % * %, % −&(%)"#'(%) "(%)"#'(%)! = 12for actual computation,these inner substitutions

are not necessary% → ! (%, %) → (!, !)

&2 ! ! + 42 !, ! + 52 % = 12symmetric,positive definite (out of singularities)

quadraticdependence on !

&2 = "#$&"#' 52 = "#$5

42 = "#$ 4 −&"#' " "#'! = "#$4 −&2 " "#'!42(!, !) = 62(!, !) ! &2 − 262 skew-symmetric

Optimal point-to-point robot motionconsidering the dynamic model

Robotics 2 50

§ given the initial and final robot configurations (at rest) and actuator torque bounds, findn the minimum-time Tmin motionn 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