RSDTR-4-B2 CONNECTION BETWEEN FORM ULATIONS OF ROBOT ARM DYNAMICS WITH APPLICATIONS TO SIM ULATION AND CONTROL J. L. Turney T. N. Mudge C. S. G. Lee November 1981 Center for Robotics and Integrated Manufacturing Robot System Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109

TABLE OF CONTENTS 1. IN TR O D U C T IO N...................................................................................... 2 2. N O T A T IO N............................................................................................... 5 3. E Q U ATIO N SE TS...................................................................................... 16 3.1. The Lagrange equations.................................................................. 16 3.2. The Recursive Lagrange.................................................................. 16 3.3. The Newton-Euler equations........................................................... 18 3.4. The Gibbs-Appell set of equations................................................... 19 4. CONNECTION BETWEEN THE LAGRANGE AND NEWTON-EULER................. 21 5. CONNECTION BETWEEN RECURSIVE LAGRANGE AND NEWTON-EULER.. I.............,................................................................................. 2 7 6. CONNECTION BETWEEN GIBBS-APPELL AND NEWTON-EULER................. 29 7. IMPROVEMENTS TO NEWTON-EULER....................................................... 31 8. COMPUTATIONAL COMPLEXITY COMPARISON........................................... 32 9. APPLICATION TO SIMULATION................................................................ 40 10. APPLICATION TO CONTROL..................................................................... 42 10.1. Determining joint velocities and accelerations......................... 42 ii

10.2. Load adjustment trajectory planning............................................. 43 11. CONCLUSION................................................................... 44 12. B IB LIO G R A P H Y................................................................................. 45 A P P E N D IX.................................................... 47 iii

1 ABSTRACT In the following, the mathematical connection between several formulations for robot arm dynamics are exhibited. This provides one with a check of their consistency. The computational complexity of the formulations are then compared, followed by a study of the inherent parallelism of each formulation. Finally, a discussion of the application of the formulations to control and simulation is presented with an eye to an optimum approach for each of these applications.

1. INTRODUCTION With the plethora of manipulator dynamics formulations now in publication [Pau72, Bej74, Lew74, LWPBO, HolBO, Hot3O], we felt it a necessary and useful exercise to exhibit the underlying mathematical connection between those of the most interest. We began this study with an exploration into the parallelism of the then existent dynamics formulations for the purpose of designing special purpose hardware to relieve the computational congestion in gross motion control [ISA paper]. In pursuing this goal we discovered inconsistencies in the Newton-Euler formulation [WaL78], the formulation of most interest. These inconsistencies were later removed in [LWP80]. Nevertheless, we acheived some insight into the optimality of a dynamics formulation by understanding the complete connection between formulations. We present here our original study along with comparisons with more recent formulations [HO180, HoTBO], together with our study of parallelism and a discussion of applications in control and simulation. To begin with, an "arm" is defined to be an open chain of links driven at each joint by an actuator in a coordinated fashion to move the end-effector or "hand" link with multiple degrees of freedom along a prescribed trajectory. Uicker [Uic67] used Lagrange formalism to derive a compact but complex set of equations for open link chains. Lewis [Lew74] (Note: The notation of Lewis is used here.) and others then applied these equations to robot arm dynamics. This compact equation set will be referred to as the "Lagrange" set in order to reflect the physical formalism involved in deriving this set of equations. These equations were the first to appear and although they are quite compact they are too comnplx for practical applications, being of the order n3. (See Appendix A for a computational breakdown.)

3 Luh, Walker and Paul [LWPBO] employ Newton's laws applied to rotating systems and obtain a computationally less complex set of equations than the Lagrange set, but at the sacrifice of losing the compact and well defined structure present in the Lagrange set. This equation set will be referred to as the "Newton-Euler" set. Intermediate in complexity between the preceeding two equation set is the equation set of Hollerbach. Hollerbach [HolBO] derived a recursive form of the Lagrange set which has roughly the efficiency of the Newton-Euler set but not the compactness of the Lagrange set of Lewis. This set will be referred to as the "recursive Lagrange." Finally, Horowitz and Tomizuka [HoT8O] use Gibbs Appell formalism to derive a set of equations whose complexity falls between the Lagrange and Newton-Euler set. They, however, did not propose to perform the actual computations. In their case the structure of the equations was obtained in order to parameterize the computation and allow adjustment of parameters by adaptive control. An examination will be made of this set, in any case, and it will be shown that it is a very close to the Newton-Euler set. This set will be referred to as the "Gibbs-Appell" set. in the following we: (1) Introduce a consistent set of notation. (2) Display the form of all the equations of interest in this notation. (3) Develope the underlying connection between all the formulations mentioned above. (4) Discuss computational complexity and inherent parallelism of the above formulations.

4 (5) Discuss applications of an "optimal" formulation to simulation and control. (6) Conclude with a summary of results.

1. NOTATION The following set of notation is adopted to provide consistentency between the formulations. Matrices, and tensors will be represented in upper case type, while vectors will be in boldface type. Greek indices will sometimes be used to denote components of a vector or matrix and the "summation convention" is employed, i.e. repeated indices are assumed summed over all three coordinates. For example the inner product between vectors a and b can be written in two ways, in matrix notation, atb and in terms of the vector using the summation convention, ab7. The product of two matrices A and B, C, is C = AB in matrix notation and C#=Aa7Byp in component notation with the summation convention. 1R represents a three by three rotation matrix which maps a vector from its representation in the ith link coordinate frame to its equivalent in the jth coordinate frame. Some well known properties of rotation matrices represented in this notation are: (Rt)=(Rj)-1= R 1.1 A superscripted t, i.e. notation ( ), denotes a transpose. A rotation between coordinate frames i and j can be written as a chain product of rotations between successive frames: R'i=R+R1 Rjf+2 RIL, 1.2 In general, with the inverse defined by Eqn. 1.1, one obtains the relation R' Ri = Rj for all integer values of k. Further define Ri =E, the identity, for consistency. Notice E has been used as the identity matrix rather than the more usual "I". "I" is used later to denote the inertial tensor.

.U2 0 Figure 2.1 Each link, i, of the arm will have its own coordinate frame fixed in the ith link and referred to as the ith frame as pictured in Fig. 2.1. A unit vector along the z axis of the ith frame and represented in the ith frame will be denoted by z,. The same unit vector may be represented with respect to the base (0oh) frame by applying a rotation, i.e. Rhzj, but to simplify notation, such vectors are starred to indicate that they have been rotated into a base frame representation, i.e. R^ziz*. The lower index indicates the fixed

7 frame to which the vector originally belongs. Rotations operate on a vector product in the following fashion: R(b x c)=Rb x Rc, 2.3 where b and c are any vectors, since a vector product must itself transform as a vector under rotation. One often encounters expressions of the form: Ri( Zi x ci)=Rk zi x Ri c, 2.4 where zi, as before, is a unit vector in the z direction of the ith frame, and Cq is also a vector in the ith frame. In order to simplify this above frequently occurring expression, define a matrix: -1 01 Q= 1 0 0 2.5 o 0 0 which is the matrix representation of the above cross product, i.e.: QZci= z x Ci 2.6 when ci is represented as a column vector with a basis in the ith frame. QZ is actually a matrix often used in mechanics when dealing with angular momentum. The QP matrices have the property that Qlc=xi X Ci, Qyc=yi x ci, and QBci= Zi x ci as shown above in Eqn. 2.6. One can capture the action of a vector cross product in a matrix operation. The QP matrices are listed below: 000o 0 01 0 -1 0 Q- 0 -1. QY= 0 00. QZ= 1 0 0 2.7 o01 0,-1 0 0 00 The QP are just components of the Levi Cevita tensor. Using this notation the cross product between two vectors can be written in matrix notation as: (b x c)=[ (bQ+byQy+bQ) c] =blQc 2.8

8 Also notice another property of the Q matrices: QXQy=QyQX+QZ. and cyclic, 2.9 or in component notation. aQ?7Q =66a a6Ps2. 2.10 where 6 is the Kronecker delta. This equation can be shown by explicit Q multiplication. From Eqns. 2.3 and 2.8 there is a useful way of commuting rotation and Q7 operator: (bi)QP ci=R-R(b x ci)-R{ bi x R -(R b)pQ (R c) 2.11 Confining ourselves to arms with links connected in the fashion of Denavit and hartenberg [DeH55], where all relative joint rotations of the ith link occur about the ze-l axis, Fig. 2.1. In this case, matrices, R'i, have the form: cosxi -cosgi sinij sinri singij RIl= sini1j cosgt cosi1 -sinoi cosi, 2.12 0 sinapi costi where Ahs is the relative joint angle between links i and i-1 (Fig. 1) and pi is a fixed structural angle which allows successive coordinate frames to be set up so that joint rotations always occur about the z axis of the previous link. For example, in Fig. 2.1, a il rotation about the zo axis aligns the xt and xo axis while the fixed rotation of gy=i2 about the xl axis brings the Zi axis into coincidence with the zo axis. ( Note that the xi axis is always chosen perpendicular to both the z-i and the zi axes.) It can be shown that: _ — ci = Q. Ri =Zi- X R-l c, 2.13 c6QRa=j.xRj1

9 by comparing the derivative to the matrix product, and hence from Eqn. 2.2: R' 6 R2~' d Ci= Rl l= R1=Rl-' Q Ri lI i=R^'l(zji_ x Ri_ ) 2.14:0;dj 8=...j. where j!k, and hence also: 62 RI ci=RR-1 Qz R": QZ R_ lc= R-l ( zu-1X (R:v- z x R-l ci)), 2.15 where u=min(j,k) and v=max(j,k). Thus, differentiation is reduced to matrix multiplication or a cross product. Denavit and Hartenberg [DeH55] introduced a matrix, Ti_, which expresses both the rotation and translation necessary to map a position vector in the ith frame to its equivalent in a displaced i-1th frame. The notation of [Lew74] is used. T_1 operates on an augmented form of a vector di in the ith frame given by: 1 and the matrix T_1- is given by: T-1=l[ P-i-11 2.16 T-_,= 0 1 The position pointed to by dia in the displaced i-1th frame is given by: TIl d.. The submatrix, VR-1, is just the rotation matrix discussed above, and pil- is the displacement of the ith origin from the i-1th origin viewed in the i-1th frame. A similar vector describing the same displacement, but viewed in the ith frame would be Rj-1 piLI. To be consistent with the notation of [LWP80] define this displacement between the ith and i-l frames as viewed in the i'h frame as r (Fig. 2.2), hence one has:

10 / so Figure 2.2 RjihlpiLFr, 22.17 The TZ-l matrices can be chained in the manner of Eqn. 2.1 to obtain: Ti = T+T+2. Ti+2 _. t~, 1-1 Rj i R J+l Pa]+ R+2 PI+] [Rii Pi- 2.18 in i c i 1 mio 1 in which case it can be shown by multtiplication of the R and p submaltrices of T

11 together with Eqns. 2.2 and that the submatrix p1 of Tj1 can be written: i I i Pi,=' RL -pm-l= RRj m'm m-p = E RJmr, 2.19 m=J+l m=j+l m=j+ 1 where j < i. Thus, the position vector p1 from the jth origin to the ith origin is composed of a chain of vectors fixed in intermediate links. Furthermore, it can be seen that: RI" i T,'dt=Tij=l &i Pi] d [RI d+i] 2.20 i.e. the position pointed to by vector di in the ith frame can be determined in the jth frame by rotating the d, position vector into the jth frame (R di) and adding a frame displacement, p1j. For convenience define a vector from the jth frame to the ith center of mass denoted,. From Eqn. 2.19 one obtains: ap _ ORg ORM aio]i 0i r 2.21 Replacing the partial derivatives using Eqn. 2.14 and Eqn. 2.15 one obtains: - = R^1 Q Rjmlrm 2.22 m=zJ and similarly: 0 P =6 ROU-l QZ RD-1 QZ Rm rm m=k where u=min(jk) and v=max(j,k).

12 The notation, atb, will be used to denote the dot product between vectors a and b, while the notation, abt, the outer product, is equivalent to a vector dyadic, in particular, (abt)c=abtc). The following vector and matrix identities are used: (a x b)tc=at(b x c), 2.25 whereas the dot and cross product can be interchanged. Tr jabt =bta 2.26 The trace of an outer product of two vectors is the inner product of the two vectors. (a x b) x c=a x (b x c)-b x (a x c), 2.27 a simple cross product relation. Another relation which appears several times is: C b. x (b, x c)=I 2 bk x (b1 x c)+t + b x (bk x c). 2.27 =1 k=l 1=k=1 k== k=1 where u=min(j,k) and v=max(j,k), and from this implies:. b x (b, x c)=: (bk x bj) x (bk x c,) 2.26 j=l k=1 =1 k=l j= k=l Tr { ABC =: Tr {(ABC)tl= Tr {CtBtAtI 2.29 Also, Tr [ ABC {= Tr l CAB j 2.30 Finally, some notation for the Lagrangian formulation concerning the link inertial tensors is introduced. Consider link q in Fig. 2.3. If one integrates the infinitesimal mass dm times the outer product dqde over the entire link mass, one obtains an inertial type matrix, Jq defined by:

13 It q Figure 2.3 Jq=fdqddm, 2.31 or f dq2dm fd% dqydm f dq dqdm Jqf=dqd dm fdd2dm fdq ddm 2.32 f d %ddm fdq dqdm fdq%2dm The integrals above are taken about the upper end of the link. Normally

14 inertias are not specified in this fashion but are taken about the center of mass. Using the parallel axis theorem [Sym71], Jq can be rewritten in terms of the link center of mass inertial matrix, Iq, and the center of mass vector, rq, shown in Fig. 2.3, as below: I'4% Qxxyy Qz-e _ _ _ 2 + mqrqa mqiqi'qy mqq~q Jq mqqrqq 2 +m qrq mq-rqrq 2qr +mqr mqrqx, mqr% rq% + mq q_ where it is assumed that a principle set of inertial axis can be found by a simple translation from the qe origin to the qth center of mass. Note that if the inertial in integrated about the center of mass one has a simpler inertial matrix, Kq:, *qy, o I Rt=+Iqyv+l 2 II -Iq +1z Kq= 0 0 ~!z' 0 2.34 2 0 0 where Kq and Jq are related by: Kq Jq-mqiriq 2.35 Notice that: Iq = Kq-Tr Kq 2.36 To be consistent with [LWPBO] define an augmented matrix, Ja, which has the form:

15 Jq qrqi 37 Jq <- t. 2.I37 Jq mqrqt mq n where mrq is the qt link mass.

16 3. EQUATION SETS The four equation sets under discussion will be introduced below. 3.1. The Lagrange equations The first formulation for an arm chronologically was the Lagrange equation set, The torque, Ti, is related to the relative joint anglular accelerations, ij, and velocities, hj, through a compact c dependent relation. The equation for Tj is: = E Tr J a J - tJ + Tr J( )l k q-i j= 1 q 6,6i k= I I 6j &dk=k q1( aj _6 3.1 + 1 g(z )t OTmq -a q=i i All of the notation has already been introduced in Sec. 2, with possibly the exception of the g term, which is just the gravitational constant 9.8 m/s. A development of these equations may be found in [Lew74] [TML80]. The equations are compact but are computationally of the order, n3 (See Appendix A), where n is the number of links. 3.2. The Recursive Lagrange Hollerbach [Hol80] has derived a iterative form of the Lagrange equations stated below: Tr= Tr E-a Di, 3.2 a~~~~~~~~~P,~~~~~~~.

17 where: d2RI d2Rc _ d2 ) D= R +'Dj++ riei+,+J ( =dt )t+mii( rit2 +m- ri +m +ri )t d2 R +miri(dt2 i dt2 and where: ej Ld2p l d2R' q=im dt2 n dt2 rq)=ei+ d2p+ d2R6 _ +m d +mi dt 2 ri and where the derivatives of Ri and pi are found iteratively as below: d2 R ( d 2R + dR' N- +. I (Q 32' 2 \ Qz ". R\ dt2 dt2 +2 dt Qz+ l (QZ)2+QZ ) ) R 3.5 and: -dRd, dRa ^-Q6j) R 3.6 dt dt 2 (Note: There were some errors in the original formulation of Di which have been corrected here.) These equations are linear in n but take more computations than the following Newton-Euler set, chiefly because they are still formulated in terms of matrices, instead of vectors, and transforming vectors between coordinate frames in computationally less expensive than transforming matrices. In the Newton-Euler case below one can take advantage of this fact.

18 3.3. The Newton-Euler equations The Newton-Euler equations are less compact, but are computationally linear in the number of links, n. (See Sec. 6 for a comparison). The Lagrangian approach allows the formulation of the solution to problems in dynamics in an "automatic" way. However, this ease of formulation is obtained at the expense of physical insight into the problem. In particular, it is often not possible to identify calculations that have little contribution to the value of the solution. With the Newton-Euler formulation these two terms can be identified and eliminated. Now for an overview. In the Newton-Euler formulation one works from the base to the hand determining kinematic terms of the links and passing them up in a causal fashion. Then one works from the hand to the base determining dynamic terms and passing them down in a causal fashion. One would assume this technique might be the most efficient and this assumption appears to be the case. C0=0 aO=0 ao=9.8 m/ s2 cj^=RI(0i- +Zi-ij) 3.7 aj=Ri'-(ai_+ji x Zi-li+2iZ-3) 3.8 ai=ri x (ri x ri)+ai x ri+Rji-aj_ 3.9 ai=ci x (ji X ri)+ai X ri+ai 3.10 fi=miai+Ri+ fi 3.11

19 nr=liatj+X x (Iiyoi)+mi(ri+rt) x &i+r1 x Ri+'fi+l+R1i+'ni+l 3.12 rT = (R'- l i )tni, 3.13 fn+l=0 nn+l=0 All the vectors above are represented in the coordinate frame of their lower index. Notice the li's, ri's, and ri's are represented in the ith frame and hence constant. There was some confusion caused by the notation of the paper by Luh, Walker, and Paul [LWP80]. In their paper Luh et al represented all vectors and inertias with respect to the base, and then rotated the final inertias and vectors into the link systems. The l's etc. looked as if they had to be rotated into the base system before being used. That, however, is not the case. 3.4. The Gibbs-Appell set of equations The Gibbs-Appell set will have roughly the same computational complexity as the Lagrange set. It is presented for completeness.'i [[ z [ _ltIq zjl-+[zi l x p' I]t[J'_1 x -P] 3.14 q=i = 1 t [ zi-l lq Z-l Zv-l + x z- 2 Tr (I) k=l j=1 +('_ a-1 p t x (, x pj) ] ] k u=min(j,k) and v=max(j,k).

20 As mentioned before the Gibbs-Appell set was derived to determine the structure of the equation for Ti. It was not intended to compete computationally with the Newton-Euler set.

21 4. CONNECTION BETIEEN THE IAGRANGE AND NETON-EULER iq= )t J + Tr q ( )t 6j t k1 -i j= 8 a^ k=-1 a j i k a 9i qn1~~~~ IC T9~~~~~~~~~4. + 2 g(za)t T mq r The Lagrange formulation when expanded by submatrix multiplication of the Tg's and J1 and can be written as below: t = E Tr Ra1Jakq + mq a t)( + ( m.r r q +Jmq al a-pg ( la + ~ Tr 02k Jq + mq O12 0 iPq t)( O 8 k=i I. q 4.2 +( aa nmq rq+ mq)( aP )t a9ja'6k 8415k O-iP +(migzo)t( ra,q + ) Defining a inertial tensor according to Eqn. 2.35, one can rewrite this as: r-f^ r TRir9^- i R + ap? viq 1P~ q=i j=1 } + Tr Rg Kq) (a )t+(mq )( k+ T I a99l } )( T A ) / 2g a^/ 9p -9 4.3 +( o qRomq)( +('O~k m Oi + Tr (migzo)(Ri-' Ri l-l x Pi li) ] (In dealing with the gravity term one can use Eqn. 2.23 and 2.14 to show that:

22 (rnigzo)t ORg _p k, + a PO 4.4 (migzo )t i rg+ - =(migzo)R (zj x p) 4.4 and then Eqn. 2.26 to obtain: = Tr i(migzo) R( Ril-' zj- x Ri'-1' _)t 4.5 One can contract the time derivatives to obtain: n F d2 Re a g d2 W a M Tr d2R Kq(,R )t+mq d2 ( )J 4.6 q=i ^ dt2 d( i mq.dt2 ( 4.6 + Tr I (migzo) RI ( Ri- ji-i x Rii-l -Piq )t Now define an acceleration matrix, Aq: d2R&4 Aq-=R dtZ 4.7 dt2 By Eqns. 2.14 and 2.15 another relation for Aq is: A R= R R1 QZ R Q R QZ Rv k 4.8 j=1 Q j =lk=l where u=min(j,k) and v=max(j,k). And now an aside to determine the form of the Aq term in Eqn. Define a quantity by the iterative relation: q= Rq ( q-qi + Zi- Oi), co=O 4.9 Upon expanding this becomes: Aq= R-Lzj_-ii 4.10 j=1 Define a second quantity by the relation: aq=R R- ( aq_1 + Zq-1 fq+ q-l X Zq-l q) 4.11 iterating this becomes:

23 J=1 J=; 4.12 a= f E Rq -1 2j 1' j + ^ S RJ-q ( -l X R-1 -) k = 2 Rd-l zjl + t i (R:-lk, x Ri Bzjl)q fk41 J=l J=l k=1 (aq and wq are, of course by Eqns. 3.7 and 3.8 just the angular acceleration and velocity expressed in the ith frame.) Consider an arbitrary vector in the qth frame Cq then: aq X Cq+oq X (Cq X Cq), 4.13 can be written as: zR-I Rl- zj-1 j x cq+ I (C Rl Zlk-ldk X Rq 1 z-1j) X cq j=i j=1 k=l 4 n 4.14,-. S RqZj-lj x (RZk- ltk X Cq). j=1 k=l Employing Eqn 2.27 on the second term and third terms and moving a rotation Ra out using Eqn 2.3 one obtains: R~O[t R7'lZj-i x Cq j+t t Ru-Iz x (Rv-1.'z_.1 x Ri-,Zcq)] 4.16 j=1 j=1 k=1 Since a cross product can be replaced by a Q8 matrix operation from Eqn. 2.8 one obtains: = Rq QZ cq RJ, ifj + t e Ro-1 QZ Rui: QZ R^-i cg1.k, 4.17 j=1 j=l k=l Thus from Eqns. 4.8 and 4.13 one can see that Aq can be written as: Aq=( aq)p QP +( C)q)pQP ( og)QX 4.18 One recognizes the acceleration aq represented in the qth system: mq dt2 =Ro mqaq 4.19 Note that:

24 d2ea A d2R n A A d_-_=. d2R - P= S Rd Aj -q= R& R5 Aj A R r.+ Aq, 4.20 dt2 j=i dt2 Pi 1' U Uj qj q at t =i at q=i j=l S=j+l hence: RaAq= Roq Aq r + R-'1 aq_i 4.21 where aq has been defined by the iterative relation: a= Aq rq+ R-1q-1 4.22 These are just equations 3.9 and 3.10. Using Eqns. 4.7, 4.21, and 4.22, the equation for tau, Eqn. 4.6 becomes: Ti= Tr R (Aq Kq(R Q (Rq- 1.zi,))t q,-(R R 1-Zi=i l X ~ 4.23 +mqaq ( ROq R R-l z-l1 x p0 _)t) The gravity term has been absorbed into the acceleration iterative relation Eqn. 4.22, whereas, now ao= migzo. Consider the second term. The trace of an outer product can be changed into an inner product by Eqn. 2.26: X R4.24 mqFrl(Rq-' Zi-, x RX-1 _, )t q=l Now interchanging the inner and cross product one obtains: (RtZli-l )t E Rr - x Ri mq i4.25 q=i The equation for Ti is: n T'ri = Tr Aq Kq Q'(Rq R Zi- )7)t q=i l ^ 4.26 +(Rq-l,-l)t E R,-l' Pl x RX " m.a q=i

25 one can see that this can be rewritten: Ti= (R1zi)7[ TrAqKq (Q7)+ ( SR1 x Rimqaq 7 4.27 q=l 1 q —i The first term in Eqn. 4.27 becomes: (RI-1z-1l), Tr{[ (aq)pQP+(cq)PQP(C)q)xQXKq ] (Q)t} 4.2 q=i Examine the first term in terms of its components: (R i' zi )y( a q)p^QxcQ^A( Kq)Xx 4.29 employing Eqn. 2.10 =( R- zl-I ),( a q)p(6Xp6X-y6xX6-7)( Kq)Ax (R-' zi- )7( Kq-E Tr Kq),p( aq)P=(R- zi- )tIq aq K is related to I through Eqn. 2.36. The second term of Eqn. 4.28, in terms of its components is just: (Rl-'zi-t)7(Vq)^p(Oq)x Tr Q7)tQ Q KqJ 4.30 Consider the inside of the trace written in component form: QQpQpwa ( Kq),, 4.31 using 2.10 this is: Q7 6c 6p 6,- 6p( Kq)a,, 4.32 where the symmetry of Kq has been taken into account to allow (Kq),, to be rewritten as (Kq),,. Contracting the Kroneker delta's and including the terms outside the trace, one obtains: (o i-) Q (KQ (K)(- Rq - ) ( K)( ( c 4.33 This is:

26 (Rqz-'zi-l)t[ q x Kq Wq+0] 4.34 Since wq x (-E Tr Kql)oq=O one can add this in and obtain an expression with the inertia I matrix: (R-zi-_l)t[a)q X Iq cq] 4.35 Adding 4.35 to the previous result of Eqn. 4.29 one has: Tr{AKq(Q(R- zi-1)7)t}=(Rq li-)t[Iqaq+ cq x (IqCJq)] 4.36 Thus one can see that the equation for Ti, Eqn 4.27 can be rewritten as: i= (R-l'zl)t[Iqaq+Oq X (Iq q)+ Ri-l l_ X RiqmqN] 4.37 n =(RZi' -z_,) Riq[1qaq+tq x (TIqcOq)+R-l q, x R qmqAq] q=i P-1 by definition Eqn. is: R2 Ri-,i r+R{Ai (rq+rq) 4.38 8=1 Combining this with Riq mqaq one obtains: n n n n v -Pq-1 x Riqmqq= Riq(rq+rq) x RiqmqAq+ RiS(r8 x Rqmqa) q=i q —i 8=i q=s+ 439 ni n n = Z Rin [( rq + rq) x mq, aq+ Ri ( rs x R:+ f+l), q=i s=l where f,.- is defined as E Rq+l mqhq. (Notice that this is just Eqn. 3.11). If q=s+1 one now defines ni by the recursion (same as Eqn. 3.12 ): ni=Iial+o x I1ji+(ri+ri) x ai+ri x Rit+fi+l+R^ifni, 4.40 one can see that ri can be written as: Rwhi h is Eqn..)t1 which is Eqn. 3.13.

27 5. CONNECTION BETWEEN RECURSIVE LAGRANGE AND NEWTON-EULER The first item to note is that the recursive vector el defined in Eqn. 3.3 is just the vector RI fi defined in Eqn. 3.11. The second substitution to be performed is to replace Ji by Ki+miririt using Eqn 2.35. Then the defining equation for Di becomes: ii-iiit2 f+)K( d )+t2 r dt25 )1 d2R d2p1 d2R D+ = RP Di + r 1i( l+ Ki ( 2 )t+min ri d2 )t + dt2+m d2 )t+mir,( dt2 r)t The last four terms can be combined into: mi(ri+r0)( )( ri + = r)(Ro mi a ), dt2 so the equation for Tj, Eqn. 3.2 can be written: 8 R6 d2 RR T -= Tr t [RI+ Di+ +r eR+ f"l+ Ki( )t+( ri + YI)(R mi )t] 5.2 a Rd The term "i- can be rewritten as Ri ( R-1 zj_ ),Q7 With this the third term can be rearranged into: lir Ro(RR-' zi-l)7, Q Ki (Ri Ai)t= Tr J(Ri-' zi-,)7 Q Ki AitJ 5.3 Taking the transpose of the contents of the trace one obtains (Ki is symmetric): Tr {Ai Ki Q7(R i- i_1)7= (Ri-l zi_l)tli ai + toI x (Iii). 4 using Eqn. 4.36. The second term in Eqn. 5.2 can be treated straightforwardly. a R After substituting in for one obtains: Tr {R RR-' zi-l x ri(Ri Ri+ fi+,)tl= Tr (Ri-'zi-_, x ri)(R,+i fi+,)tl =(Rif+' f+l)t(Rt-'z_-1 X ri)=(Ri-lzi-l)tr_ x Ri f+i,.

28 upon exchanging a dot and cross product. In a similar fashion one can show that the third term of Eqn. 4.23 can be written as: (Ri -' )t [ (ri+r,) x m, ] 5.6 The terms found in Eqns. 5.4, 5.5, and 5.6 are just the terms of Eqn. 3.12 for ni. With a little more effort one can show that the R,+' Di term gives a R?1n1i+l contribution. The Recursive and Newton-Euler formulations are very close. As mentioned before the only reason that the Recursive-Lagrangian is computationally more complex than the Newton-Euler is that the Recursive Lanrangian passes 3 by 3 matrices between link frames. These matrices are then combined with the I,r, and, r fixed objects. The Newton-Euler, however, passes all quantities as vectors between frames and saves considerable computation.

29 6. CONNECTION BETWEEN GIBBS-APPE.LL AND NEWTON-EULER The Gibbs-Appell set of Horowitz and Tomizuka [HoT80] is yet another formulation that falls between the Lagrange and the Newton-Euler. We show in the following that it is quite close to the Newton-Euler. Examine Eqn. 3.14. If all cross products with zi'- are exchanged with dot products evoking Eqn. 2.25 the zi 1 terms may be pulled to the front: Tt(_-l )tE [ n [ Iq Zi + p*i X (Zf_1 X p ) ] j q=i j=1 +I x1 [ Iq 1- Tr1lq] 6.1 J=1 k=l + X pi X ( X (-, X p ) ] ] The second and fifth term are from Eqn. 4.16 just: p x Aj* pj=_ pq x s 6.2 q=i = 1 q=i In order to combine the first, third and fourth terms, they are pulled into a trace. The first term is: -' tl; z._1;2 =(Z'-_ )[ (K-),-6f(K()~^]( z;-1 j)p 6.3 from Eqn. 2.36: -(i - )7(Kq)AX( zj'.-1 j)P(6x67A-A-6X,) =( z-i)7(KOAx( z.-1X j)PQlQ~, With some work the third and fourth terms can be combined in a similar manner to yield: t nTriz _ x (zlx Kxq)(zQz)t j 6.5 J=l k=l Employing relation Eqn. 2.28, Eqn. 6.4 and 6.5 are combined to yield:

30 Tr{[ t zeia o x Kq+ (Zk-i X Z-l) X KIk Xj J=1 j=l k=l + x x Zl X (IZk- X Kq) k j](Z-l Q')ti k=l j=1 =Trcaq x Kq+cq x (cq x KI)(zi-1 QZ)t =TrtA~Kq( Q )tI 6.6 Combining 66 and 6.2 we have Eqn. 4.26 represented in the base frame. The final connection between the Gibbs-Appell and the Newton-Euler follows from the discussion in Sec. 4.

31 7. IMPROVEMENTS TO NEWTON-EUIER Eqns 3.7 and 3.8: i=R?- (fil +-izil) 7.1 ai=Rii-l(ai-_l+ci x Zi-lqi+ iZil), 7.2 are basic to the model but one can use the consolidation operation introduced in Eqn. 4.18 to reexpress Eqns. 3.9 - 3.11 as: ai=Airi+Ri-'- 7.3 i=Ai i+ai 7.4 fi=mij+Ri+f li+ 7.5 And if one uses the first term from Eqn. 4.27 for ri and hence for ni one can write: n~= Tr IAiKt( Q? )ItI+[mi(ri+ ri) x ai+(ri) x Rrilfi+l+Rt+ln+l, 7.6 The first term for n looks ominous but amounts to a (AiKi)yz-(AiKi)2y contribution to nx and similar contributions to ny and nr. This form saves some computation (see next section, Sec. 8, especially Tables 8.4 and 8.5 and gives more parallelism to the computations (see Table 8.6).

32 8. COMPUTATIONAL COMPLEXITY COMPARISON In the following we compare the Lagrange, recursive Lagrange, NewtonEuler, and modified Newton-Euler formulations to determine their relative computational complexity as a function of the number of links in the arm, n. The complexity of the three approaches is displayed in Table 8.1. Approach multiplications additions!-Lagrange: - - -,,16 n2 -,n.n.+ 5 n2...,n - Lagrange 6 2 3 3 Recursive Lagrange 226n 144n Newton-Euler 10Bn-12 100n-9 Modified Newton-Euler 90n-27 88n-24 Table 8.1. Computational Complexity of Formulations A similar table was derived by Hollerbach [Hol80]. However, he arrives at an n4 dependence for the Lagrange formulation and a 150n dependence for the linear Newton-Euler formulation. The discrepancy can be accounted for by the fact that he carried out the operations "more or less as set forth" [Hol80] and made no effort to interpret the equations more efficiently. To see how Table 8.1 was derived consider first the Lagrange approach. Determining the kinematic contribution for the ij coefficient is linear in the number of links, n, but determining the coefficient of the 1mk is of order n2 since the calculations must be done for each value of j and k. These kinematic calculations are then reperformed for all n torque calculations. So the whole process is of order n3. A breakdown of computations is shown in the Table 8.2.

33 Lagrange terms multiplications additions _TI _32n(n-1) 24n(n-1) [T- 32n(n-1) 24n(n-1) a,6j C _' _,1..1321 02T~.2 n(n2-1) Bn(n2 -1);'I' ]+ I ], + J b~lL k=1 l'n3+-Ln2+-20n 16 n3+n ITr| | l,& B- 38n + n 8n2+7n (zOt o'q mqg-q 2n2+2n 2n2+n q=i Total n3+ n2n +5n 58n2 n 6 2 3 3 Table 8.2. Breakdown of Lagrange Terms Multiplications by Qi have been ignored since they amount to a row interchange and a row negation. Appendix A shows in more detail how the terms are computed. Now consider the Recursive Lagrange formulation.

34 Recursive Lagrange terms multiplications additions dRg dt- - 33n 24n dt d2 R 46n 36n dt2 9R R 45n 27n d2.. 9n 9n dt2 Ri+' Di+,i 27n 18n Jd ( )t 27n 18n dt2 d2p mi(i + r) )t 9n 0 d2 R' r( r.20i 18n 6n adt ri eil 9n 0 d2 P dR t 2Mj( P )t+r di o )t 3n 6n mJ( ~dt2 )e+ dt, Total 226n 144n Table 8.3. Breakdown of Recursive Lagrange Terms Now consider the Newton-Euler computations. Using the Newton-Euler equation set one moves from the base of the arm to the hand in computing the kinematics and then from the hand to the base in computing the dynamics.

35 Thus to compute the torque, T, the kinematic and dynamic calculations are performed only once for each link. If there are K kinematic and D dynamic calculations per link, the computational complexity of the Newton-Euler set for an n link arm is (K+D)n: a linear computation scheme. Besides this linearity another advantage of the Newton-Euler set is that terms representing insignificant torque contributions can be easily identified and, if approximations are acceptable, deleted. Identifying insignificant terms in the Lagrangian formulation is made difficult because individual contributions tend to be combined in unintuitive ways. (It was noted earlier that the Newton-Euler formulation provided greater physical insight into the problem.) The Newton-Euler and the modified Newton-Euler computations are broken down in Table 8.4 and 8.5. Many of the arithmetic operations tabulated in Table 8.4 can be performed concurrently. Table 8.6 presents the number of steps required to perform the modified Newton-Euler equations if this concurrency is accounted for.

36 N-E terms multiplications additions _ i __9n 7n ai, 9n 9n A4 6n 9n a, 18n 15n ai 9n 9n fi 12(n-1) 9(n-1) Iiiai 9n 6n ci x (Iii) 15n 9n m(ri+ri) x ^ 6n 3n ri x RI+Ifi+l 6n 3n Rt n1i + 9n 6n =__ __ __ add n 0 15n Total 10Bn-12 10On-9 Table 8.4. Breakdown of Newton-Euler Terms (Note: It is assumed that an Ai is computed rather than all the cross product terms.)

37 N-E terms multiplications additions 6i~ 9n 7n ai 9n 9n Al 6n 9n a, 18(n-l) 15(n-1) mi.A12n 9n fi 9(n-l) 9(n-1) Tr KAiKi( Q7)tj 6n 3n m(ri+ri) x ai 6n 3n ri X Ri+lfi+ 1 6n 3n Ritlni+l 9n 6n add n 0 15n Total 90n-27 88n-24 Table 8.5. Breakdown of modified Newton-Euler Terms

38 N-E terms multiplications additions (Jil ~~9 7 cxai 9 9 Al 6 9 CO 2 9 7 a2 9 9 i+2, ai+2, Ai+, a, 18n 15n mia, Tr tAiKiQ7il, miri_ x a~_ fi, ni, ri x Rifi+l. 9n 12n Total 27n+ 42 27n+ 41 Table 8.6. Simultaneous steps in N-E computation. As mentioned before some applications require that the torque be in the following form: T=M(5):d +C(+1,')+G(O)+Tioad 8.1 This form results naturally from the Lagrangian formulations, however, it can be obtained with less effort from the Newton-Euler equation set using the following technique. If the application requires the form given in Eqn. 8.1, one can obtain the M(f)ij matrix element by "strobing" the iterative set of equations above with an input 6 unit vector with all inputs except ij set to zero and with gravity and the load fn and nn set to zero. This is the technique we use to obtain the M matrix in our simulation program. If one also requires the Cjk elements,

39 they can be obtained by zeroing all i's and 5's except ij and dk which are set to 1. Gravity and the load are again set to zero. The number of calculations involved will be 36n(n+l) multiplications and 26n(n+l) additions for the M matrix and 54n8+27n2+9n multiplications and additions for the C' matrices. (Symmetry of the M and C' matrices has been taken into account) The later shows that the Lagrangian could be useful if the Ci's were needed (See Table 8.2) However, in the following section, Sec. 9, on simulation one only needs the M matrix and a lumped C and G contribution. We never use the explicit C matrices.

40 9. APPLICATION TO SIMULATION The improved Newton-Euler together with the strobing technique proposed at the end of section 8 can be used to perform efficient simulations. An important use for simulation is in the evaluation of arm control strategies and allow improvements to the arm model. In simulating the arm one is given an input torque vector T(t) and initial values of the relative angular velocity vector, 6(0) and relative angular position vector, i (0), and are required to determine the resulting' (t), 6 (t), and' (t). Solving Eqn. 8.1 for ) (t), one has an expression of the form:'=M(' ) -[r -load-C( )- G(' )] =f (5,6) 9.1 If 6 is represented by 7, one has a system of 2n equations (where again n is the number of links): -7=ff(D.At) 9.2 -~=7 9.3 Now that one has the equations in this form one can perform a standard Runge-Kutta four point integration. Assume that y represents the augmented 2n vector (ry,a)tl and further assume that h represents the augmented 2n vector (f,7)t. The equations become simply: y=h(y) 9.4 The new values of y are, of course, just the new values of e and di. The step size e can be determined once one knows the maximum possible change in t or 6, which can in turn be found from the maximum arm velocity. e should be taken small enough so that changes in angle and angular velocity are not excessive. Evaluating h at its various input values is not simple. One must obtain the C and G contributions and subtract them from r. Then one must determine

41 the M matrix, invert it, and then solve for 6. The Lagrange equations yield an explicit form for the M, C and G matrices, but as was shown in the previous section, Sec. 8, they are of order n3. Instead the following approach works better. First one obtains the combined C+G contribution by zeroing the the's input vector and inputing the af and O vectors. This contribution is subtracted from r. Next one inputs zero gravity and also zeroes out the 6 contributions, but retains the ) values. The Newton-Euler equations are then strobed for various components of M by setting all but one component of 6 to zero. This process takes 36n(n+l) multiplications and 26n(n+l) additions as mention in the previous section, Sec. 8. Inverting M can be done by Gaussian elimination with no pivoting necessary since M is a positive definite matrix. [HoT80] ) can be found using: =M-'(7r-Toad-C-G) 9.5 The resultant 5 and a are determined from the input r using Runge Kutta four point integration discussed above. With real time simulation, one can compare measured arm torques with the simulation predicted torques allowing one to adjust inertias vectors, friction and other parameters in the model to fit the real arm motion.

42 10. APPLICATION TO CONTROL In order to discuss real-time control of the arm one must consider an integrated model for the arm's motion. By this is meant, given the desired hand trajectory of the arm one should be able to perform all steps necessary to determine the torques needed to move the hand along that trajectory. Also one should take into account friction at the joints as well as vibration in the arm. 10.1. Determining joint velocities and accelerations Consider the trajectory problem. In order to apply the equations of motion Eqns. 7.1, 7.2, 7.3, 7.5, and 7.6. one need know the joint angular velocities and accelerations ij's and fj's. If one looks at the equations of motion one can see that one can relate the hand's angular and linear acceleration to the ij's and 6's by:,,I~~~~~~~~~~~..~10. ==L- +N(4). 10.1 where 6 and 5 are six vectors, L is a six by six matrix which exhibits the linear dependence of ah and ah, (the angular and linear hand accelerations) on * and N is a nonlinear contribution from d terms. To solve for, one could "strobe" the equations as in Sec. 8. First 4 is set to zero. One can find the nonlinear contributions of the last computed c's and that of gravity to the angular and linear hand acceleration. One can then "strobe" out the matrix L, one element at a time by zeroing all nonlinear contributions and by zeroing all but one 6 element. One can then solve for e through: =L-[ ih -N )] 10.2 One can then use a Runge Kutta scheme to integrate fi to obtain the new e

43 and t. Once one has t, I?, and A, one can apply the equations of motion to determine the needed torques. 10.2. Load adjustment trajectory planning One would like, if possible, to plan the arm trajectory "on the fly", allowing one to eliminate an initial delay time for trajectory computation, and also allowing one to adjust for unforeseen shifts in the load (i.e. sloshing mode in a liquid being carried). The difficulty in planning any trajectory is determining hand linear and angular accelerations which do not overload the arm's actuators. The following method is proposed. First block out a crude trajectory solving the Eqns. 10.1 and 9.5 for [ ah, abt in terms of the available ri's at widely separated points on the desired path. l ahm|-N(,) =LM-'(T -Tlad-C-G) 10.3 hinmeax]. This allows one a global view of what maximum linear and angular acceleration will be attainable on the actual trajectory. During actual arm movement one can use, Eqn. 10.3 to determine what acceleration the hand is actually capable in response to loading.

44 11. CONCLUSION A thorough investigation of the Lagrange, Recursive Lagrange, GibbsAppell, and Newton-Euler arm formulations was performed. They were all shown to be consistent with minor corrections. The insight gained in this study enabled us to formulate a more efficient Newton-Euler equation set suitable for real-time control applications or high speed simulation studies. Additionally, the technique of strobing, outlined in Sec. 8 allows one to identify the inertial and coriolis-centrifugal matrices used in an explicit representation of the torques without recourse to the Lagrangian formulation.

45 12. BIBLIOGRAPHY [Bej74] Bejczy, A. K., "Robot Arm Dynamics and Control," Technical Memo 33669, Jet Propulsion Laboratory, February 1974. [DeH55] Denavit, J., R. S. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices," Journal of Applied Mechanics, pp. 215-221, Jun. 1955. [HolBO] Hollerbach, J. M., "A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Dynamics Formulation Complexity," IEEE Trans. on Systems, Man, and Cybernetics, Vol. SMC-10, No. 11, pp. 730736, Nov. 1980. [HoTBO] Horowitz, R., M. Tomizuka, "An Adaptive Control Scheme for Mechanical Manipulators- Compensation of Nonlinearity and Decoupling Control," Dynamic System and Control Dvwision of the A.S.M.E,. Winter Annual Meeting, Chicago, ll., Nov. 1980. [Lew74] Lewis, R. A., "Autonomous Manipulation on a Robot: Summary of Manipulator Software Functions," Technical Memo 33-679, Jet Propulsion Lab, Mar. 1974. [LWP80] Luh, J. Y. S., M. W. Walker, and R. P. C. Paul, "On-Line Computational Scheme for Mechanical Manipulators," Trans. ASME: Journal of Dynamic Systems, Measurement, and Control, Vol. 120, Jun. 1980 pp. 69-76. [Pau72] Paul, R. "Modeling, Trajectory Calculation, and Servoing of a Computer Controlled Arm," Stanford Artificial Inteligence Laboratory Memoo AM-177, Nov. 1972. [Sym71] Symon, K. R., Mechanics, 3rd addition, Addison-Wesley, 1971.

46 [Uic67 Uicker, J. J., "ynamic Force Analysis of Spatial Linkages", Journal of Apptied Mechanics, pp. 418-423, June 1967. [WaL78] Walker, M. W., and J. V. S. Luh, "Manipulator Control", Technical Report TR-EE78-12 School of Electrical Engineering, Purdue University, March 1978.

47 APPENDIX The following is a possible strategy for computing the Lagrange equations in order n8 operations. First build up all the transformations TI. Product pairs are of the form: Tj T? T2 T2 T T * Tn- Tnn- n-1 T multiplications Triples can be built up as: To T T. T2 T T T' Tn-2 Tn- nT_ n-2 T multiplications n-i Thus to compute all the Tk's takes 3 (n-m) matrix multiplications or 32n(n-1) m=1 multiplications and 24n(n-1) additions. One can compute the in the following way. Q8 To takes one Q multiplication Qo To takes one Q multiplication Tcl Qi T? takes one Q and one T multiplication Q0 To takes one Q multiplication To Q01 T3 takes one T multiplication T0 Q1 T2 takes one Q and one T multiplication One can see the pattern. Each link takes n-1 T multiplications. (Q multiplications are not counted since they consist of a row negation followed by a row n interchange.) The total number of calculations is Z (m-1) T multiplications or m=l 32n(n-1) multiplications and 24n(n-1) additions.

48 The terms can be computed using the T and fragments as the 8-j61Pk 6TOi examples below: QO Qo To takes one Q multiplication Q8 QO To takes one Q multiplication Q~ Tj Q1 T2 takes one Q multiplication To Q1 Q1 T2 takes one Q and one T multiplication Qo Qo0 T3 takes one Q multiplication Q0 To Q1 Ts takes one Q multiplication Q~ Tg Q2 T3 takes one Q multiplication To Q1 Q1 T? T3 takes one T multiplication To Q1 T2 Q2 T3 takes one T and one Q multiplication T2 Qi Qz TW takes one T and two Q multiplications For each link i it takes (i ) T multiplications. If this is summed over all the 2 links, one has 32 n(n2- 1) multiplications and 16n(n2-1) additions. 3 To produce -T YiT j takes 16 q multiplications and 16(q-1) additions. j=1 *' q (q-1) O2Tiq t2Tiq To produce pairs jk takes - multiplications. Since a- q = 2Tq kin order to produce tfk takes an additional 16q multiplicaJi=lk= o a 1 ja.i tions and 16(q+l)(q-1) additions. To combine these two terms takes an additional 16 additions. To multiply by Jq takes 64 multiplication and 48 additions.

49 To produce the kinetic term: |^ aTg l ATgv 1 = % 16i k=1 81jaOk to takes 8 2 q(q+ 1)+64 multiplications and 16q(q+ 1)+32 additions. To produce the above terms for all values of q takes: E [8 q(q+ )+64]= 7 n(n+l)(n+2)+64n multiplications q= [16q(q+ 1)+32] =32n+ 3n(n2-1) additions q=l Now pick a value for i. To produce: for q=i to n takes 16(n-i+1) multiplications and 15(n-i+1) additions since one is only interested in the diagonal terms. Besides this there is a cost for summing the terms: E....~ n r 1 q-=i of n-i additions. Summing contributions for all i's results in: 1 6(n-i+ 1)8n(n+ 1) multiplications i=1 and n E 15(n-i+ 1)+n-i=n2+7n additions i=1 In the gravity terms: n OTii nqai only one component of -i mqg-r need be considered since the (zo)t projects out only one component. Computing the needed component requires 4(n-i+l) multiplications and 3(n-i+l) additions. Summing this for all i yields 2n2+2n mul

50 tiplications and n2+ n additions. Add to this -- additions to sum up the 2 2 2 2 results. Summing all contributions gives us a total of: in + 6-n2+5n total additions 6 2 4 n+58n2- 6 n total multiplications 3 3 These are the results reported in Tables 8.1 and 8.2.

UNIVERSITY OF MICHIGAN 3 9015 03526 7346

UNIVERSITY OF MICHIGAN 3 9015 03526 7346