SEL-TR-142 Equivalence of Two Formulations For Robot Arm Dynamics J. L. Turney T. N. Mudge C. S. G. Lee December 1980 Prepared under an Unrestricted Grant from Fairchild Camera and Instrument Corp. DEPARTMENT OF ELECTRICAL AND COMPUTER ENGINEERING SYSTEMS ENGINEERING LABORATORY THE UNIVERSITY OF MICHIGAN, ANN ARBOR 48109

ABSTRACT In the following, two formulations for robot arm dynamics are developed, one based on Lagrangian mechanics, and the other on Newton-Euler mechanics. It is then shown that the two formulations are mathematically equivalent, providing a check on their consistency. The computational complexity of the methods are compared. Finally, a modified formulation is developed which proves to be less computationally complex and that allows more parallelism in its computation than the original two formulations.

TABLE OF CONTENTS Introductionr..........,...............2........ 2 N o ta tio n........................................................................................ 3 Lagrange................................................................................................. 10 N ew to n -E u le r................................................................................................ 13 Connection between Lagrange and Newton-Euler................................ 17 Improvements to Newton-Euler..............................................24 Computational complexity comparison............2........................... 26 Application to simulation.......................,,,...,.........31 C on c lu sion.................................................................................3 3 Bibliography.....................................................,............... 34 Appendix enumerating Lagrange computations...........35

0. INTRODUCTION A mechanical manipulator is 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. In this paper we refer to such a manipulator simply as an "arm." An accurate dynamic model for arm motion is useful in both simulation and model based control of an arm. For the latter application several authors [Pau72, Bej74, Lew74, HolBO, LWP80, HoT80] have derived their own set of arm equations from different physical approaches. Since the physical assumptions of each author are identical, the derived equation sets, although dissimilar in appearance, should be equivalent or consistent in content. Their computational complexity, on the other hand, varies greatly. Lewis [Lew74] uses Lagrange formalism to derive a compact but complex set of equations which we refer to as the Lagrange set. Luh, Walker and Paul [LWP80] employ Newton's laws applied to rotating systems and obtain a less compact but computationally less complex set of equations which we refer to as the Newton-Euler set. Hollerbach [Hol0O] derives a recursive Lagrange set which has roughly the efficiency of the Newton-Euler set but not the compactness of the Lagrange set of Lewis. Horowitz and Tomizuka [HoT80] 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. We will not discuss the last two sets further. It was our goal to find the most computationally efficient set of arm dynamics equations in order to allow real-time control and high speed simulation of the arm. To this end we have studied the Lagrange and Newton-Euler sets of equations and have explored the connection between the two to check for consistency and to determine if there might not be a middle ground where one could achieve solutions of less complexity than either set of equations. This report presents that study. In the following sections we: (1) introduce a standard set of notation (2) present the Lagrange derivation (3) present the Newton-Euler derivation (4) exhibit the mathematical connection between the Lagrange and NewtonEuler formulations (6) develop an improved Newton-Euler formulation (6) discuss the attributes of each equation set and determine the best set for real-time control and high speed simulation (7) discuss our simulation application (8) summarize our results

3 1. NOTATION We adopt the following set of notation which is consistent with most of the literature. Matrices, and tensors will be represented in upper case type, while vectors will be in boldface type. Rj 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: i)t=(Ri)-'= 1.1 A superscripted t denotes a transpose. A rotation between coordinate frames i and j can be written as a chain product of rotations between successive frames: Rl= RI +lJ+t Rl 1.2 In general, with the inverse defined by Eqn. 1.1, we have the relation RjkRI-Rj for all integer values of k. We further define RL=E, the identity, for consistency. 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. 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 (Oth) frame by applying a rotation, i.e. R6zl, but to simplify notation we star vectors which are normally represented in their own link frame to indicate that they have been rotated into a base frame representation, i.e. R^zj=z*. The lower index indicates the fixed frame to which the vector belongs. Rotations operate on a vector product in the following fashion: R(b x c)=Rb x R, 1.3 where b and c are any vectors, since a vector product must itself transform as a vector under rotation. We often encounter expressions of the form: Rk(zi x cq)=Rkz1 x RxcI, 1.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, we define a matrix: 0-1 0' Qf= I 0 lO 1.5 000 which can be shown by multiplication to have the property that: Ql=zj x ct, 1.6 when ec is represented as a column vector in the ith frame. Qj is actually the aiz spin matrix used often in Quantum Mechanics. The sigma matrices have the property that aoiC=xj x Cl, aryCi=yj x c, and aicj=zi x ci. The ai matrices are listed below:

4 i'we.,,. eV ^'" -^I,,,.-'V Y, Figure 1 oo 0 0 00 1 0 - IaX= 00-1 aly=I 0 0, aiz= 1 0 0 1.7 01 0 -1 0 0 10 00 Using this notation a vector product can be defined as: aFx bj=(acxix+ ayrjaiy+ aizaiz)br (atca )bL, where ao is a vector whose components are the a matrices. We also define a more general matrix transformation, Qt, such that: (Riz1) x b1Qlbk, 1.8

5 where bk is any vector in the kth frame. Eqns. 1.4, 1.6 and 1.8 yield: RQc1i)R\(Zj zx CR x RKci=QIRci 1.9 We confine 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 zi_ axis, Fig. 1. In this case, matrices, Ri_, have the form: costi -cosirl sin~i sinai sinai ]RI-, 1.10 RZ..= sino cos~o cost;i -sin~o cost, 1.10 0 sineo cossiJ where OA 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. 1, a Ad rotation about the %o axis aligns the xl and xo axis while the fixed rotation of Vl= -. about the x1 axis brings the zl axis into coincidence with the zo axis. ( Note that the 3 axis is always chosen perpendicular to both the z-1 and the zi axes.) It can be shown that: B-kl. — l-i, and hence from definition Eqn. 1.9: c=RAlQ-IRi Rct=Q l1RCi 1.11 where jsk, and hence also: a Cf CRA1QJ Rj lQ, cl1=Q I-k R C 1.12 where j;k:i. Thus, differentiation is reduced to matrix multiplication. Denavit and Hartenberg [DeH55] introduced a matrix, TiL, which expresses both the rotation and translation necessary to map a position vector in the ith frame to its equivalent in a displaced i-lth frane. We use the notation of [Lew74]. Til operates on an augmented form of a vector di in the ith frame given by: and the matrix TI1_ is given by: T1I-I= pl 1.13 The position pointed to by di" in the displaced i-1th frame is given by: Ti-l aI The submatrix, Rj_,. is just the rotation matrix discussed above, and pl-. 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

6 would be RI-~-_l, To be consistent with the notation of [LWPBO] we define this displacement between the ith and i-1th frames as viewed in the ith frame as ri (Fig. 2), hence we have: 1.14 The TI1- matrices can be chained in the manner of Eqn. 1.1 to obtain: Tj=T^+ITj-2I Till 1.15 -L P 1 o ^ 1 Ki o1I- i,0 1,!, 0 1 1, 0 1 I0 1 4';e. Xf r 4. Figure 2

7 in which case it can be shown by multiplication of the R and p submatrices of T together with Eqns. 1.2 and 1.14 that the submatrix pl of TI can be written: mj+1 P-= E RJm-Ipmi-L= RjmRm-pm-'l= S Rj$rm, 1.16 mrJ41 m~J+1 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: Ti^ -^ ^ al - P 1.17 Tdia=TJiIf = I| I [ =L 1 17 i.e. the position pointed to by vector di in the ith frame can be determined in the jth frame by rotating the di position vector into the jth frame (Rjd1) and adding a frame displacement, p1, From 1.16 we have: apt A, BRf = r Replacing the partial derivatives using Eqn. 1.11 and Eqn. 1.12 and moving the Q to the left using Eqn. 1.9 we have: d -=i R-lIRQRiJlrm= 1Qi-tRkrm. m=J mar and similarly: 1.19 = C Q1-lQ-Romrm m-k Assuming without loss of generality that jk. 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)ca(btc). The following vector and matrix identities are used: a x (b x c)=(at c)b-c(at b)=(Trtc atJE-c at) b 1.20 Notice we have used E as the identity matrix rather than the more usual I. I is used later as the inertial tensor. Trta(b x c)tl=(b x c)ta=bt(c x a)=(e x a)tb 1.21 (ax b) x c=ax (bx c)-bx (ax c) 1.22 Tr B TABC BC =(ABC)tTr CtBtAt 1.23 Finally, we introduce some notation for the Lagrangian formulation con

8 Cs, / /v A/ Figure 3 cerning the link inertial tensors. Consider link q in Fig. 3. If we integrate the infinitesimal mass dm times the outer product dqd< over the entire link mass, we obtain an inertial type matrix, Jq defined by: Jq=fdqgddm, 1.24 or fdqxdm fdqdqydm fdqxdqzdm Jq fdddqxdm df dm /fddqdm 1.25 [fdq.,dqdm fd^dqydm fdqdm The integrals above are taken about the upper end of the link. Normally inertias

9 are not specified in this fashion but are taken about the center of mass. Using the parallel axis theorem [Sym7l], Jq can be rewritten in terms of the link center of mass inertial matrix, Iq, and the center of mass vector, iq, shown in Fig. 3, as below: 1.26 ^-IgxXWl I<1Z +rmEiqx mrIxyqy mr lqz Jq= m q +m rg mlqsz | -m-x mr+ IyIg +Iqyy -qzz 2 M'q r nqx mirqzIqy 2 +mqqz where it is assumed that a principle set of inertial axis can be found by a simple translation from the qth origin to the qth center of mass. Note that the inertial tensor, Iq, can be written as: Iq=Tr Jq-mq-r E-(Jq-mrqri) 1.27 To be consistent with [LWP80] we define an augmented matrix, Jj, which has the form: Q mq l mq' where mq is the qth link mass. Brackets will be sometimes be subscripted by Greek letters to indicate that the brackets that follow with the same subscript have the same contents. For example, [. COMPLEX CONTENTS ]., might be represented simply by [...,,,.

10 2. LAGRANGE Lagrange's equations allow one to determine a generalized force vector ir from the difference in the kinetic energy and potential energy of the arm, L = K.E.(X,7 ) - P.E.(7 ) where K.E. and P.E. are expressed in terms of a generalized coordinate vector, r7, corresponding to i. The Lagrange relation is: d OL aL dt "8= l 0r7i We can compute L as follows. First we consider the P.E. contributions from gravity. The potential energy of link q is just the mass times the height of the center of mass times the acceleration of gravity, mgh. The position of the center of mass of link q from the base is, using the T matrix: The height of the center of mass is just the vertical component of the position vector from the origin to the center of mass. h= (zo)tTrqP, where zS=(0010)t in the base frame. The total potential energy contribution is summing over all the links: P.E =- g(zo)tTTmr q' q=I To compute the K.E. term consider a position vector, s, as in Fig. 3 which points from the base coordinate system to an infinitesimal mass, dm, located in the qth link. s can be written as: B=p8+<-d=p&+RSdq 2.2 and using 1.17, the augmented form of s, s can be written as: s^=Td. The velocity of this infinitesimal mass in the base frame is: VIi= dg=jes,~5d; -2.4 dto = aj The associated kinetic energy is -(v^)tvadm or -2Trwva(va)tJdm which 2 2 equals: 1 - Tr-.jT d~(d~)tdm( - J-)t jk 2.5 J=lkul Jj (10 The scalar, dm, has been moved inside the brackets in preparation for integration. When each link is integrated over its mass and the kinetic energy of all n links is summed, we have: K.E.=- L - Tr k.q a'k 2.6 since: d( ^)tdm=J q, and J" is defined in Eqn. 1.28.

11 In our case the generalized coordinate is 9 and the corresponding generalized force is,. a torque about the actuator axis. Applying Lagrange's equation to this kinetic energy we obtain the torque, r1, at joint i, necessary to drive the arm link: d OK.E. OK.E. 8P.E. idt aa, ai2.7 i=1 to n. Note the potential energy has no e dependence. The potential energy contribution can be written by Eqn. 2.1 as: aP.E. = eg(z,)t T ~ ~ =g(z mrq1 2.8 Now consider the K.E. contribution: O —o, =2 -T lT. a (-)t J+ J( a( )T q=U-=l l - j o0 1 from Eqn. 1.23 and the symmetry of J. The summation over q ranges from i to n since — c for all q<i. A similar argument applies to the range of j. The first term becomes: d OK.E. = TJ 8T TrI Ja( 2.9 dt aO J' la l 2q9.f~f^J ___.... a0T% ).k 2. T' L'r —------ J fk+ aT Jq ( aT ) } jk q=,,=,k-, "JO~k "',-' The second term of Eqn. 2.7 using 1.23 and the symmetry of J is: aK.E. 2 jaJ2T t iJlk ^= Tr| -— J"( S82Ts t )tJ~kl 2.10 qu -lk= I,6 J Combining both terms of Eqn, 2.9-2.10 with 2.8 we have:.n... I. aT J)OT )t +'t= t ear..... J a1 6q Bit 2Te T

12 The matrix structure of this formulation is appealing from a programming viewpoint. It also has some appeal for control purposes in that it gives a set of equations in a O and i dependent form that allows the easy incorporation of feedback terms. The equations can be written: r M(O)+ +C(i,.))+G((), 2.11 where M is a symmetric mass-inertial matrix, C is a nonlinear corioliscentrifugal term, and G is a gravity term. M's symmetry follows from Eqn. 1.23 and the fact that ~M -." fOTJ - OT#,.'OrT& OT t q,~= ts~r8a( t tj( oj _ 2.Tr |,. J ( )tJ =M3J because of Ja's symmetry. C can be written as a column of symmetric matrices: C - c=. ~C where: and where the symmetry of these submatrices, Cjk="Cj, results from the order of the partials being immaterial. G is a simple n by 1 column vector. In spite of its concise notational representation, the Lagrange formulation is computationally inefficient compared to other formulations. As we shall see 02T& later in section 4, the -- and the _ terms represent kinematic terms Which are recalculated each time a new element of the torque vector is determined. Furthermore, the.- term is unnecessarily recalculated for each value of j, k, and i.

13 3. NEWTON-EULER 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. This is not the case with the Newton-Euler formulation. 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 true. A brief derivation of the formulation will be presented below. This derivation follows that of [LWP80]. Since Newton's second law applies only to an inertial frame, in order to avoid pseudo-forces all vector time derivatives must be taken with respect to the base coordinate frame. We assume that the base (i=O) frame is such an inertial frame, and as mentioned before in the interest of clarity all vectors represented in the base frame will be starred. Assume that frame i is rotating with angular velocity, ci, with respect to the base frame ( wi with respect to the ith frame ). The time derivative of any vector, 5e, in the jth frame as seen by the base (Oth frame) is [Sym7l]: ds -x^ * d 3.1 dto =-' dtl j dtd turn is the time derivative of another vector, say l', (i.e. s'= i —) from Eqn. 3.1 dto we have: d1l d di' d4; d dlj dtg dto dto ) dt dt dto dt dt dt1 =r;j* x (c^i x 1;)+to' x dt ( + d't(cl); x t'+' ci,-) dl1 d213' = x ( x l';)+2>; x 1+a x l'+r d 3.2 where the angular acceleration, do, do t v ds do a dto = x dt dtt The relative joint rotation angle 4i between links i-1 and i is, by convention, measured about the zi*l axis (see Fig. 1). Therefore, Fizi 1 is the relative angular velocity between link i-1 and i. Angular velocities can be built up from the relative angular velocities of the lower links by: omi =&=-, + L+3S 3.3 From Eqn. 3.1 and 3.3 the angular acceleration is then:

14 do= dc.* d(z;_1o) dto dto dto 34 =al-1 +clt X Z1 I'j++z;-1 since %-1 is constant in the ith frame. Recall from Fig. 2, p6 is a vector from the base origin to the origin of the ith frame. ri is a vector of constant length in the ith frame from the i-lth origin to the ith origin. rI is a vector fixed in the ith frame pointing to the center of mass of the ith link. We see: p=r;*+p-1 3.5 and: *d2^p d dr; d2p -' a, 0 3.6 Vdtg dtg dt3 which from Eqn. 3.2 implies: 4=)w; x (. x r;)+a; x r+aj'-,. 3.7 since ri' is constant with respect to time derivatives in the ith frame. It is also easy to show using Eqn. 3.2 that the center of mass acceleration is: d2(Yj+p) dg;i d2p' =.."+' dt dto dtg -= x (c; x' +a; x a r;+i a 3.8 Newton's law relates the acceleration and link forces shown in Fig. 4. mtik=o-f -fj, 3,9 fi is the force on the ith link caused by lower links and actuators acting at the origin of the i-lth frame, and fti is the force caused by upper links and actuators acting at the origin of the it frame. The total torque at the center of mass, Ni, can be expressed as the time derivative of angular momentum [Sym7!]: N1d(l I;). x (I d(I ) 3.10 dto dt -=li; x (I;)+la;, since I s constant in its own frame. The total torque, Ni is also related to the moments in Fig. 4 by: N,=n;i-n;,+i; x [^,-(i t +r;) x t,, which by Eqn. 3.9 gives: Nin= -nj-+ -(I +r;) x mia' -r( x f 1 3.11 Equating 3.10 and 3.11 yields: n+,(=A*+c> x m(1I,+! x 3+.12 + (UA + ) m1rx m +ri x f + A

15 e rn; /^^ ^\ \< ijc, ~'T -, Figure 4 Only the contribution of nB parallel to the axis of rotation z-1 will be produced by the ith joint actuator (compare Figs. 2 and 4). Therefore, the torque which the ith actuator must produce to achieve the desired motion is: ri=(Zi-lOtnl 3.13 Inertias 1 and vectors i, ri and ziA assume angle dependent forms, 1;(o), t'(), r1i(G), a'-l() if they are expressed in the base frame as required by Eqns. 3.3-3.13. If, however, they are expressed in their own ith coordinate frame they are constant, independent of A. We apply a rotation Ri~ to Eqn. 3.3-3.13 to allow all the vectors and tensors to be represented in their own coordinate

16 frames. Take Eqn. 3.12 for an example. Under rotation it becomes: Ru'=R Ifn R RROa *+R; O; x (RPI/RoRPJ;) +mj(R~Pj*+Rp x RPS*+ROr, x RsIRitf 3.14 +Rj+'R~,nt2+l, Since r'=R^ri and I'=RoliR~, etc, and RRlrji=ri and RP(RoliRi~)R6=Ii we can now express the rotated set of equations for an n link system iteratively as: 00=0 ao=O ao=9.8 m/ s 0c1=8sR^'(-R^O l+^fig ) 3.15 al=Rl-(ai-l+N x zi-1t+i55z-l) 3.16 aj=fi x (oi x ri)+ai x r1+R-lai- 3.17 A=-cj x (01 x Ti)+ai x ri+aj 3.18 f=ml.+ R+ 1fi41 3.19 nl=llai+ei x (Ic1i)+mjl(F+ri) x l+ri x Ri+lfi+t 3,20 f +,=:n+i=O. Gravity can be included by starting with a base acceleration, ao, of +9.8 m/ s2 since an upward acceleration of g, as in an elevator, is equivalent to the effect of earth's gravity. It is possible to assign values to f,,+ and nn+l from wrist sensor measurements and allow the arm to adjust to forces and torques encountered by the hand link. If the application requires the form given in Eqn. 2.11, one can obtain the M(- )lj matrix element by "strobing" the iterative set of equations above with an input - unit vector with all inputs except 6j set to zero. and with gravity set to zero. This is the technique we use to obtain the M matrix in our simulation program. If one also requires the Ck elements, they can be obtained by zeroing all W's and W's except Aj and %k which are set to 1. Gravity is again set to zero.

17 4. CONNECTION BETWEEN LAGRANGE AND NEWTON-EULER We are now in a position to show the connection between the Lagrange and Newton-Euler equations sets, and thereby able to show consistency or "equivalence" between these equation sets. Eqn. 3.15 can be expanded by iteration to obtain: cI= RsRrzj-j 4.1 j1l Similarly, Eqn. 3.16 can be expanded by iteration: al=iRJ'zjl-l+ER9-1(Ga)j-l x zj-3)j J=l ju -st+l Rkl_ X i -j4.2 NR^zj-,"d'j+ tR1-1zk-j x RV zlJ- k J=1 J=lk=l With expansion Eqn. 4.1 and using Eqn. 1.8 we can rewrite the first term in the acceleration expression, Eqn. 3,17, as:;i X (Wi x rj)= Rizi -l [RX -izk-, x ri) Jk Ijulkul 4.3'-k=1l From Eqn. 4.2 the second term of acceleration in Eqn. 3.17 becomes: ai x rt=R lZj-l x rij+j ((( h-'Zk-l x Ri-'zj-i x ri)jk J=1 j=lk=land with the help of vector identity Eqn, 1.22 and Eqn. 1.8: =R j'l-i x rj+ 2(R xlZkl X lZjl X rj'=1 j=lk=l — zj-i x Rk-1 X ri)j ao x risEQj-1ri a1 4,4 jul3w1~~~~~~~~~~~~ -4.4 + j (Qj lQ _L.-QlQOk-l)ri'j jalk=l Combining Eqn. 4.3 and Eqn. 4.4 we have: ai, tQ r+ k-lQ rk +SE Q-lQ E i +-R~lai-k jul I J=ik=1 i 4.5 julk=J i ri, 4= QU r1m + i 1 Q'Q1riJk+R 1l. ju;l julk=l where u=max(j,k) and v=min(j,k). The second and third term above added to

18 give k a range of 1 to i. Expanding the R-lai term gives the following: RI "a_,=Z QI:-~ I rl _,'rl_ lj9k+ R-2 ai_2, R iSQ 1r-1i+S 1 QVlQlFr6j6k+RVSi 2, j"l J=lkk l Continuing we have: i m ma=l J=l mV1 j=1 +i 2 2 QsV ir ru-'Rmr 131k+i g1 zo mil J=lk=l Changing the order of summation: mrAJ J=1 4.6 t4.6 + 2i Ql Q-tRrmj k+ROgzo, mnu J=lk=l Pulling a rotation out: -o=R~ SE Q-'Rfrmj mJ j=1 f ~ Q'lQoR-'R rmJ 0J +R ~g zo mnu J=lk= 1 From Eqn. 1.18 and 1.19 we have:,P)+ROgzo 4.7 julk=l i 0jdk O kJio From Eqn. 3.18 the center of mass acceleration, ai, can also be expanded: ^1~ = Rio Q~- ^r 4.8 + S QVQre-' rV'j'6ik+ai, j=Ik=l where again u=max(j,k) and v=min(j,k). Using Eqns. 1.11 and 1.12 the above becomes: I..), 2R t. aj=RP ( 4a~ ( aj Y)j'h+ aj I i j ~ i a J=lk=l a One can already see terms in - above which are submatrices of the Lagrange terms and. These are just contributions of the coriolis and centrifug al acce leration.

19 In developing the expansion for a and a, we expanded the corioliscentripetal acceleration, a x b+w x(0 x b) operation where b was r for a and b was r for a. In the following we write the inertia, I, as an integral over an outer product, ddt; split this outer product and apply the coriolis-centripetal operation to one of the d vectors; then use the same expansion as above for the coriolis-centripetal operation and then gather the d vectors back together into an outer product and reintegrate to obtain an inertial J matrix, For the present we ignore the last two terms of Eqn. 3.20, which represent terms passed down from upper links. We discuss their contribution later. Eqn. 3.21 then simplifies to: s=(RR-O)'[iai+fJ X J11r+ml(r1+-r) x A] 4.10 Using the relation between the center of mass inertia, I, and the J inertial matrix, Eqn. 1.27, this is: Ti=(R lt)t[(Tr{JtE-J)ati+w x (Tr}JIE-J)1i -(Tr i]~iP 3E-P]tP))a,-wi x ((Tri} fitt -E-pipt))c, + mi(r+r,) x ], using vector identity Eqn. 1.20, this becomes: (Ri-zj_.l)tl(TrJ{lE-J)ai+tj x (Tr{J{E-J)1-Pi x (ai x 4i) 4.11 - x x ( 1 X ( X Ti))+m,(r1+r0) X i] The J terms of Eqn. 4.11 are defined as a mass integral of a dyadic, didit, Eqn. 1.24. If the integral over mass is pulled outside of the trace, these terms become: rf [(RI-z,_)t(Trdc;tE-didta,) 4.12 +(R1izll)tw1j x ((TrdidtlE-didit)c) dm, and then using vector identity, Eqn. 1.20: =f IRI2-1zj lX dj x (a, x d) + (Rl-lzoj_)t( i x (d x ( dx)) dm Examining the multiple vector product in the second term of the above expression: 6i x (ad x (c, x d))=c4 x (oi x (wj x id))+(ji x di) x (oi x di) =d x (ei x (,i x d)) Both terms then yield: =:fRIslt-_)tdd x(a X4) f v~lx~4x (a1 x d~)4.13 + (R-lzl.i)td x (1 X (c. x di)] dm

20 exchanging a dot and cross product we have: =f(Ri-'zl- x d)t(ai x d)+(Rl-Zl x di)tI( x (f1 x di))] dm =fTr(ai x dx)(Q'd+)tI+ x (w x di)(QI-dt} dm =Trj(ai x d)dpT(QX )+(fi X (w x Qd))dt(Q-I)t dm Using steps identical to Eqn, 4.1-4.4 which expand aO and wi Eqn. 4.13 becomes: =fTr{ R?( R1-QJ?1 Rr 1j; = J=l + t i (R- *, Q RIQR-1 IRI- j^;) 4.14 j= lka l ddidt(RPRIQtllRi-)t1) dm Bringing the integral over m inside the trace and using Eqn. 1.11 and Eqn. 1..12 we have: Tr tR (+ 2 2eR i 1eJ,9l( O )tRi} 4.15 Since TrtBCD]=Tr}CDBJ and R1IR =E, the identity, this is: 5Tr|8 ( 0g ) ~+C TrI a8 -j ( a^; 8)tJ 4.16 Now consider the last three terms of Eqn. 4.11 which using Eqn. 4.9 to expand a and Eqns. 4.1 and 4.2 to expand co and a with some cancellation we have: ji xI jS=lki JOlkO l a k 4.17 j=1 aoj J=lk=l O'6J9313k +0(RA _lm)t Imsi+rl) x RO~migzo Consider the first and third terms of Eqn. 4.17: (RN~ i-lR)t(Mn Ti x R0 1 ) k 4.18 Bi 0 > J=lkjiBl 7 The lower subscript on the bracket will be used to indicate that the contents of the bracket remain the same in the following discussion. Using: vector identity,

21 Eqn. 1.20, and Q relation, Eqn. 1.9, this becomes: Tr, [ (RI * z-I X mjrj) 1 =Tr!Rs~ | ]mi(Qit-1)t 4.19 and since OR' Q-'=RRQO i-Z,1=R, 4.20 Eqn. 4.19 becomes: rL-o^ O2p. 8R =Tr ap + sk t eo 3 |m ( ~aj 4.21.1 J j]lkul I kr 0R, Consider middle two terms of Eqn. 4.17: (R~W-'j )tmirI x RiO t d-iij+ 4.221 0 15' J I jo k=tk42 _=1 8J jI _lk=l 813Jsfk using Eqn. 1.20 and again exchanging dot and cross products they can be recast into: TriRp| ]r(RWiziz X mir). 4.23 and since by Eqn. 1.9 and Eqn 1.18: 0pd RI-,ir-, x r=Qjr=RjRi=RR- WQj-iR_, rj=R0 ---. 4.24 they become: TrmR4. ]Xp t,tJ 4.25 Now consider the last terms in 4.17, i.e. the gravity terms. (i-tl)t(ri() + r) X Rj mrgzo) By exchanging a dot and vector product: =(R]-^ll X mil(Fl+ri))tRimigzo=(Q-Il(ii+ri))tRiOmigzo =Tr Rlomgzo[Q1L1(pj+rt)] t Using 4.20 and 4,24 this can be written: =Trlmigzo [-rj + =Tr) i+ migzo)t T-L^) - t+ OTi mig (rV)' O0-~i -

22 Combining this result with Eqns. 4.21 and 4.25 we have: + Tr (( J + mi aR t)( ) - OUr _Rl Op~ 8 3P p Y r+( kmJ1+ri + k )(m i -)t) ljk+mig(Zo)t ta which can be shown by submatrix multiplication of the T matrices to be equivalent to: 4.27 O2Rr J1m)t1 P 4.26 +., a,, ( 1 Tt}k+mig(zOW T a J=lk=l 81,6ja where Ji is defined as in Eqn. 1.26. This is the same result as the Lagrange set of equations, Eqn. 2.7, when upper link contributions are ignored. Consider now the upper link contributions, i.e. te last two two terms in Eqn. 3.20: (Ril-l, l)t(r x RI+XR i+l+Rfi'nql.) 4.28 Assume for simplicity that i+1 is the last link of the arm, i.e. f1+ and n,+2 are zero. We relax this assumption shortly. We can rewrite the first term of Eqn. 4.28 using Eqn. 1.8, Eqn. 1.9, and Eqn. 1.21 as: Tr{RPj + i (QjIlrj)tl Tr{R6+ ful+ I(R'- Qi _1ri)t 4.29 TrN~1f'4Q(4r~~)3, Tr 4.29 using Eqn.,319 and Eqn. 4.9 this can be written: _i2R'~~+l'~ OW I 4.30 J=ikrl= 61.. + + M+lgzo] (R iQi-lr)t Using the same steps used in deriving Eqn. 4.26 the second term in Eqn. 4.28 (RhlZi-jl)tP4+'nsi, can be written:

23 _=_Tv( _ d _ J_ + mi+m 1 8P ri + +mJ+1gzO)( )t +(mit+l e "ri +l+m+l 4 3mi+l3gzo) lQii-lR+ 1)t 4 (Mi + I 04i'+1+ mJ+J Mi+90)(R iQ-1Qji-]ri++)t| 0 2OII r+RO A + 431 =P,4.32 +Tr,' dR + J dp pit+,) A1 1) +(m- l r 1 FP+l O OR'+_ dtJ +V Tr ((. Ji+l + mi t( 3d R^i 1 + ( 811R+ f +( QmRmj+lt,6m 1 _2_ _ 1 +(3OmRk m+ p' a+, O*k mi+')(Rt1Q —at+r')t { Since from Eqn. 1. R+Oi- Tro( -2+ =6. I 4.32 and. 4.30 and Eqn. 4.31 can be combined to form: i actuator axis., R i, ODJil+m 61+)(1 + s (M + 1 Th+ fln+i 1 $n +(ml+l, dwjl+,+mi+l, 0 )(-O:) I } A / 4.33 +TrC ck(( 6q{ Jt"+mi+' O4~:0~+')( el 02+" opt' } 0,2I +Trmi+1gzo ( 0'-"! vhich when combined with Eqn. 4.27 can be reformed into: 7mS OTP',a If the arm consists of n links then m can be summed from i to n, and we have aTF1 a2 Tff Eqn. 2.11. One can see that the and the o - terms contain the kinematic information of the arm. Tie J matrices determine the dynamic OTm" response and the -- projects the dynamics of the ith and upper links onto the i-l1th actuator axis.

24 5. IMPROVEMENTS TO N EWTON-EULER We were successful in improving the Newton-Euler set in the following way. If the operation, wi x (0 x b!)+aj x bi is performed on general vector bi, it is equivalent by Eqns. 1.20 and 1.7 to [owtit -Trcljiot JE+(ai)ta]b or Aib where Ai is given by: _-(c2+Ws2) Wcy-cxG cWsz+cxy A Y= cy+cz -(ca+c ) cyAYC4-jX cWZAX+Y _(W2,+)co W(2+CY) WCOix-ay C4czy+ az - (0,2+ W)Y Using this consolidated operation, Eqns. 3.17-3.19 can be reexpressed as: t =Alr1+R'la4 fi= mAl(i +r) +miR/-'a-l + Rl+ 1lt+ 5.1 And from Eqn. 4.11 for r we have: i;=(Rlzj-_l)t[(TrIJJE- J)ai+wi x (Tr{JJE-J)o> -mt(Tr-r'PjtrjE-VilPtt)ai-ol x mi((Tr}rir^itE-r-iF-t))ai +mi(ri+r) X (O,)] Thus, we obtain the following for T:.= T|r 1Ai(Qi1 )1('1ztl+s -)ti(+r) x ni+ri x Ri^llf'i++Ril] Ki is an inertial matrix about the i-lth origin but express in the ith frame as below: 2 Th 0 0y li0 2 The TrjAjKj(Ql")tj can be written as (Rl-)tTrjAiKijai and therefore n can be written: n=Tr{AiKj(ot1m)t+m(i-+rj x i+(r x +(r x Rfi+ +Rt+1n, 5.2 The first term for n looks ominous but amounts to a (AiKi)yz-(AIKi)zy contribution to n, and similar contributions to ny and ns. a selects components of n. It is a matrix which performs the action of a cross product.

25 Replacing 3.19 by 5.1 and 3.20 by 5.2, we have the modified Newton-Euler set. This form saves some computation (see next section, especially Tables 3 and 4) and gives more parallelism to the computations (see Table 5). l

26 6. COMPUTATIONAL COMPLEXI`TY COMPARISON In the following we compare the Lagrange, Newton-Euler, 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 1. Approach multiplications additions.1......., 64 Lagrange -n+ -— n5 +5n -n3+5n2- — n Newton-Euler 108n-12 100n-9 Modified Newton-Euler 90n-27 88n-24 Table 1. Computational Complexity of Formulations A similar table was derived by Hollerbach [Hol80]. However, he arrives at an ni 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 1 was derived consider first the Lagrange approach. Determining the kinematic contribution for the Hj coefficient is linear in the number of links, n, but determining the coefficient of the 6jk 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 2.

27 Lagrange terms multiplications additions T? 32n(n-1) 24n(n-1) T)l 32n(n-1) n(n 2 1) 2T1 _ 32_ 0a^, 3 (n2-1) 8n( 2-1) F1~rP~ilP.,,.. I^m 17 l[3 17 32 209 16 3 80,1 === = * |.^E[ * - * |JW q -g^ -2^ 3 n 3n +... Trj1 | | l8n2+8n 8n2+7n (Z — )tmggrq 2n2+2n 2n2+n q=j Total 83+ +5n + 58n2- 4 6Ta2 2. Bre3 3 L Table 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 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. 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. (We noted earlier that the Newton-Euler formulation provided greater physical insight into the problem.) The Newton-Euler and the modified NewtonEuler computations are broken down in Table 3 and 4. Many of the arithmetic operations tabulated in Table 4 can be performed concurrently. Table 5 presents the number of steps required to perform the

28 modified Newton-Euler equations if this concurrency is accounted for. This line of inquiry will be pursued in a future report. N-E terms multiplications additions wci 9n 7n a9 9n 9n Al 6n 9n 8a 18n 15n ~ai 9n 9n r fj 12(n-1) 9(n-1) Iiaji 9n 6n 6i x (IItlJ) 15n 9n m(rl+Pj) x al 6n 3n rix RtI+lf+i 6n 3n RJ+nis+R 9n 6n add n 0 15n Total 108n-12 lOOn-9 Table 3. Breakdown of Newton-Euler Terms

29 N-E terms multiplications additions jt 89n 7n ai 9n 9n Ai 6n 9n a, 18(n-l) 15(n- 1) mii 12n 9n: ts 9(n-1) 9(n-1) TrIAKt(o )tj 6n 3n m(rl+-) x A 6n 3n Tri x ]Ri"fiC, B6n 3n li+i+l 9n 6n add 1 O 15n Total 9'n-27 88n-24 Table 4. Breakdown of modified Newton-Euler Terms

30 N-E terms multiplications additions Qi 9 7 Oal9 9 Al 6 9 tog 9 7 as 9 9 i+2. atZ, Al+, ai, 18n 15n mil, TriAiKtoul, mtXi-i x a-i fi, ni, ri x Rtl +l, n 12n R+ n+li+1 ji~:~".... - ~ ^........... _............. ~ —------------ Total 27n+42 27n+41 Table 5. Simultaneous steps in N-E computation. It was noted earlier in Section 2 that some applications require the torque to be in the form given by Eqn. 2. 11, viz: T-M(t^)t +eC(, )+G(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 technique outlined at the end of Section 3. The number of calculations involved will be of the form, kn(n +l) dn(n+) for the M matrix and of the form 2 2 k.n(n+l)(2n+l) d2n(n+-) for the C1 matrices. (Symmetry of the M and C' m 2 matrices has been taken into account)

31 7. APPLICATION TO SIMULATION The improved Newton-Euler together with the strobing technique proposed at the end of section 3 can be used to perform efficient simulations. An important use for simulation is in the evaluation of arm control strategies. In simulating the arm we are given an input torque vector T(t) and initial values of the relative angular velocity vector, (0) and relative angular position vector,' (0), and are required to determine the resulting' (t),' (t), and' (t). Solving Eqn. 2.11 for, (t), we have an expression of the form: =M(' )-'[T-C(, )-G ( )]=f(4,a) If' is represented by 7, we have a system of 2n equations (where again n is the number of links): i yf(''5) i =7 Now that we have the equations in this form we can perform a standard Runge-Kutta four point integration. Assume that y represents the augmented 2n vector (7,')t and further assume that g represents the augmented 2n vector (f,7)t. The equations become simply: *=g(y) In the four point technique the function, g, is computed four times each step of the simulation to determine values, hi given below: h1= tg(yn) h -eg(yn+ 4h1) bh e g (y. + -bhz) h4=cEg(yn+h3) These h terms are then weighted and summed to determine the next incremental value of y: n+ =Yn+;-(h1 + 2h2+ 2h + h4) The new values of y are, of course, just the new values of'5 and'. The step size e can be determined once one knows the maximum possible change in d or t, 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 g 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 the M matrix, invert it, and then solve for 4. The Lagrange equations yield an explicit form for the M, C and G matrices, but as was shown in the previous section, they are of order n3. Instead the following approach works better. First one obtains the combined C+G contribution by zeroing the the d input vector and inputing the A and f vectors present in the y, y+ -hl, y+ —h2, or y+hs input vector (depending on which point is being evaluated). This contribution is subtracted from r. Next one inputs zero gravity and also

32 zeroes out the d contributions present in y, y+,-h, y+ -bh2, or y+hs, but retains 2 2 the 9 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 involves operations of the order kl-n —+dlnn — 4-kn+d 2n One can take 2 2 advantage of the symmetry of M (see Section 2). Inverting M can be done by Gaussian elimination with no pivoting necessary since M is a positive definite matrix. [HoTBO] i can be found using: I =M-'(r-C-G) The resultant s and a are determined from the input r using Runge Kutta four point integration discussed above.

33 8. CONCLUSION A thorough investigation of the Lagrange and Newton-Euler arm formulations was performed. They were shown to be consistent. 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 Section 3 allows one to identify the inertial and coriolis-centrifugal matrices used in an explicit representation of the torques without recourse to the Lagrangian formulation.

34 9. BIBLIOGRAPHY [B3eJ74] Bejczy, A. K., "Robot Arm Dynamics and Control," Technical Memo 33669, Jet Propulsion Laboratory, February 1974. [DeH56] Denavit, J., R. S. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices," Journal of Applied Mechanics, pp. 215-221, Jun. 1955. [HoBO] 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, Nov. 1980 pp. 730-736. [HoT80] Horowitz, R., M. Tomizuka, "An Adaptive Control Scheme for Mechanical Manipulators- Compensation of Nonlinearity and Decoupling Control," Dynamic System and Control Division of the A.S.M.E., Winter Annual Meeting, Chicago, III., 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 Intelligence Laboratory Memo AM-1 77, Nov. 1972. [Sym7l] Symon, K. R., Mechanics, 3rd addition, Addison-Wesley, 1971.

I APPENDIX The following is a possible strategy for computing the Lagrange equations in order n3 operations. First build up all the transformations Tr. Product pairs are of the form: TdT2 T2TR TjT.' TnTnl n-1 T multiplications Triples can be built up as: TdT2T T2Tr,T Tn2Tn-lTn n-2 T multiplications 0 1 2 1L2 S' 3n- n-! Thus to compute all the Tk's takes E (n-m) matrix multiplications or 32n(n-1) mal multiplications and 24n(n-1) additions. HTb One can compute the in the following way. QoTo takes 1 Q multiplication oTo2 takes 1 Q multiplication TJIQT2 takes 1 Q and 1 T multiplication Q8T3 takes 1 Q multiplication TIQTa? takes 1 T multiplication T8QItT takes 1 Q and 1 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 interchange.) The total number of calculations is E (m-1) T multiplications or 32n(n-l) multiplications and 24n(n-1) additions. The A terms can be computed using the T1 and,fragments as the examples below: Q8Q8ToJ takes 1 Q multiplication Q8TQRTE takes 1 Q multiplication To'QzQIT2 takes 1 Q and 1 T multiplication Q8QToJQT takes 1 Q multiplication TQOITQIT takes 1 Q and 1 T multiplication QfTJQT? takes 1 Q multiplication

ii Q8TRQjTj takes 1 Q multiplication T'IQIWQT~T takes 1 T multiplication T&Qx'TQ2TI takes 1 T and 1 Q multiplication T26Q2Q2T takes 1 T and 2 Q multiplications For each link i It takes - T multiplications. If this is summed over all the ~32~2 links, we have 32 (ne-1) multiplications and 16n(n2-1) additions. TTo produce T.. To produce 5=-J- j takes 16 q multiplications and 16(q-1) additions. j=1 j _.(SL9l-multiplication. Since 61',2TA _2TA To produce pairs'j)k takes multiplications. Since 2 0c6jaaO j aljdiji, e2T1q.. in order to produce J l 5-. — jOk takes an additional 16q(2 multiplications 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. To produce the kinetic term: 0%9 8 J I68 0160J takes 8 lq(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+ 1) +64] = -n(n+l)(n+2)+64n multiplications q=l [16q(q+ 1) +32]= 32n+ 1(n2-1) additions q=i Now pick a value for i. To produce: TrLj| |V q$! for q=i to n takes 16(n-i+1) multiplications and 15(n-i+l) additions since we are only interested in the diagonal terms. Besides this there is a cost for summing the terms: n r 1 of n-i additions. Summing contributions for all i's results in: 16(n-i+l)=8n(n+1) multiplications ai1 and E 15(n-i+ 1)+n-i=8n2 +7n additions lcl

iii In the gravity terms: ti OTt grq BTiq only one component of.-.mqga 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+1) additions. Summing this for all i yields 2n2+2n mul-.... ~+3 2.. n2 n. tiplications and i-n+i-n additions. Add to this L- -additions to sum up the 2 2 2 2 results. Summing all contributions gives us a total of: -n-3+ 2 + 5n total additions 4 64 8 + 58n2- -n total multiplications a a These are the results reported in Tables 1 and 2.