Dynamic model of robots:

Transcription

1 Robotics 2 Dynamic model of robots: Analysis, properties, extensions, parametrization, identification, uses Prof. Alessandro De Luca

2 Analysis of inertial couplings! Cartesian robot! Cartesian skew robot! PR robot! 2R robot! 3R articulated robot (under simplifying assumptions on the CoM) " B = b 11 0 # 0 b 22 & " B = b 11 b 12 # b 21 b 22 & " b B(q) = 11 b 12 (q 2 ) # b 21 (q 2 ) b 22 & " B(q) = b 11(q 2 ) b 12 (q 2 ) # b 21 (q 2 ) b 22 & ( ) 0 0 " b 11 q 2,q 3 B(q) = 0 b 22 (q 3 ) b 23 (q 3 ) # 0 b 23 (q 3 ) b 33 &! dynamic model turns out to be linear if! g 0! B constant c 0 d = 0 in PR d 2 = 0 in 2R center of mass of link 2 on joint 2 axis Robotics 2 2

3 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

4 Bounds on dynamic terms! for an open-chain (serial) manipulator, there always exist positive. real constants k 0 to k 7 such that, for any value of q and q k 0 " B(q) " k 1 + k 2 q + k 3 q 2 S(q, q ) " g(q) " k 6 + k 7 q ( k 4 + k 5 q ) q inertia matrix factorization matrix of Coriolis/centrifugal terms gravity vector! if the robot has only revolute joints, these simplify to k 0 " B(q) " k g(q) " k 1 S(q, q ) " k 4 q 6 NOTE: norms are either for vectors or for matrices (induced norms) Robotics 2 4

5 Robots with closed kinematic chains - 1 Comau Smart NJ130 MIT Direct Drive Mark II and Mark III Robotics 2 5

6 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

7 2 3! c2! c1 Robot with parallelogram structure (planar) kinematics and dynamics!! c3 y c4 q 1 4 q 2 1 x 5! 5 q 2 -! E-E direct kinematics " p EE =! c 1 1 #! 1 s 1 & + "! cos(q ()) 5 2 #! 5 sin(q 2 ()) & = "! c 1 1 #! 1 s 1 & ( "! c 5 2 #! 5 s 2 & position of center of masses center of mass: arbitrary! ci parallelogram:! 1 =! 3! 2 =! 4 " p c1 =! c c1 1 " #! c1 s p c2 =! c c2 2 " 1 & #! c2 s p c3 =! c & #! 2 s 2 & + "! c c3 1 " #! c3 s p c4 =! c & #! 1 s 1 & ( "! c c4 2 #! c4 s 2 & Robotics 2 7

8 # v c1 = "! s c1 1 &! c1 c ( # # q 1 v c3 = "! s c3 1 & 1! c3 c ( # q 1 + "! s 2 2 & v 1! 2 s ( c2 = "! s c2 2 &! c2 c ( q # v c4 = "! s 1 1 &! 1 c ( # q 1 " "! s c4 2 & 1! c4 c ( 2 Kinetic energy linear/angular velocities q 2 " 1 = " 3 = q 1 " 2 = " 4 = q 2 q 2 Note: a 2D notation is used here! T i T 1 = 1 2 m! 2 1 c1q I 2 c1,zz q 1 T 2 = 1 2 m! 2 2 c2q I 2 c2,zzq 2 ( q 2 ) T 3 = 1 2 I c3,zz q m! 2 3 2q 2 2 +! 2 c3 q ! 2! c3 c 2"1 q 1 ( q 2 ) T 4 = 1 2 I c4,zzq m 4! 12 q 2 1 +! 2 c4 q 2 2 " 2! 1! c4 c 2"1 q 1 Robotics 2 8

9 Robot inertia matrix T = 4 " i=1 T i = 1 2 q T B(q) q # B(q) = I +m! c1,zz 1 c1 + I c3,zz +m 3! c3 +m 4! 1 2 ( m 3! 2! c3 "m 4! 1! c4 )c 2"1 I c2,zz +m 2! c2 symm 2 + I c4,zz +m 4! c4 & 2 ( +m 3! 2 structural condition in mechanical design m 3! 2! c3 = m 4! 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! Robotics 2 9

10 Potential energy and gravity terms from the y-components of vectors p ci U i U 1 = m 1 g 0! c1 s 1 U 2 = m 2 g 0! c2 s 2 U 3 = m 3 g 0 (! 2 s 2 +! c3 s 1 ) U = m g! ( s "! s 1 1 c4 2) # g(q) = "U & ( " q T in addition, when (*) holds U = 4 " i=1 U i # = g m! 0( + m! + m! 1 c1 3 c3 4 1)c 1 & # ( = g (q ) & 1 1 ( g 0 ( m 2! c2 + m 3! 2 )m 4! c4 )c 2 g 2 (q 2 ) b 11 q 1 + g 1 (q 1 ) = u 1 b 22 q 2 + g 2 (q 2 ) = u 2 gravity components are always decoupled u i (non-conservative) torque producing work on q i further structural conditions in the mechanical design lead to g(q) 0!! Robotics 2 10

11 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: u V,i = "F V,i q i u S,i = "F S,i sgn( q i )! 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) Robotics 2 11

12 Placement of motors along the chain.! m1.! mn motor 1 motor N RF w (world frame) joint 2 link 1 link 2 RF 0 RF 1 RF N-1 link N RF N link 0 (base) joint1 motor 2 link N - 1 joint N..! mi = n ri! i " i = n ri " mi Robotics 2 12.! m2

13 diagonal Resulting dynamic model! simplifying assumption: in the rotational part of kinetic energy, only the spinning rotor velocity is considered T mi = 1 2 I mi! including all added terms, the robot dynamics becomes ( B(q) + B m ) q + c(q, q ) + g(q) + F V q + F S sgn( constant 2 = 1 2 I n 2 mi riq 2 i = 1 2 B mi " mi does NOT contribute to c F v > 0, F S > 0 diagonal q ) = " moved to the left... motor torques (after reduction gears) Robotics 2 13 q i 2 N T m = " T mi = 1 q 2 T B m q i=1 diagonal, >0! scaling by the reduction gears, looking from the motor side ( " I m + diag b ii(q) + * # & 2 - ". ) n m + diag 1 # & ri, n ri ( * ) + / b j (q) q j + f(q, q ) - = 0 m, j=1,..,n except the terms b jj motor torques (before reduction gears)

14 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

15 Robots with joint elasticity DLR LWR-III Dexter with harmonic drives with cable transmissions motor elastic load/link spring (stiffness K) Quanser Flexible Joint (1-dof linear, educational) Robotics 2 video Stanford DECMMA with micro-macro actuation 15

16 Dynamic model of robots with elastic joints! introduce 2N generalized coordinates! q = N link positions!! = N motor positions (after reduction,! i =! mi /n ri )! add motor kinetic energy T m to that of the links T mi = 1 2 I mi" 2 mi = 1 2 I n 2 mi ri" 2 i = 1 2 B mi! add elastic potential energy U e to that due to gravity U g (q)! K = matrix of joint stiffness (diagonal, >0) U ei = 1 2 K i( q i " (# mi /n ri )) 2 = 1 2 K i( q i "# i ) 2 N U e = " U ei = 1 2 q- #! apply Euler-Lagrange equations w.r.t. (q,!) 2N 2 nd -order differential equations " i 2 T m = " T mi = 1 # T B m # 2 B(q) q + c(q, q ) +g(q) +K(q"#) = 0 B m # + K(# " q) = N i=1 i=1 T q = 1 2 q T B(q) q diagonal, >0 ( ) T K ( q- #) no external torques performing work on q Robotics 2 16

17 Dynamic parameters of robots - 1! consider a generic link of a fully rigid robot Center of Mass (CoM) frame i link i I Ci z i joint i CoM C i r Ci y i kinematic frame i (DH or modified DH) m i g 0 O i base frame 0! each link is characterized by 10 dynamic parameters r i m i r Ci =! however, the robot dynamics depends in a nonlinear way on some of these parameters (e.g., through the combination I ci,zz +m i r xi2 ) Robotics 2 17 " # r xi r yi r zi & x i I Ci = " I ci,xx I ci,xy I ci,xz I ci,yy I ci,yz # symm I ci,zz &

18 Dynamic parameters of robots - 2! 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)! fundamental kinematic relation! kinetic energy of link i v ci = v i + " i # r ci = v i + S(" i )r ci = v i S(r ci )" i T i = 1 2 m i v T ci v ci " T i I ci " i = 1 2 m i v i # S(r ci )" i = 1 m v T 2 i i v i + 1 " T 2 i I ci +m i S T (r ci )S(r ci ) Steiner theorem ( ) T ( v i # S(r ci )" i ) " T i I ci " i ( ) " i # v i T S(m i r ci )" i = I ci +m ( i r ci 2E - r T ci r ) = I ci i = 3!3 identity matrix " I i,xx I i,xy I i,xz I i,yy I i,yz # symm I i,zz & Robotics 2 18

19 Dynamic parameters of robots - 3! gravitational potential energy of link i U i = " m i g 0 T r 0,ci = " m i g 0 T r i +r ci ( ) = " m i g 0 T r i " g 0 T # m i r ci! by expressing vectors and matrices in frame i, both T i and U i are linear in the set of 10 (constant) standard parameters " i T i = 1 2 m i i v i T i v i +m i i r ci T S( i v i ) i " i i " i T i I i i " i U i = "m i g 0 T r i " i g 0 T # m i i r ci # m i " i = m i i r ci vect { i I i } 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) = ( m i m i i r ci,x m i i r ci,y m i i r i ci,z I i i,xx I i i,xy I i i,xz I i i,yy I i i,yz I i,zz ) T! since the E-L equations involve only linear operations on T and U, also the robot dynamic model is linear in the standard parameters " # R 10N Robotics 2 19

20 Dynamic parameters of robots - 4! 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 B(q) q + c(q, q ) + g(q) = Y " (q, q, q ) " = u " T = " 1 T ( T T " 2! " ) N! 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 ) = # Y 1,1 Y 1,2 Y 1,N 0 Y 2,2 Y 2,N! "! 0 0 Y N,N & ( ( ( with row vectors Y i,j of size 1!10 A nice property: element b ij of B(q) is function of at most (q k+1,...,q N ), with k=min{i,j}, and of at most the inertial parameters of links r to N, with r=max{i,j} Robotics 2 20

21 Dynamic parameters of robots - 5! 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 Y indep of Y " ) and partition matrix Y " in two blocks, the second containing dependent (or zero) columns as Y dep = Y indep T, for a suitable constant p!(10n-p) matrix T Y " (q, q, q ) " = Y indep # ( Y ) " & indep dep " ( = Y indep Y indep T dep = Y indep " indep + T" dep q, q ) a ( ) = Y(q, ( ) " indep! these grouped parameters are called dynamic coefficients a # R p, 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) Robotics 2 21 # " dep & (

22 Linear parameterization of robot dynamics it is always possible to rewrite the dynamic model in the form regression matrix a = vector of dynamic coefficients B(q) q + c(q, q ) + g(q) = Y(q, q, q ) a = u N! p p! 1 e.g., the heuristic grouping (found by inspection) on a 2R planar robot ( ) " s 2 ( q 2 ) # q 1 c 2 2 q 1 + q 2 q q c 2 q 1 + s 2 q 1 # q 2 c 1 c & 12 ( q 1 + q 2 0 c 12 a 1 = I c1,zz +m 1 d I c2,zz +m 2 d 2 2 +m 2! 1 2 a 2 = m 2! 1 d a = I +m d c2,zz 2 2 ( ) a 4 = g 0 m 1 d 1 + m 2! 1 NOTE: 4 more coefficients will be present when including the coefficients F v,i and F s,i of viscous and dry friction at the joints ( decoupled terms, each appearing only in the respective equation i=1,2) Robotics 2 22 a 1 a 2 a 3 a 4 a 5 & ( ( # ( = u & 1 ( u 2 ( a 5 = g 0 m 2 d 2

23 Linear parametrization of a 2R planar robot (N=2)! being the kinematics known (i.e., and g 0 ), the number of dynamic coefficients can be reduced since we can merge the two coefficients a 2 = m 2! 1 d 2 a 5 = g 0 m 2 d 2! therefore, after regrouping, p = 4 dynamic coefficients are sufficient # a 1 & # q 1! 1 c 2 ( 2 q 1 + q 2 ) "! 1 s 2 ( q q 1 q 2 ) + g 0 c 12 q 2 g 0 c & 1 a ( ( 2 # 0! 1 c 2 q 1 +! 1 s 2 q g 0 c 12 q 1 + q 2 0 a ( = Y a = u = u & 1 ( 3 u ( 2! 1 a 2 = m 2 d 2 a 1 = I c1,zz +m 1 d I c2,zz +m 2 d 2 2 +m 2! 1 2 a 2 = m 2 d 2 (factoring out! 1 and g 0 ) a 4 a 3 = I c2,zz +m 2 d 2 2 a 4 = m 1 d 1 +m 2! 1! 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 Robotics 2 23

24 Linear parametrization of a 2R planar robot (N=2)! 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 d i = r ci,x ) m 1 d 1 I 1,zz = I c1,zz +m 1 d 1 2 (link 1) m 2 m 2 d 2 I 2,zz = I c2,zz +m 2 d 2 2! in the 2!5 matrix Y ", the 3 rd column (associated to m 2 ) is (link 2)! after regrouping/reordering, again p = 4 dynamic coefficients are sufficient # g 0 c 1 q 1! 1 c 2 2 q 1 + q 2 q q 1 0 0! 1 c 2 q 1 +! 1 s 2 q g 0 c 12 a 1 = m 1 d 1 +m 2! 1 ( ) "! 1 s 2 ( q 2 ) + g 0 c 12 a 2 = I 1,zz +m 2! = ( I c1,zz +m 1 d 1 ) +m 2! 1! determining a minimal parameterization (i.e., minimizing p) is important for! experimental identification of dynamic coefficients q 1 + q 1 + Y "3 = Y "1! 1 + Y "2! 1 2 a 3 = m 2 d a = I = I +m d ,zz c2,zz 2 2! adaptive/robust control design in the presence of uncertain parameters Robotics 2 24 q 2 q 2 # & ( a 1 a 2 a 3 a 4 & ( # ( = Y a = u = u & 1 ( u ( 2

25 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

26 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 = K P (q d -q)+k D (q d -q) (PD, no model information used). 3. measure q (encoder) and, if available, q in n c 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(t k ), q (t k ), q (t k )) a = u(t k ) k = 1,,n c Robotics 2 26

27 n c! N Least Squares (LS) identification! set up the system of linear equations " Y(q(t 1 ), q (t 1 ), q (t 1 ))! Y(q(t nc ), q (t nc ), q # (t nc )) & a = " u(t 1 ) # u(t nc ) & Y a = u! sufficiently exciting trajectories, large enough number of samples (n c! N _ p), and their suitable selection/position, guarantee rank(y) = p (full column rank) _! solution by pseudoinversion of matrix Y a = Y # u = ( Y T Y ) -1 Y T u! one can also use a weighted pseudoinverse, to take into account different levels of noise in the collected measures Robotics 2 27

28 N! 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 n c n c " Y 1 (q(t 1 ), q (t 1 ), q (t 1 )) " u 1 (t 1 )!! Y 1 (q(t nc ), q (t nc ), q (t nc )) u 1 (t nc ) Y 2 (q(t 1 ), q (t 1 ), q (t 1 )) a = u 2 (t 1 )!! Y N (q(t 1 ), q (t 1 ), q (t 1 )) u N (t 1 )!! # Y N (q(t nc ), q (t nc ), q (t nc ))& # u N (t nc )&! further practical hints Y a = u " numerical check of full column rank is more robust rank = p (# of col s)! 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 Robotics 2 28 Y = _ n c n c

29 Summary on dynamic identification KUKA IR 361 robot and optimal excitation trajectory J. Swevers, W. Verdonck, and J. De Schutter: Dynamic model identification for industrial robots IEEE Control Systems Mag., Oct 2007 Robotics 2 results after identification (first three joints only) 29

30 Dynamic identification of KUKA LWR4 video data acquisition for identification dynamic coefficients: 30 inertial, 12 for gravity validation after identification (for all 7 joints): C. Gaz, F. Flacco, A. De Luca: on new desired trajectories, compare Identifying the dynamic model used by the KUKA LWR: torques computed with the identified model A reverse engineering approach and torques measured by joint torque sensors IEEE ICRA 2014, Hong Kong, June 2014 Robotics 2 30

31 Dynamic identification of KUKA LWR4 video using more dynamic robot motions for model identification J. Hollerbach, W. Khalil, M. Gautier: Ch. 6: Model Identification, Springer Handbook of Robotics (2 nd Ed), 2016 free access to multimedia extension: Robotics 2 31

32 Adding a payload to the robot! 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)? Robotics 2 32

33 Rule of change in dynamic parameters! only the dynamic parameters of the link where a load is added will change (typically, added to the last one link n as payload)! mass! last link dynamic parameters: m n (mass), c n = (c nx c ny c nz ) T (center of mass), I n (inertia tensor w.r.t. frame n)! payload dynamic parameters: m L (mass), c L = (c Lx c Ly c Lz ) T (center of mass), I L (inertia tensor w.r.t. frame n)! center of mass (weighted average) where i = x, y, z! inertia tensor 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) Robotics 2 33

34 Example: 2R planar robot with payload g 0 gravity acceleration robot dynamics robot dynamics 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., I 2zz + m 2 a 2 2 is the inertia of the second link w.r.t. z 1 (parallel axis theorem) Robotics 2 34

35 Validation on the KUKA LWR4 robot 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 Robotics 2 of slides! 35

36 Use of the dynamic model Inverse dynamics! given a desired trajectory q d (t) of motion..! twice differentiable ( q d (t))! possibly obtained from a task/cartesian trajectory r d (t), by (differential) kinematic inversion the input torque needed to execute it is " d = ( B(q d ) + B m ) q d + c(q d, q d ) + g(q d ) + F V q d + F S sgn( q d )! 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 Robotics 2 36

37 Dynamic scaling of trajectories uniform time scaling of motion! given a smooth original trajectory q d (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 q s (r) satisfies q d (t) = q s (r(t)) same path executed (at different instants of time) q d (t) = d q d dt = " dq s # dr q d (t) = dq d dt = dq s dr dr dt dr dt = q s (r) r! uniform scaling of the trajectory occurs when r(t) = k t r + q s (r) r = q s (r) r 2 + q s (r) ṙ & q d (t) = k q s (kt) q d (t) =k 2 q s (kt) Q: what is the new input torque needed to execute the scaled trajectory? (suppose dissipative terms can be neglected) Robotics 2 37

38 Dynamic scaling of trajectories inverse dynamics under uniform time scaling! the new torque could be recomputed through the inverse dynamics, for every r = k t [0,T ]=[0,kT] along the scaled trajectory, as " s (kt) = B(q s )q s + c(q s,q s ) + g(q s )! however, being the dynamic model linear in the acceleration and quadratic in the velocity, it is " d (t) = B(q d ) q d + c(q d, q d ) + g(q d ) = B(q s )k 2 q s + c(q s,kq s = k 2 ( B(q s )q s + c(q s,q s )) + g(q s ) = k 2 " s (kt) # g(q s ) ) + g(q s ) ( ) + g(q s )! thus, saving separately the total torque d (t) and gravity torque g d (t) in the inverse dynamics computation along the original trajectory, the new input torque is obtained directly as " s (kt) = 1 ( k " (t) # g(q (t)) ) 2 d d + g(q d (t)) k>1: slow down reduce torque k<1: speed up increase torque gravity term (only position-dependent): does NOT scale! Robotics 2 38

39 Dynamic scaling of trajectories numerical example! 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 torque only due to non-zero initial acceleration total torque gravity torque component at equilibrium = zero gravity torque for both joints Robotics 2 39

40 Dynamic scaling of trajectories numerical example original total torque gravity torque to sustain the link at steady state! scaling with k=2 (slower) T = 1 s " d (0.1) # g(q d (0.1)) = 20 Nm scaled total torque gravity torque component does not scale T=0.5 s * * " s (2# 0.1) g(q s (2# 0.1)) = = 5 Nm * * 0 Nm 1 4 T=0.5 s k = 2 T =1 s T =1 s Robotics 2 40

41 Lagrangian dynamic model State equations Direct dynamics B(q) q + c(q, q ) + g(q) = u " defining the vector of state variables as x = x 1 x # 2 & = " q # q & (IR2N state equations N differential 2 nd -order equations x = " # x 1 x 2 = " x 2 & #(B (1 (x 1 ) c(x 1,x 2 ) + g(x 1 ) [ ] + " 0 & # B (1 (x 1 ) u & 2N differential 1 st -order equations = f(x) + G(x)u 2N!1 2N!N another choice... x = " q # B(q) q & generalized momentum. ~ x =... (do it as exercise) Robotics 2 41

42 Dynamic simulation Simulink block scheme input torque command (open-loop or in feedback) u + c(q, q ) q, q B -1 (q) q 1 (0) q 1 (0) q 1 q 1 q 2 here, a generic 2-dof robot " " q 2 " " q 1 q 2 g(q) q q q 2 (0) q 2 (0) including inv(b) " 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) Robotics 2 42

43 an incorrect realization u 1 u 2 c 1 + g 1 _ b 12 b 12 q 1 (0) q 1 (0) 1 q 1 q " 1 b 11 _ " 1 b 22 algebraic loop! q 2 (0) q 2 (0) q 2 q 2 " " q 1 q 2 causality principle is violated!! c 2 + g 2 inversion of the robot inertia matrix in toto is needed (and not only of its elements on the diagonal...) Robotics 2 43

44 Approximate linearization! 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 2 nd -order model! same result, but easier derivation... equilibrium state (q,q) = (q e,0) [ q=0 ] equilibrium trajectory (q,q) = (q d (t),q d (t)) [ q=q d (t) ] g(q e ) = u e B(q d ) q d + c(q d, q d ) + g(q d ) = u d Robotics 2 44

45 Linearization at an equilibrium state! variations around an equilibrium state q = q e + "q q = q e + " q = " q! keeping into account the quadratic dependence of c terms on velocity (thus, neglected around the zero velocity) B( q e )" q + g(q e ) + # g "q + o "q, " q # q q=qe! in state-space format G(q e ) q = q e + " q = " q u = u e + "u ( ) = u e + "u infinitesimal terms of second or higher order #"q& "x = " q ( " x = # 0 I & -B -1 ( q e )G(q e ) 0 ( "x + # 0 B -1 q e ( ) & ("u = A "x + B "u Robotics 2 45

46 Linearization along a trajectory! variations around an equilibrium trajectory q = q d + "q q = q d + " q q = q d + " q u = u d + "u! developing terms in the dynamic model... B( q d + "q)( q d + " q ) + c(q d + "q, q d + " q ) + g(q d + "q) = u d + "u B(q d + "q) # B(q d ) + N i=1 b i q e T i "q q=q d C 1 (q d, q d ) c(q d + "q, q d + " q ) # c(q d, q d ) + c "q + q q=q d q = q d g(q d + "q) # g(q d ) + G(q d )"q i-th row of the identity matrix c q q=q d q = " q q d C 2 (q d, q d ) Robotics 2 46

47 Linearization along a trajectory (cont)! after simplifications with B( q d )" q + C 2 (q d, q d )" q + D(q d, q d, q d )"q = "u! in state-space format " x = D(q d, q d, q d ) = G(q d ) + C 1 (q d, q d ) + # 0 I & -B -1 ( q d )D(q d, q d, q d ) -B -1 ( q d )C 2 (q d, q d ) ( "x + # 0 B -1 q d = A t ( ) "x + B ( t) "u a linear, but time-varying system!! N # i=1 " b i " q T q d e i q=q d ( ) & ( "u Robotics 2 47

48 q " IR N Coordinate transformation B(q) q + c(q, q ) + g(q) = B(q) q + n(q, q ) = u q 1 if we wish/need to use a new set of generalized coordinates p p " IR N p = f(q) q = f "1 (p) p = "f(q) q "q = J(q) q q = J "1 (q) p, u q = J T (q)u p p = J(q) q + J (q) q [ ] q = J "1 (q) p " J (q)j "1 (q) p 1 B(q)J "1 (q) p " B(q)J "1 (q) J (q)j "1 (q) p + n(q, q ) = J T (q)u p J "T (q) # pre-multiplying the whole equation... Robotics 2 48

49 Robot dynamic model after coordinate transformation ( ) = u p 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 for actual computation, q " p these inner substitutions q, q " p, p are not necessary B p (p) p + c p (p, p ) + g p (p) = u p non-conservative generalized forces performing work on p B p = J "T BJ "1 ( ) = J "T c " B p c p = J "T c " BJ "1 J J "1 p ( ) = S p p, p c p p, p symmetric, positive definite (out of singularities) ( ) p g p = J "T g J J "1 p B p " 2S p skew - symmetric quadratic. dependence on p when p = E-E pose, this is the robot dynamic model in Cartesian coordinates Q: What if the robot is redundant with respect to the Cartesian task? Robotics 2 49

50 Optimal point-to-point robot motion considering the dynamic model " given the initial and final robot configurations (at rest) and actuator torque bounds, find! the minimum-time T min motion! the (global/integral) minimum-energy E min motion and the associated command torques needed to execute them " a complex nonlinear optimization problem solved numerically video video T min = 1.32 s, E = 306 T = 1.60 s, E min = 6.14 Robotics 2 50