Robotics 2

Prof. Alessandro De Luca

Dynamic model of robots:Analysis, properties, extensions, uses

Analysis of inertial couplings

n Cartesian robot

n Cartesian “skew” robot

n PR robot

n 2R robot

n 3R articulated robot

(under simplifyingassumptions on the CoMs)

Robotics 2 2

! =#$$ 0

0 #&&

! =#$$ #$&

#$& #&&

! =#$$((&) #$&((&)

#$&((&) #&&

! =

#$$((&, (+) 0 0

0 #&&((+) #&+((+)

0 #&+((+) #++

! =#$$ #$&((&)

#$&((&) #&&

Analysis of gravity term

n absence of gravityn constant ,- (motion on horizontal plane)

n applications in remote space

n static balancingn distribution of masses (including motors)

n mechanical compensationn articulated system of springs

n closed kinematic chains

Robotics 2 3

. ( ≈ 0

Bounds on dynamic terms

n for an open-chain (serial) manipulator, there always exist positive real constants 00 to 07 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)

03 ≤ ! ( ≤ 0$ + 0& ( + 0+ ( &

6 (, ( ≤ 07 + 08 ( (

. ( ≤ 09 + 0: (

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

(the same holds true with bounds (;,<;= ≤ (; ≤ (;,<>? on prismatic joints)

03 ≤ ! ( ≤ 0$ 6 (, ( ≤ 07 ( . ( ≤ 09

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

(& − A

1

2

34 center of mass:

parallelogram:

arbitrary BC;

5 E-E

D

E

Robotics 2 7

BC+

B8

BC$ B$ = B+

B& = B7

direct kinematics

FGG =B$H$B$I$

+B8 cos (& − A

B8 sin (& − A=

B$H$B$I$

−B8H&B8I&

position of center of masses

FC$ =BC$H$BC$I$

FC& =BC&H&BC&I&

FC+ =B&H&B&I&

+BC+H$BC+I$

FC7 =B$H$B$I$

−BC7H&BC7I&

BC7

(&($

BC&

Kinetic energy

linear/angular velocities

Robotics 2 8

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

OC$ =−BC$I$BC$H$

($

OC& =−BC&I&BC&H&

(&

OC+ =−BC+I$BC+H$

($ +−B&I&B&H&

(&

OC7 =−B$I$B$H$

($ +BC7I&−BC7H&

(&

P$ = P+ = ($

P& = P7 = (&

Q; Q$ =$

&#$BC$

& ($& +

$

&RC$,SS($

& Q& =$

&#&BC&

& (&& +

$

&RC&,SS(&

&

Q+ =$

&#+ B&

&(&& + BC+

& ($& + 2B&BC+H&U$($(& +

$

&RC+,SS($

&

Q7 =$

&#7 B$

&($& + BC7

& (&& − 2B$BC7H&U$($(& +

$

&RC7,SS(&

&

Robot inertia matrix

Robotics 2 9

Q = V

;W$

7

Q; =1

2(Y! ( (

! ( =RC$,SS + #$BC$

& + RC+,SS + #+BC+& + #7B$

& symm

#+B&BC+ − #7B$BC7 H&U$ RC&,SS + #&BC&& + RC7,SS + #7BC7

& + #+B&&

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

(*)structural condition

in mechanical design#+B&BC+ = #7B$BC7

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

big advantage for the design of a motion control law!

!$$ 0

0 !&&

($(&

=^$^&

Potential energy and gravity terms

from the E-components of vectors FC;,;

gravitycomponentsare always

“decoupled”

Robotics 2 10

, =V

;W$

7

,;

,$ = #$.3BC$I$ ,& = #&.3BC&I&

,+ = #+.3 B&I& + BC+I$ ,7 = #7.3 B$I$ − BC7I&

. ( =_,

_(

Y

=.3 #$BC$ + #+BC+ + #7B$ H$.3 #&BC& + #+B& − #7BC7 H&

=.$ ($.& (&

in addition,when (*) holds

^; are(non-conservative) torques

performing work on (;

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

#$$($ + .$ ($ = ^$#&&(& + .& (& = ^&

Adding dynamic terms ...

1) dissipative phenomena due to friction at the joints/transmissions

n viscous, Coulomb, stiction, Stribeck, LuGre (dynamic)...

n local effects at the joints

n difficult to model in general, except for:

Robotics 2 11

^a,; = −ba,; (; ^c,; = −bc,; sgn (;

ef ghi(j)

ekj

eglmnlmoi

Adding dynamic terms ...

2) inclusion of electrical actuators (as additional rigid bodies)

n motor p mounted on link p − 1 (or before)

n often with its spinning axis aligned with joint axis p

n (balanced) mass of motor included in total mass of carrying link

n (rotor) inertia has to be added to robot kinetic energy

n transmissions with reduction gears (often, large reduction ratios)

n in some cases, multiple motors cooperate in moving multiple links: use a transmission coupling matrix q (with off-diagonal elements)

Robotics 2 12

Unimation PUMA family

Mitsubishi RV-3S

rsptu 2

#susv 2

#susv 3rsptu 3

, with very few exceptions

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 13

x<$

x<&

x<y

x<; = tz;x;

{; = tz;{<;

Resulting dynamic model

n simplifying assumption: in the rotational part of the kinetic energy, only the “spinning” rotor velocity is considered

Robotics 2 14

diagonal, > 0

Q<; =1

2R<;x<;

& =1

2R<;tz;

& (;& =

1

2}<;(;

& Q< = V

;W$

y

Q<; =1

2(Y}<(

n including all added terms, the robot dynamics becomes

constant does NOTcontribute to H

b~ > 0, bc > 0diagonal

motor torques(after

reduction gears)

moved tothe left ...

! ( + }< ( + H (, ( + . ( + ba( + bc sgn ( = {

R< + diag#;;(()

tz;&

x< + diag1

tz;V

ÅW$

y

Ç!Å(()(Å + É (, ( = {<

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

motor torques(before

reduction gears)

diagonal

except the terms #ÅÅ

Including joint elasticity

n in industrial robots, use of motion transmissions based onn belts

n harmonic drives

n long shafts

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

n in research robots, compliance in transmissions is introduced on purpose for safety (human collaboration) and/or energy efficiencyn actuator relocation by means of (compliant) cables and pulleys

n harmonic drives and lightweight (but rigid) link design

n redundant (macro-mini or parallel) actuation, with elastic couplings

n in both cases, flexibility is modeled as concentrated at the joints

n in most cases, assuming small joint deformation (elastic domain)

Robotics 2 15

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 16

video

Dynamic modelof robots with elastic joints

n introduce 2Ñ generalized coordinatesn ( = Ñ link positionsn x = Ñ motor positions (after reduction, x; = x<;/tz;)

n add motor kinetic energy Q# to that of the links

n add elastic potential energy ,Ü to that due to gravity ,-(()n á = matrix of joint stiffness (diagonal, > 0)

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

diagonal, > 0

no external torquesperforming work on (

Robotics 2 17

Q<; =1

2R<;x<;

& =1

2R<;tz;

& x;& =

1

2}<;x;

&

Qà =1

2(Y!(()(

Q< = V

;W$

y

Q<; =1

2xY}<x

,â; =1

2á; (; −

x<;

tz;

&

=1

2á; (; − x;

& ,â = V

;W$

y

,â; =1

2( − x Yá ( − x

2Ñ 2nd-orderdifferentialequations

! ( ( + H (, ( + . ( + á ( − x = 0

}<x + á x − ( = {

Use of the dynamic modelinverse dynamics

n given a desired trajectory (ä(u)

n twice differentiable (∃ (ä(u))

n possibly obtained from a task/Cartesian trajectory vä(u), 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 (∀u) is not efficient when using the Lagrangian modeling 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 18

{ä = ! (ä + }< (ä + H (ä, (ä + . (ä + ba(ä + bc sgn (ä

…− éâ?èY (ä bâ?è,ä(in contact, with an external wrench)

State equationsdirect dynamics

Ñ differential2nd orderequations

another choice... êD = ... (do it as exercise)

Lagrangiandynamic model

generalizedmomentum

Robotics 2 19

! ( ( + H (, ( + . ( = ^

defining the vector of state variables as D =D$D&

=(

( ∈ ℝ&y

êD =(

!(()(

state equations

2Ñ × 1 2Ñ × Ñ

2Ñ differential1st order

equations

D =D$D&

=D&

−!U$(D$) H D$, D& + .(D$)+

0!U$(D$)

^

= É(D) + î(D)^

Dynamic simulation

^

.(()

!U$(()

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 20

H((, ()

( (

(, (

($

(&

($

(&

(&(0)

($(0)($(0)

(&(0)

($

(&

e.g., 4th-order Runge-Kutta (ode45)

Approximate linearization

n we can derive a linear dynamic model of the robot, which is valid locally around a given operative condition

n useful for analysis, design, and gain tuning of linear (or, the linear part of) control laws

n approximation by Taylor series expansion, up to the first order

n linearization around a (constant) equilibrium state or along a (nominal, time-varying) equilibrium trajectory

n usually, we work with (nonlinear) state equations; for mechanical systems, it is more convenient to directly use the 2nd order model

n same result, but easier derivation

Robotics 2 21

equilibrium trajectory ((, () = ((ä(u), (ä(u)) [ ( = (ä(u) ]

! (ä (ä + H (ä, (ä + . (ä = ^ä

equilibrium state ((, () = ((â, 0) [ ( = 0 ] . (â = ^â

Linearization at an equilibrium state

n 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 ∆D =∆(

∆(

Robotics 2 22

( = (â + Δ( ( = (â + Δ( = Δ( ( = (â + ∆( = ∆( ^ = ^â + Δ^

! (â ∆( + . (â + ô_.

_(àWàö

∆( + o ∆( , ∆( = ^â + ∆^

infinitesimal termsof second or higher order

∆D =0 R

−!U$ (â î((â) 0∆D +

0!U$((â)

∆^ = õ ∆D + } ∆^

î((â)

Linearization along a trajectory

n variations around an equilibrium trajectory

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

p-th row of theidentity matrix

Robotics 2 23

( = (ä + Δ( ( = (ä + Δ( ( = (ä + ∆( ^ = ^ä + Δ^

!((ä + ∆() (ä + ∆( + H((ä + ∆(, (ä + ∆() + . (ä + ∆( = ^ä + ∆^

! (ä + ∆( ≅ ! (ä +V

;W$

y

ô_!;_(

àWàù

Ü;Y∆(

. (ä + ∆( ≅ . (ä + î((ä)∆(

H (ä + ∆(, (ä + ∆( ≅ H (ä, (ä + ô_H

_( àWàùàWàù

∆( + ô_H

_( àWàùàWàù

∆(

û&((ä, (ä)

û$((ä, (ä)

Linearization along a trajectory (cont)

n after simplifications …

with

n in state-space format

a linear, but time-varying system!!

Robotics 2 24

!((ä) ∆( + û&((ä, (ä) ∆( + ü (ä, (ä, (ä ∆( = ∆^

ü (ä, (ä, (ä = î (ä + û$ (ä, (ä +V

;W$

y

ô_!;

_(àWàù

(äÜ;Y

∆D =0 R

−!U$ (ä ü (ä, (ä, (ä −!U$ (ä û& (ä, (ä∆D

+0

!U$((ä)∆^ = õ(u) ∆D + }(u) ∆^

Coordinate transformation

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

1

1

Robotics 2 25

( ∈ ℝy ! ( ( + H (, ( + . ( = ! ( ( + t (, ( = ^à

F ∈ ℝy F = É(() ( = ÉU$(F)

F =_É

_(( = é(()( ( = éU$(()F

F = é ( ( + é(()( ( = éU$(() F − é(()éU$(()F

! ( éU$ ( F − !(()éU$(() é(()éU$(()F + t (, ( = éY(()^†

pre-multiplying the whole equation...éUY(() °

by duality(principle of virtual work)

^à = éY(()^†

Robot dynamic modelafter coordinate transformation

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

non-conservativegeneralized forces

performing work on F

Robotics 2 26

éUY ( ! ( éU$ ( F + éUY ( t (, ( −!(()éU$(() é(()éU$(()F = ^†

for actual computation,these inner substitutions

are not necessary( → F ((, () → (F, F)

!† F F + H† F, F + .† F = ^†

symmetric,positive definite (out of singularities)

quadraticdependence on F

!† = éUY!éU$ .† = éUY.

H† = éUY H −!éU$ é éU$F = éUYH −!†é éU$F

H†(F, F) = 6†(F, F) F !† − 26† skew-symmetric

NOTE: in this case, we have implicitly assumed than ! = Ñ (no redundancy!)

Robot dynamic modelin the task/Cartesian space, with redundancy

Robotics 2 27

( ∈ ℝy

v = É ( ∈ ℝ£

! < Ñ

∈ ℛ éY ∈ ℛ éY ¶ éYßéY= éY

ß is any generalized inverse of éY

é is full rank = !

! ( ( + t (, ( = {

dynamic model in the joint space

v = é ( ( + é(()(

second-order task kinematics

{ = éY ( b + R − éY ( ß(() {3

2) decompose the joint torques in two complementary spaces

( = !U$ ( { − t (, (1) isolate the joint acceleration from the dynamics

torques coming fromgeneralized forces in the task space …

v = é ( !U$ ( éY ( b + R − éY ( ß(() {3 − t (, ( + é(()(

3) substitute 1) and 2) in the differential task kinematics

{3 = éY ( b, ∀b ∈ ℝ£ ⇒ R − éY ( ß ( éY ( b = 0

… and joint torques {3 that do not!

4) isolate on the right-hand side the generalized forces b in the task space …

Robot dynamic modelin the task/Cartesian space, with redundancy

Robotics 2 28

5) choose as generalized inverse ß = é!U$éY U$é!U$ = é£# Y

, i.e., the transpose

of the inertia-weighted pseudoinverse of the task Jacobian (see block of slides #2)

é ( !U$ ( éY (U$v = b +

é ( !U$ ( éY (U$

é ( !U$ ( R − éY ( ß(() {3 − t (, ( + é(()(

é ( !U$ ( éY (U$v = b + é ( !U$ ( éY (

U$é ( ( − é ( !U$ ( t (, (

!z ( v + tz (, ( = b

in this way, the joint torque component {3 will NOT affect the task acceleration v

6) the resulting (! −dimensional) task dynamics is then

…+ bâ?èexternal forces can be added on the rhs of the equations in a dynamically consistent way!

!z ( = é ( !U$ ( éY (U$

tz (, ( = !z ( é ( !U$ ( t (, ( − é(()(

with

task inertia matrix

7) an additional Ñ − ! -dimensional dynamics is needed to describe the full robot!

for ! = Ñ, these termsare identical to slide #26

Dynamic scaling of trajectoriesuniform time scaling of motion

Robotics 2 29

n given a smooth original trajectory (ä(u) of motion for u ∈ [0, Q]

n suppose to rescale time as u → v(u) (a strictly increasing function of u)

n in the new time scale, the scaled trajectory (©(v) satisfies

n uniform scaling of the trajectory occurs when v u = 0u

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)

(ä u = (© v(u) (ä u =™(ä

™u=™(©

™v

™v

™u= (©

´(v) v(u)

(ä u =™(ä

™u=

™(©´

™v

™v

™uv + (©

´™v

™u= (©

´´(v)v&(u) + (©´(v)v(u)

(ä u = 0(©´(0u) (ä u = 0&(©

´´(0u)

n the new torque could be recomputed through the inverse dynamics, for every v = 0u ∈ 0, Q´ = [0, 0Q] 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 {ä(u) and gravity torque .ä(u)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 30

0 > 1: slow down⇒ reduce torque

0 < 1: speed up⇒ increase torque

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

{© 0u = ! (© (©´´ + H (©, (©

´ + .((©)

{ä u = ! (ä (ä + H (ä, (ä + . (ä = ! (© 0&(©

´´ + H (©, 0(©´ + .((©)

= 0& ! (© (©´´ + H (©, (©

´ + . (© = 0& {© 0u − . (© + . (©

{© 0u =1

0&{ä u − . (ä(u) + . (ä(u)

Dynamic scaling of trajectoriesnumerical example

Robotics 2 31

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 Q = 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 32

n scaling with 0 = 2 (slower) → Q´ = 1 soriginaltotal

torque

scaledtotal

torque

gravity torquecomponent

does not scale

gravity torqueto sustain the link

at steady state

Q = 0.5 s

Q´ = 1 s

Q = 1 sQ = 0.5 s

* *

0 Nm

0 = 2

1

4

*

{ä 0.1 − . (ä 0.1 = 20 Nm

*

{© 2 ° 0.1 − . (© 2 ° 0.1 =&3

&Ø= 5 Nm

Optimal point-to-point robot motionconsidering the dynamic model

Robotics 2 33

§ given the initial and final robot configurations (at rest) and actuator torque bounds, find

n the minimum-time Tmin motion

n 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