O(n) mass matrix inversion for serial manipulators and polypeptide chains using Lie derivatives Kiju Lee, Yunfeng Wang and Gregory S.

Transcription

1 Robotca 7) volume 5, pp Cambrdge Unversty Press do:7/s Prnted n the Unted Kngdom On) mass matrx nverson for seral manpulators and polypeptde chans usng Le dervatves Ku Lee, Yunfeng Wang and Gregory S Chran Department of Mechancal Engneerng, Johns Hopns Unversty, Baltmore, MD 8, USA Department of Mechancal Engneerng, he College of New Jersey, Ewng, NJ 868, USA Receved n Fnal Form: August 7, 7 Frst publshed onlne: September 8, 7) SUMMARY Over the past several decades, a number of On) methods for forward and nverse dynamcs computatons have been developed n the multbody dynamcs and robotcs lterature A method was developed by Fxman n 974 for On) computaton of the mass-matrx determnant for a seral polymer chan consstng of pont masses In other of our recent papers, we extended ths method n order to compute the nverse of the mass matrx for seral chans consstng of pont masses In the present paper, we extend these deas further and address the case of seral chans composed of rgd-bodes hs requres the use of relatvely deep mathematcs assocated wth the rotaton group, SO3), and the specal Eucldean group, SE3), and specfcally, t requres that one dfferentates real-valued functons of Legroup-valued argument KEYWORDS: Seral manpulators; Mass-matrx nverson; Forward dynamcs; Polymer chans; Polypeptdes Nomenclature M G G H G q [q,,q N ] Soft varables Hard varables s [s,,s f ] h [h,,h r ] m I R, p ) the constraned mass matrx the unconstraned mass matrx the nverted mass matrx of the unconstraned system the vector of generalzed coordnates varables defnng allowable moton of chans wth nematc constrants constraned varables of the chan the vector of soft varables the vector of hard varables the th pont mass or the mass of the th rgd body the moment of nerta of the th rgd body n a frame attached at the center of mass the rgd-body transformaton from the global reference frame to the Denavt-Hartenberg D H) frame SEN), R SON) and p IR N for N or N 3 * Correspondng author E-mal: gregc@huedu c R c, pc ) the transformaton from the global reference frame to the th center of mass Q R,p A B ) A B screwe,θ,x) I I mii 3 IR 6 6 J IR 3 3 J IR 6 6 the homogeneous transformaton from to c ) the relatve transformaton from the D H frame to the D H frame for any square matrces A and B a transformaton made by the rotaton θ about the axs and the translatonal dsplacement, x, along the same axs the nerta matrx n a frame at the center of mass of a rgd body the Jacoban assocated wth parameterzed rotatons the Jacoban assocated wth parameterzed rgd-body transformatons II dentty matrx zero matrx l l zero matrx Introducton Seral chans consstng of n rgd bodes connected wth rotatonal or prsmatc onts have been studed for many years he frst On) algorthm for dynamcs computaton was developed n the multbody systems lterature n 975 In the robotcs area, the Luh Waler Paul recursve Newton Euler approach has been a cornerstone of manpulator nverse dynamcs for many years Another On) algorthm wthn a Lagrangan dynamcs settng was presented n 3 In addton, recursve technques from lnear flterng and smoothng theory for seral manpulators were ntroduced for both the forward and nverse dynamcs problems 5,6 In Ref [6], two recursve factorzaton methods of the mass matrx were presented for fxed-base and moble-base manpulators: Newton Euler factorzaton and nnovatons factorzaton As another approach, a decomposton method usng analytcal Gaussan Elmnaton GE) of the nerta matrx 7 and a recursve forward dynamcs algorthm for open-loop, seral-chan robots 8 were presented by Saha Hs algorthm has On) computatonal complexty and s also

2 74 Seral manpulators based on reverse GE appled to analytcal expressons of the elements of the nerta matrx It bulds on the Natural Orthogonal Complement for the manpulator mass matrx developed by Angeles and Ma 9 In a seres of papers, 4 Anderson and hs colleagues presented a numercal analyss and smulatons of mult-rgd-body dynamc systems An On + m) algorthm for multbody systems wth arbtrarly many closed loops, contanng n generalzed coordnates and m ndependent constrants, was presented n Ref [5] Featherstone showed a new effcent factorzaton of the ont space nerta matrx JSIM) for branched nematc trees 6,7 A coordnate nvarant algorthm for forward dynamcs usng Le groups and Le algebras was ntroduced n [8] More recently, a recursve On) forward dynamc computatons was used to obtan a set of Hamltonan equatons for openloop and closed-loop multbody systems 9, Interestngly, all of these approaches appear to be unaware of developments n the polymer physcs lterature n whch Fxman developed an On) method for computng the mass-matrx determnant of a seral chan structure composed of rgd lns and pont masses In a seres of recent conference papers, we extended Fxman s method to yeld a new method for On) nverson of the mass matrx for planar seral manpulators and polymer chans consstng of pont masses,3 In Ref [4], we examned chans of rgd bodes he nverse of the constraned mass matrx M )s obtaned by computng the nverse of the unconstraned mass matrx H ) composed of four bloc matrces whch appear to be sparse and band-lmted due to the specal propertes of the seral chan structure Usng these propertes, M s calculated by M H H H ) H ) where H s are bloc matrces of H hs form s nown as the Schur complement 5 he man dfference of our wor and others that use the Schur complment s that we adapt Fxman s method of parttonng generalzed coordnates nto soft and hard varables hs parttons H nto four sparse and band-lmted matrces nstead of usng the mathematcal manpulatons n Ref [5] In order to do ths, the seral chan s vewed as a collecton of rgd bodes, and the constrants between them that defne the onts are wrtten as functons on SE3) hese functons are then dfferentated usng the approprate concept from Le theory In contrast to other Legroup-based approaches where dynamcal phenomena on the groups of nterest are consdered, we perform operatons on the space of dfferentable functons on the group he rest of the paper s organzed as follows In Secton, we brefly revew Fxman s method and our extenson to solve Mx b for gven M and b n On) tme for n- ln seral chans consstng of n pont masses Further extensons to rgd-body applcatons are descrbed n Secton 3 In Secton 4, we explan how to use the algorthm n detal, and nclude numercal examples for the PUMA 56 robot arm and a polypeptde chan he Denavt Hartenberg D H) parameterzaton s used to descrbe rgd-body motons for the examples Fxman s heorem and Effcent Inverson of the Mass Matrx Bacground Gven a set of n pont masses {m,,m n } wth a correspondng set of absolute postons {x,,x n }, we defne the 3n-dmensonal composte poston vector as x [x,,x n ] IfN generalzed coordnates, q,,q N, are used to parameterze x, then the partal dervatves of x wth respect to the N generalzed coordnates can be arranged n the 3n N Jacoban matrx: q q [ ] q q N q q n q n q If we defne, for m m,,m n ), q N n q N IR 3n N ) m II 3 [dagm)] m II 3 IR 3n 3n, m n II 3 then the generalzed mass matrx s gven by [ ] [ ] G [dagm)] IR N N 3) q q In the case when no constrants are mposed, N 3n, and all of the matrces n Eq 3) are square and nvertble almost everywhere herefore, t follows that, for m /m,,/m n ), H [ q ] [dag m)] [ ] q 4) Recall that the dervatve of a scalar-valued functon of vector-valued argument, f z) wth z IR n, wth respect to ts argument s a row vector, f z [ f z,, f ] z n hs means that q q q n [ ] q q q q n q N q N q N n where each entry n the above matrx s a 3-dmensonal row vector For general nvertble matrces A and B, AB) B A and A ) A )

3 Seral manpulators 74 Fxman s method and extenson to solve Mx b We begn by ntroducng Fxman s heorem to the robotcs communty and showng how extensons of Fxman s results can be used to effcently nvert the expresson Mx b, where M s the mass matrx for a seral chan composed of pont masses and b s any gven vector wth matchng dmenson Gven an n-ln seral chan wth pont masses, the vector of generalzed coordnates s parttoned nto the vector of soft varables and the vector of hard varables, such that q [s, h ] where s [s,,s f ] and h [h,,h r ] when f + r 3n hen, Eqs 3) and 4) are also represented as parttoned matrces, such that ) ) G G H H G G ; H G H H Now we consder the fast nverson of G of the equaton x G b 5) where G M) s the mass matrx for a seral chan wth constrants In general, G s a full matrx, and thus, the drect numercal nverson of G requres On 3 ) computatons By usng the fact that ) ) ) G G H H II G H G G H, H II G s now computed usng blocs of H as G H H H H 6) hs s nown as a form of the Schur complement Instead of Eq 5), our approach wll be to solve x H H H H ) b, 7) where each bloc matrx of H s calculated as follows: [ s H [ s H [ h H ] [dag m)] ] [dag m)] ] [dag m)] [ ] s ; [ h [ ] h ] H ; 8) For seral manpulators, the matrces n Eq 8) can be computed effcently due to ther structural propertes In the case of the planar n-ln seral chan wth constraned ln lengths, the vector of generalzed coordnates s parttoned as q [θ,,θ n,l,,l n ], where θ s are the ont angles soft varables) and L s are the ln lengths hard varables) he analytcal expressons for soft and hard varables are wrtten as some functons of related poston vectors, such that θ f x, x, x ) and L gx, x ) Snce θ / except of when,, and L / except of when,, there are 3n ) nonzero elements n [ s n n ] IR and n ) nonzero entres n [ h ] IR n n, notng that each element s a row vector If matrces have On) nonzero entres, matrx multplcaton can be made n On) computatons by extractng zero elements Once all the blocs of H whch are also sparse and band-lmted) are computed, Eq 7) can be calculated n On) as well here are On) algorthms to solve H c for some vector c wth a matchng dmenson6 he computatonal steps for an n-ln spatal manpulator composed of n pont masses are summarzed as follows: I Defne a parttonng of the generalzed coordnates as q [s, h ] where s [s,,s f ] s the vector of soft varables and h [h,,h r ] s the vector of hard varables, such that f + r 3n II Obtan analytcal expressons for each varable n terms of poston vectors, such that s f x) and h gx), where s and h denote the th soft varable and the th hard varable, respectvely, and f and g are some functons of poston vectors 3 III Compute all nonzero entres of [ s ] IR f 3n and [ h ] IR r 3n here are On) nonzero elements for each matrx he elements of these matrces are: [ ] s s IR 3 ; [ ] h h IR 3 IV Compute H, H H ), and H usng Eq 8) Recall that matrx multplcatons for band-lmted matrces can be done n On) by extractng zero elements from the matrces For example, the sparse ) command n Matlab stores all nonzero entres of a sparse matrx as an array) V Compute Eq 7) as follows: Fnally, c H b d H c e H d f H b G ) b f e Multplcaton of a sparse matrx wth On) nonzero entres wth a vector wth the correspondng sze requres On) computatons Note that H s also sparse and band-lmted, and, therefore, H c can be computed usng an On) algorthm, such as LU decomposton 6 3 Extenson to Rgd Bodes Whle Fxman s theorem represents a clever nsght nto how to drectly explot the seral nature of a chan consstng of pont masses, the mathematcs requred s nothng more than multvarable calculus hs s because the postons of pont masses are quanttes that belong to IR 3, and For spatal chans wth rgd-bodes, f + r 6n 3 In the rgd-body case, each generalzed coordnate wll be wrtten as a functon of homogeneous transformatons, such that s f, )andh g, )

4 74 Seral manpulators tang gradents n ths space s a common mathematcal operaton In contrast, t s not at all clear wthout nvong hgher mathematcs how to do the same for rgd bodes In other words, whereas t maes sense to compute gradents of the form / where x IR 3, and the unconstraned Jacoban [/q] n Eq ) s square, when consderng rgd bodes, would t mean anythng to compute /R where R SO3)? Also, the dmensons of the assocated Jacobans would certanly not be square gven that rotaton matrces have nne elements and only three free parameters Hence, n ths secton we address how to compute dervatves n an approprate way for functons of rotatons and rgdbody motons n order to extend Fxman s approach 3 Rotatons and sew-symmetrc matrces o begn, recall that f R s a rotaton matrx, then R R II 3, and and so d dt R R) d dt II 3) 3, R Ṙ Ṙ R R Ṙ) Due to the sew-symmetry of ths matrx, we can wrte ω R Ṙ), where the operator s defned by S) s where s [s,s,s 3 ] and S s 3 s s 3 s S s s Any 3 3 sew-symmetrc matrx, S, can be wrtten as S 3 s E where E E E 3 hese can be wrtten n a vector form as E ) e, such that e e e 3 hen t follows that s 3 s e snce the operaton s lnear We wll use ths fact later If the vector ω s the angular velocty as seen n a bodyfxed frame of reference, the netc energy of a rgd-body s then K mẋ ẋ + ω Iω 9) where I s the constant moment of nerta matrx as seen n the specfc body-fxed frame wth orgn at the center of mass, and x s the poston of the center of mass of the rgd body as seen n a space-fxed frame of reference he followng subsectons develop the mathematcal framewor needed to handle the rotatonal contrbuton to netc energy n our extenson of Fxman s theorem 3 Jacobans assocated wth parameterzed rotatons When a tme-varyng rotaton matrx s parameterzed as 4 Rt) Aq t),q t),q 3 t)) Aqt)), then by the chan rule from calculus, one has Ṙ A q q + A q q + A q 3 q 3 Multplyng on the left by R and extractng the dual vector from both sdes, one fnds that: 7 where J Aq)) ω J Aq)) q ) [ A A ), A A ), A A ) ] q q q 3 ) whch s called the body Jacoban When usng the ZXZ Euler angle parameterzaton α, β, γ ), the Jacoban s wrtten explctly as: 7 sn β sn γ cos γ J sn β cos γ sn γ ) cos β 33 Dfferental operators for SO3) Let A SO3) be an arbtrary rotaton, and f A) be a functon that assgns a real or complex number to each value of A In analogy wth the defnton of the partal dervatve or drectonal dervatve) of a complex-valued functon of IR N - valued argument, we can defne dfferental operators whch act on functons of rotaton-valued argument: f A) lm ξn ɛ ɛ [f A A nɛ)) f A)] df A A nt)) dt 3) t where A n θ) denotes a counterclocwse rotaton by an angle θ around an axs defned by the unt vector n In the above defnton, the dummy varable ξ s ntroduced to emphasze that the dervatve s not wth respect to n, but rather the dervatve along a coordnate defned by the drecton n 5 Note that for a small value of θ, A n θ) expθn) II + θnii + θn E + n E + n 3 E 3 ) 4 We use the dfferent symbols Rt) and Aq), because these functons have dfferent arguments even though Rt) Aqt)) 5 In the Le theory lterature, the dervatve f/ξ n would be denoted Nf, whch may be confusng for an engneerng audence

5 Seral manpulators 743 We now fnd the explct forms of the operators n ξ n any 3-parameter descrpton of rotaton A Aq,q,q 3 ) Expandng Eq 3) n a aylor seres n ɛ and usng the classcal chan rule, one wrtes f A) ξ n 3 f A) q r q ɛ ɛ where {q r} are the parameters such that Aq,q,q 3 ) A n ɛ) Aq r,qr,qr 3 ) he r denotes the fact that each q s perturbed by multplcaton of Aq) on the rght by A n ɛ) At ths pont, the coeffcents qr ɛ ɛ s not nown, but can be determned by observng two dfferent-loong, though equvalent, ways of wrtng the product A A n ɛ)for nfntesmally small ɛ: A A n ɛ) A + ɛa N A + ɛ 3 A q r q ɛ ɛ he frst equalty results from drect multplcaton of A and A n ɛ) II + θn, and the second equalty results from expandng Aq r ) n a aylor seres about ɛ From the above equaton, we have that N 3 A A q r q ɛ or, usng the lnearty of the operator, n N) 3 A A q, ɛ ) q r ɛ, ɛ whch s rewrtten usng the defnton of the Jacoban Eq )) as n J qr ɛ ɛ hs allows us to solve for q r ɛ J n ɛ For example, f A s parameterzed wth ZXZ Euler angles, J s the Jacoban calculated n Eq ), and ts nverse s sn γ/sn β cos γ/sn β J cos γ sn γ cot β sn γ cot β cos γ Mang the shorthand notaton the ZXZ Euler angles ξ e ξ, we then wrte for cot β sn γ ξ γ + sn γ sn β α + cos γ β ; cot β cos γ ξ γ + cos γ sn β ξ 3 γ α sn γ β ; he exact form of the dfferental operators wll depend on the specfc parameterzaton used Each dfferent parameterzaton wll result n dfferent concrete forms of these abstract operators 34 Infntesmal motons and assocated acobans For small motons, the matrx exponental descrpton of a rgd-body moton s approxmated well when truncated at the frst two terms: exp [ v ) ] t II + ) v t 4) Here and ω descrbe the rotatonal part of the dsplacement Snce the second term of the rght sde n Eq 4) conssts mostly of zeros, t s common to extract the nformaton necessary to descrbe the moton as ) v ω IR v) 6 hs sx-dmensonal vector s called an nfntesmal screw moton or nfntesmal twst he fact that we have used the operaton to extract a sx-dmensonal vector from 4 4 screw matrces as well as usng t to extract a threedmensonal vector from 3 3 sew-symmetrc matrces should not be a source of concern, snce ts use wll always be clear from the context Gven a homogeneous transform ) Rq) bq) q) parameterzed wth q [q,,q 6 ] IR 6, one can express the homogeneous transform correspondng to a slghtly changed set of parameters as the truncated aylor seres 6 q + δq) q) + δq q) q hs result can be shfted to the dentty transformaton by multplyng on the left by to defne an equvalent relatve nfntesmal moton: ω J q) q v) where J q) [ ) ) ],, 5) q q 6 35 Dfferental operators for SE3) he dfferental operators / ξ for,,6 actng on functons on SE3) are calculated smlarly wth the case of SO3) For small translatonal and rotatonal dsplacements from the dentty along or about) the th coordnate axs, the homogeneous transforms representng nfntesmal motons are gven by ɛ) expɛẽ ) II ɛẽ

6 744 Seral manpulators where Ẽ Ẽ Ẽ 3 Ẽ 4 Ẽ 5 Ẽ 6 he tlde symbol s used to dstngush the SE3) bass elements from those for SO3) It s often convenent to wrte the SE3) bass elements n a 6 vector form as Ẽ ) ẽ, such that all elements are zeros except for the th element wth Gven that elements of SE3) vewed as homogeneous transforms) are parameterzed as q), the dfferental operators tae the form f ) lm ξ ɛ ɛ [f ɛ)) f )] df II + tẽ )) dt 6) t Snce and ɛ) are 4 4 matrces, we henceforth drop the notaton snce t s understood as matrx multplcaton In analogy wth the SO3) case, we defne q r, such that q) ɛ) q r, ), and we observe for the case of + ɛ Ẽ + ɛ r, 6 q q ɛ ɛ ξ that In analogy wth the SO3) case, these are two equvalent ways of wrtng ɛ) Subtractng and multplyng on the left of ths expresson, we then have that or Ẽ ẽ 6 6 r, q, q ɛ ɛ q ) q r, ɛ, ɛ whch s wrtten as ẽ J q) dqr, dɛ ɛ where J s the SE3) Jacoban defned n Secton 34 hs allows us to solve for dq r, dɛ J ẽ, ɛ whch s used to calculate r, f 6 f q ξ q ɛ as ɛ ) f f J ξ ẽ ) 7) q 36 Extenson to chans of rgd bodes Eq 9) can be rewrtten as K ) ω, v ω )I v q J IJ q he mass matrx s Gq) J IJ, and thus, the nverse of the mass matrx for a sngle rgd body s H q) J I J where I I mii 3 ) s the 6 6 nerta matrx he nverse of the mass matrx can be rewrtten usng the dervatves defned n the prevous secton Partcularly, f we defne the SE3) gradent of a functon to be [ ] f f ξ f,,, ξ ξ 6 then we can apply ths gradent to the parameters q [q,,q 6 ] used to parameterze a moton Usng Eq 7) and observng that q II, we fnd that q [ ] q ξ J 8) hs means that the nverse of the mass matrx for a sngle rgd-body can be wrtten n the Fxman-le form: [ ] [ ] q q H q) I 9) ξ ξ For a collecton of n rgd bodes, the confguraton space s SE3)) n SE3) SE3) SE3) Each rgd body has sx degrees of freedom descrbed by twsts, the th of whch s ξ IR 6, and can be descrbed alternatvely by the sx parameters q IR 6 Composte vectors ξ [ ξ,, ξ n ] IR 6n and q [s, h ] IR 6n can be formed hen, the nverse of the Jacoban s computed as J J [ s ξ [ h ξ ] ] IR 6n 6n s the nverse of the unconstraned seral chan when the generalzed coordnates are parttoned nto the soft and hard varables Hence, t dffers from the drect sum of the nverse of Jacoban matrces, J Jn, where J denotes the nverse Jacoban of the th rgd-body he nverse of the

7 Seral manpulators 745 Fg c s the homogeneous transformaton from the global reference frame {S} to the center of mass of the th rgd body, and s the one from {S} to the frame Q s the relatve transformaton from to c unconstraned mass matrx for ths collecton of rgd bodes s then of the form H q) J [ I In ] J ) Everythng then follows usng the extenson of Fxman s theorem as n the pont-mass case, wth Eq ) replacng Eq 4) he same parttonng nto soft and hard varables and the same On) performance results 4 Examples Examples of an n-ln planar revolute manpulator and a polymer chan composed of pont masses at each ont are presented n our earler papers,3 In ths secton, we descrbe how to use our extenson of Fxman s algorthm for chans of rgd-bodes and demonstrate wth a PUMA 56 robot arm and a polypeptde chan he homogeneous transformatons,, c and Q,are defned as shown n Fg Usng the facts that c Q and due to the seral nature, we have that ) Q c ) Q ) Snce s wrtten as a functon of c and c, n order to dfferentate c, we use the dfferental operators for SE3) descrbed n Secton 35 hen, we have the Le dervatves of Eq ) gven by 6 ξ 6 For SE3), Q c Q Ẽ c ) Q, Ẽ Q,, otherwse ) II 4 d dt + d ) dt 4 d ) dt d dt ) In our algorthm, we compute the nverted Jacoban of an unconstraned system to get the nverse of an unconstraned mass matrx, e, G J I J However, most manpulators have sngulartes where the Jacoban matrx s not nvertble In contrast, the mass matrx for most manpulators s nvertble at all values of the generalzed coordnates, and therefore, there always exsts G he problems related wth sngulartes can be mostly elmnated by choosng the parameterzaton carefully or usng more than one method for assgnng frames, because the Jacobans computed for dfferent parameterzatons may have sngulartes n dfferent locatons We begn by descrbng rgd-body motons usng D H framewor n the followng secton 4 Denavt Hartenberg parameterzatons A screw transformaton s a combned rotaton and translaton along a common axs In partcular, screwn, d, θ) An θ) ) dn 3 where n IR 3 s any unt vector D H parameterzaton s a method for assgnng frames of reference to a robot arm constructed of rotatonal onts connected wth rgd lns he relatve transformaton from the D H frame tothe D H frame appear as two screw motons, such that screwe,b,β ) screwe 3,c,γ ) cγ sγ b sγ cβ cγ cβ sβ c sβ sγ sβ cγ sβ cβ c cβ 3) where c ) cos ) and s ) sn ) are used for a shorthand wrtng Here e s are as defned n Secton 3 he ln parameters are defned as follows: 8 b, the dstance from Ẑ to Ẑ measured along ˆX, β, the counter clocwse measured angle between Ẑ and Ẑ measured about ˆX, c, the dstance from ˆX to ˆX measured along Ẑ, and γ, the counter clocwse measured angle between ˆX and ˆX measured about Ẑ he above four parameters descrbe constraned motons n SE3) In other words, we need sx parameters to descrbe a full rgd-body moton n SE3), but we only have four D H parameters herefore, we mpose two dummy varables whch wll be set to be zeros as constrants) to the D H parameters as follows Equaton 3) can be obtaned by settng α a from ether A screwe 3,a,α ) screwe,b,β ) screwe 3,c,γ ), 4) B screwe,b,β ) screwe 3, c,γ ) screwe,a,α ), 5)

8 746 Seral manpulators or C screwe,b,β ) screwe,a,α ) screwe 3,c,γ ) 6) If J A, J B and J C are the Jacoban matrces of Eq 4), 5), and 6) respectvely, then the determnant of each Jacoban s calculated as detj A ) sn β ; detj B ) sn γ ; detj C ) cos α J A becomes sngular when sn β, J B becomes sngular when sn γ, and J C becomes sngular when cos α In the D H parameterzaton, we set α, and therefore, cos α for all Hence, we choose Eq 6) to descrbe homogeneous transformatons of rgd bodes n SE3) We frst need to fnd analytcal expressons for generalzed coordnates n terms of homogeneous transformatons If the relatve transformaton from the frame to the frame s represented by three screw motons as Eq 6), t can be wrtten as C cα cγ cα sγ sα b + c sα sβ sα cγ + cβ sγ cβ cγ sβ sα sγ sβ cα a cβ c sβ cα cβ sα cγ + sβ sγ sβ cγ + cβ sα sγ cβ cα a sβ + c cβ cα 7) Mang the shorthand notaton, C and denotng the, l)th element of C by l, when α [ π, π ], β [ π, π] and γ [ π, π], the generalzed coordnates can be extracted as: 7 3 α atan + 3 β atan, + γ atan, + a ; b ; c In Matlab M, two functons compute the nverse of the tangent, atan and atan atanz) returns the nverse tangent of z For real z, atanx) s n the range [ π/,π/] atany, x) gves the value of θ, such that sn θ y and cos θ x hevalueofθ les n the nterval [ π, π] In Mathematca M, the same functons are defned as Arcan[z] and Arcan[x, y], respectvely If l denotes the, l)th element of, the Le dervatves ξ of the above equatons are computed as: α ξ + β ξ ; γ ξ + ; a ξ ) ) ; ) ) + ) + 3/ ; b ξ ) [ ] + ) ) + ) ) + ; c ξ ) + ) ) + 8) hese are elements of J Soft and hard varables are defned dfferently dependng on each system In the followng sectons, we present specfc examples of the PUMA 56 and a polypeptde chan 4 PUMA 56 robot arm D H parameters for the PUMA 56 arm are shown n able I he vector of generalzed coordnates s defned as q [s ; h ] where s [γ,,γ 6 ] and h [h, h,,h 5, h 6 ] wth h [α,β,a,b,c ] for,,6 Here γ denotes the th ont angle of the manpulator All necessary constants ncludng the ln mass values, the moment of nerta about the center of mass com) and the locaton of com of each ln Q s) were adapted able I D H parameters of PUMA 56 Arm 9 β [ ] b [m] c [m] γ [ ] 9 γ 438 γ γ γ γ 5 6 γ 6

9 Seral manpulators 747 from Ref [9] he soft varables are arbtrarly chosen as γ π/3 for,,6 for the purpose of testng our algorthm Based on these parameters, the homogeneous transformatons from the global reference frame to the center of mass of each ln are computed as Gven the homogeneous transformatons, nonzero elements of the followng matrces can be computed usng n Eq 8): γ ξ 6 6 γ γ ξ ξ [ ] γ s 3 γ 3 6 ξ ξ 3 ξ γ 4 γ 4 IR 6 36 ; ξ 3 ξ 4 γ 5 γ 5 ξ 4 ξ 6 5 γ 6 6 γ 6 6 ξ 5 ξ 6 h ξ h h ξ ξ [ ] h h 3 h ξ ξ 3 ξ h 4 h 4 IR 3 36 ξ 3 ξ 4 h 5 h 5 ξ 4 ξ h h ξ 5 ξ 6 where able II D H parameters of a polypeptde chan 3 β [ ] b [Å] c [Å] γ [ ] β 75 β β β β β β γ ξ h ξ [ γ ξ α ξ c ξ, γ ξ 6 α ξ 6 c ξ 6 ] IR 6 ; IR 5 6 As shown above, [ s h ] and [ ] are sparse and band-lmted ξ ξ Once all nonzero entres are computed, H, H, and H can be obtaned usng Eq 9) We recall that the matrx multplcatons for sparse matrces can be done n On) for matrces wth On) nonzero elements by extractng all zero entres Fnally, we can solve Eq 7) n On) by followng the steps descrbed n Secton Numercal results of M and M of the PUMA 56 arm are provded n the Appendx for verfcaton of the result 43 A polypeptde chan he D H parameters for a polypeptde chan are shown n able II Frames are attached to each atom n the bacbone structure he man chan atoms are represented as rgd peptde unts, lned through the C α atoms he parameters, such as bond-length, b s, and bond-angle values, γ s, for a polypeptde chan are adapted from Ref [3] he offset values, c, for all, and torson angles along each bond ln, denoted by β for all,,n, are vewed as only soft varables In fact, the torson angles along C N bonds are fxed to about 8 n polypeptde chans As shown n Fg, each C α atom s connected to four atoms, C,N,H, and R CH 3 for a polyalanne chan) We assume that ths structure s a etrahedron wth C α at the center, and CH 3 s consdered as a pont mass at the locaton of C he com of l pont masses at the frame can be computed as c p M l m r where M s the sum of all pont masses and r s the poston of the th partcle seen from the orgn of the frame Q s gven by pure translaton, c p When c p for all,,n are calculated, the moment of nerta at the com can be computed, respectvely he atomc masses 8 used for numercal examples are 8 amu Atomc mass unt, defned to be / of the mass of a C- atom

10 748 Seral manpulators Fg A polypeptde chan wth alanne sde chans C 7 amu, N 4674 amu, H 794 amu, and O amu We frst consder that all torson angles, β for,,n, are soft varables hen, the vector of generalzed coordnates s defned to be q [s ; h ] where s [β,,β n ] and h [α,γ,a,b,c,, α n,γ n,a n,b n,c n ] Forn 7 contanng two rgd peptde planes), we arbtrarly choose β and β π/3 for,,7 hen, c s are gven by c c Gven the homogeneous transformaton matrces, we can compute [ s h ] and [ ] usng the analytcal expressons of ξ ξ generalzed coordnates derved n Secton 4 H, H and H can be obtaned smlarly as descrbed for the PUMA 56 arm As mentoned earler, the torson angles along the C N bonds are fxed to be about 8 herefore, some of the β s should not be treated as soft varables We now set β 3 β 6 8 he vectors of soft and hard varables can be redefned as s [β, β, β 4, β 5, β 7 ] and h [α,, α 3, β 3, γ 3,, α 6, β 6, γ 6,, b 7, c 7 ] H, H and H can be computed accordngly for gven q [s, h ] Numercal results are shown n Secton 44 and the Appendx 44 Computatonal tme he computatonal tme s hghly dependent on the computer n whch the program runs, such as the memory sze, the type of processor, the operatng system, etc herefore, one should be careful when nterpretng the result of computatonal tmes requred to run the algorthm he program to test the runnng tmes n dfferent szes of seral chans s wrtten n Matlab verson 65 and runs n a 8GH z Pentum4 computer wth Gb RAM he operatng system s Wndow XP Home edton he polypeptde chan example n Secton 43 s revsted, whle all torson angles are consdered as soft varables Fgure 3 shows the computatonal tme to nvert the mass matrx for n,,4 he dashed lne ndcates the tme to solve x M b by nvertng the n n mass matrx, M, usng the nvm) functon n Matlab, and the sold lne shows one to compute the equaton, x H H H H )b H c n the computatonal step V n Secton ) s calculated usng the LU decomposton he tme s counted usng the tc-toc functon n Matlab Snce

11 Seral manpulators 749 mathematcs requred for ths extenson have also been developed and presented Acnowledgments hs wor was supported by Natonal Insttutes of Health Grant RGM753 and Natonal Scence Foundaton Informaton echnology Research 5466 hs paper s dedcated to the memory of Eun-Jung Rhee, who as a promsng doctoral canddate n theoretcal partcle physcs at Johns Hopns Unversty would have come to use Letheoretc methods extensvely Fg 3 Computatonal tme s) vs the number of lns n): the tme requred for drect nverson of M dashed lne) and the tme requred when usng the extended Fxman s method sold lne), for n,,4 the program can be optmzed n many dfferent ways, the runnng tme cannot be vewed as an absolute measure of the computatonal speed However, t can be used as a measure of speed or effcency of an algorthm wth provded specfcatons of the computer and software n whch the program runs he graph shows that the computatonal tme lnearly ncreases Instead of countng real tme, we can count the number of operatons to verfy the On) computatonal complexty We analytcally proved that our algorthm requres On) operatons, but dd not nclude an operaton count n ths paper he number of mathematcal operatons wll vary accordng to dfferent systems and depends on how to optmze the program In, 3 the actual tme requred to compute the forward dynamcs for an open chan usng the recursve Hamltonan method s provded For n 4, the runnng tme s about 47 s Our algorthm to solve x M b for n 4 requres about 9 s as shown n the graph We note that these numbers are not drectly comparable because we do not compute the whole forward dynamc equatons as done n 3 Also, the specfcaton of the computer and software C ++ ) used to run the algorthm n Ref [3] dffer from ours 5 Conclusons More than 3 years ago, a method for On) computaton of the determnant of the mass matrx for a chan of pont masses constraned wth rgd bonds was developed by Professor Marshall Fxman Whereas ths theorem apparently has remaned unnown to the multbody and robotcs lterature, we have appled t to develop On) forward dynamcs algorthms, especally to compute the nverse of the mass matrx, n a seres of papers 4 he specfc contrbuton of ths paper s the extenson of Fxman s theorem to the case of seral chans of rgd bodes We demonstrate t on two examples: the 6 DOF PUMA 56 manpulator and polypeptde chan on several lengths he assocated References Vereshchagn, A F, Gauss prncple of least constrant for modelng the dynamcs of automatc manpulators usng a dgtal computer, Sov Phys-Dolady ), pp ) J Y S Luh, M W Waler and R P Paul, On-lne computatonal scheme for mechancal manpulators, rans ASME J Dy Sys Meas Control : ) 3 J M Hollerbach, A recursve Lagrangan formulaton of manpulator dynamcs and a comparatve study of dynamcs formulaton complexty, In: utoral on Robotcs C S G Lee, R C Gonzalez and K S Fu, ed) IEEE Computer Socety Press, Slver Sprng, Maryland, 983), pp 7 4 G Rodrguez, Kalman Flterng, smoothng, and recursve robot arm forward and nverse dynamcs, IEEE J Robot Automat RA-3 6), ) 5 G Rodrguez and K Keutz-Delgado, Spatal operator factorzaton and nverson of the manpulator mass matrx, IEEE rans Robot Automat 8), ) 6 A Jan and G Rodrguez, Recursve flexble multbody system dynamcs usng spatal operators, J Gud Control Dyn 56), ) 7 S K Saha, A decomposton of the manpulator nerta matrx, IEEE rans Robot Automat 3), ) 8 S K Saha, Analytcal expresson for the nverted nerta matrx of seral robots, Int J Robotcs Research 8), ) 9 J Angeles and O Ma, Dynamc smulaton of n-axs seral robotc manpulators usng a natural orthogonal complement, Int J Robot Res 75), ) K S Anderson and S Duan, Hghly Parallelzable Low Order Dynamcs Algorthm for Complex Mult-Rgd-Body Systems, AIAA J Gud Control Dy 3), ) K S Anderson and S Duan, A Hybrd Parallelzable Low Order Algorthm for Dynamcs of Mult-Rgd-Body Systems: Part I, Chan Systems, J Math Comput Mod 3, ) K S Anderson and S Duan, Parallel Implementaton of a Low Order Algorthm for Dynamcs of Multbody Systems on a Dstrbuted Memory Computng System, J Eng Comput 6), 96 8 ) 3 K S Anderson, An Order-N Formulaton for the Moton Smulaton of General Constraned Mult-Rgd-Body Systems, J Comput Str 433), ) 4 K S Anderson, An Order-N Formulaton for the Moton Smulaton of General Mult-Rgd-Body ree Systems, J Comput Str 463), ) 5 K S Anderson and J H Crtchley, Improved Order-N performance algorthm for the smulaton of constraned multrgd-body dynamc systems, Multbody syst dyn 9, 85, 3) 6 R Featherstone, he calculaton of robot dynamcs usng artculated-body nerta, Int J Robot Res ), ) 7 R Featherstone, Effcent factorzaton of the ont space nerta matrx for branched nematc trees, submtted to Intl Journal of Robotcs Research

12 75 Seral manpulators 8 S R Ploen and F C Par, Coordnate-Invarant Algorthms for Robot Dynamcs, IEEE rans Robot Automat 56) ) 9 J Naudet and D Lefeber, General Formulaton of an Effcent Recursve Algorthm based on Canoncal Momenta for Forward Dynamcs of Closed-loop Multbody Systems, Proceedngs of IDEC/CIE, Long Beach, CA, USA Sep 5) DEC J Naudet, D Lafeber and Z erze, General Formulaton of an Effcent Recursve Algorthm based on Canoncal Momenta for Forward Dynamcs of Open-loop Multbody Systems, Multbody Dynamcs, 3 M Fxman, Classcal statstcal mechancs of constrants: A theorem and applcaton to polmers, Proc Nat Acad Sc 78), Aug 974) K Lee and G S Chran, A New Perspectve on On) Mass-Matrx Inverson for Seral Revolute Manpulators, IEEE Internatonal Conference on Robotcs and Automaton, Barcelona, Span Apr 5) 3 K Lee, G S Chran, A New Method for On) Inverson of the Mass Matrx, Internatonal Symposum on Multbody Systems and Mechatroncs, Uberlanda, Brazl Mar 5) 4 Y Wang and G S Chran, A New On) Method for Invertng the Mass-Matrx for Seral Chans Composed of Rgd Bodes, Proceedngs of IDEC/CIE, Long Beach, CA, USA Sep 5) DEC A Fan and G M D Eleutero, Parallel Olog N) Algorthms for Computaton of Manpulator Forward Dynamcs, IEEE rans Robot Automat, 3), 995) G H Golub and C F Vam Loan, Matrx Computatons Johns Hopns Unversty Press, Baltmore, MD, USA and London, UK, 996) 7 G S Chran and A B Kyatn, Engneerng Applcatons of Noncommutatve Harmonc Analyss CRC Press, Boca Raton, FL, ) 8 J J Crag, Introducton to Robotcs, nd ed Addson-Wesley, Readng, MA, 989) 9 R P Paul and H Zhang, Computatonally effcent nematcs for manpulators wth sphercal wrsts, Internatonal Journal of Robotc Research, 5), 986) R V Pappu, R Srnvasan and G D Rose, he Flory solatedpar hypothess s not vald for polypeptde chans: Implcatons for proten foldng, Bophyscs 973), pp , ) 3 J Naudet, Forward Dynamcs of Multbody Systems: A Recursve Hamltonan Approach PhD hess Brussels, Belgum Vre Unverstet Brussel, 5) Appendx he mass matrx of the PUMA 56 arm n Secton 4 s gven by M puma he nverse of the constraned mass matrx usng the extended Fxman s algorthm n Eq 6) s computed as Mpuma Matrx multplcaton of the above two matrces yelds that M puma Mpuma II 6 We note that Eq 7) can be computed n On), but not the matrx M whle each column of M can be obtaned n On)) We provde the numercal results of M for a verfcaton purpose he constraned mass matrx of 7-ln polypeptde chan when all β s are vewed as soft varables) n Secton 4 s gven by M poly he nverse of the constraned mass matrx for the 7- ln polypeptde chan computed from H H H H s gven by Mpoly Matrx multplcaton of the above matrces yelds M poly M poly II 7