Lie Group Formulation of Articulated Rigid Body Dynamics

Transcription

1 Le Group Formulaton of Artculated Rgd Body Dynamcs Junggon Km 11/9/2012, Ver 1.01 Abstract It has been usual n most old-style text books for dynamcs to treat the formulas descrbng lnearor translatonal and angularor rotatonal moton of a rgd body separately. For example, the famous Newton s 2nd law, f = ma, for the translatonal moton of a rgd body has ts partner, so-called the Euler s equaton whch descrbes the rotatonal moton of the body. In ths artcle a smple and elegant equatons of moton of a rgd body s presented usng generalzed notatons of knematc and dynamc quanttes such as velocty, force and momentum Secton 1. Then effcent recursve algorthms for nverse and forward dynamcs of artculated rgd bodes are derved usng the notatons. A hybrd dynamcs algorthm, or a generalzed verson of the nverse and forward dynamcs algorthms, s also presented Secton 2. Fnally, analytc dervatves of the dynamcs algorthms are derved Secton Dynamcs of a Rgd Body Ths secton descrbes the equatons of moton of a sngle rgd body n a geometrc manner. 1.1 Rgd Body Moton Fgure 1: Coordnates frames for a rgd body To descrbe the moton of a rgd body, we need to represent both the poston and orentaton of the body. Let B} be a coordnate frame attached to the rgd body and A} be an arbtrary coordnate frame, and all coordnate frames wll be rght-handed Cartesan from now on. We can defne a 3 3 matrx R = [x ab, y ab, z ab ] 1 1 GEAR Geometrc Engne for Artculated Rgd-body smulaton s a C++ mplementaton of the algorthms presented n ths artcle. 1

2 where x ab, y ab, z ab R 3 are the coordnates of the coordnate axes of B} wth respect to A}. A matrx of ths form s called a rotaton matrx as t can be used to descrbe the orentatonor rotaton of a rgd body, relatve to a reference frame. Snce the columns of a rotaton matrx are mutually orthonormal and the coordnate frame s rght-handed, the rotaton matrx has two propertes, RR T = R T R = I, det R = 1, 2 and t s denoted by SO3 2. Let p R 3 be the poston vector of the orgn of B} from the orgn of A}, and R SO3 be the rotaton matrx of B} relatve to A}. The confguraton space of the rgd body moton can be represented wth the par R, p, whch s denoted as SE3. A 4 4 matrx, [ ] R p T = s called the homogeneous representaton of T = R, p SE3, and ts nverse can be obtaned wth [ ] T R = T R T p From now on, a smple declaraton, T SE3 : A} B}, wll be used to notfy that T SE3 represents the orentaton and poston of a coordnate frame B} wth respect to another coordnate frame A}. The Le algebra of SE3, denoted as se3, s dentfed as a 6-dmensonal vector space w, v R 6 where w so3, the Le algebra of SO3. ξ = w, v se3 can also be represented as a 4 4 matrx, [ ] [w] v ξ = ] where [w] = [ 0 w3 w 2 w 3 0 w 1 w 2 w 1 0 R 3 3 s a skew-symmetrc matrx. The adjont acton of T SE3 on ξ se3, Ad : SE3 se3 se3, s defned as Ad T ξ = T ξ T. 6 From 3, 4 and 5, Ad T can be regarded as a lnear transformaton, Ad T : se3 se3, whch s defned by a 6 6 matrx [ ] R 0 Ad T = 7 [p]r R where T = R, p SE3. The coadjont acton of T on ξ dse3 whch s the dual of ξ, Ad T : dse3 dse3, s defned by a 6 6 matrx 1.2 Generalzed Velocty and Force Ad T = Ad T T. 8 Let T t = Rt, pt SE3 be a moton trajectory of a coordnate frame attached to a rgd body wth respect to an nertal frame. The generalzed velocty of the rgd body s defned as [ ] V = T T [w] v = where [w] = R T Ṙ and v = R T ṗ. The physcal meanng of w R 3 s the rotatonalor angular velocty of the coordnate frame attached to the body relatve to the nertal frame, but expressed n 2 The notaton SO abbrevates specal orthogonal and specal refers to the fact that det R = +1 rather than ±1. See [2] for more detals. 2

3 the body coordnate frame. Smlarly, v R 3 represents the velocty of the orgn of the coordnate frame relatve to the nertal frame, and stll expressed n the body frame. The generalzed velocty s an element of se3, and can be smply regarded as a 6-dmensonal vector,.e., w V =. 10 v As the generalzed velocty s an nstance of se3, t follows the adjont transformaton rule defned n 7. Let A}, B} be two dfferent coordnate frames attached to the same rgd body, and T a, T b SE3 represent the orentaton and poston of the two frames wth respect to an nertal frame. Then, from 6 and 9, the generalzed veloctes of A} and B} have the followng relaton: V b = Ad Tba V a 11 where T ba SE3 : B} A}. Wth a coordnate frame attached to a rgd body, the generalzed force actng on the body can be defned as m F = 12 f where m R 3 and f R 3 represent a moment and force actng on the body respectvely, vewed n the body frame. The generalzed force s known as the member of dse3, and has the followng transformaton rule, F b = Ad T ab F a 13 where F a and F b denote a generalzed force vewed from dfferent body frames A} and B}, and T ab SE3 : A} B}. 1.3 Generalzed Inerta and Momentum The knetc energy of a rgd body s gven by the followng volume ntegral 1 e = 2 v 2 dm 14 vol whch means the sum of the knetc energes of all the mass partcles consttutng the body. By ntroducng a coordnate frame attached to the body, 14 can be restructured as the followng smple quadratc form, e = 1 2 V T IV 15 where V R 6 s the generalzed velocty of the body and I R 6 6, whch s known as generalzed nerta, represents the mass and mass dstrbuton wth respect to the body frame. To obtan an explct form of the generalzed nerta of a rgd body, let r R 3 be the poston of a body pont relatve to the body frame and R, p SE3 represents orentaton and poston of the body frame wth respect to an nertal frame respectvely. Usng v 2 = ṗ+ṙr 2, R T Ṙ = [w] and R T ṗ = v, 14 can be rewrtten as e = 1 ṗ 2 + 2ṗ T Ṙr + 2 Ṙr 2 dm 16 vol = 1 } ṗ T ṗ dm 2ṗ T R [r]dm w + w T [r] T [r]dm w 17 2 vol vol vol } mv T v 2v T w + w T = 1 2 [r]dm vol [r] T [r]dm vol = 1 2 V T IV 19 w 18 3

4 where V = w, v s the generalzed velocty of the body and the generalzed nerta, I, has the followng explct matrx form: [ vol I = [r]t [r]dm vol [r]dm ]. 20 vol [r]t dm m1 The generalzed nerta s symmetrc postve defnte, and ts upper dagonal term, I = vol [r]t [r]dm, s the defnton of the well-known 3 3 nerta matrx of the rgd body wth respect to the body frame. If the orgn of the body frame s located on the center of mass, then the generalzed nerta becomes a block dagonal matrx because [r]dm = 0. In addton, f the orentaton of the body vol frame also concdes wth the prncple axes of the body, then the generalzed nerta becomes a dagonal matrx. Let A} and B} be coordnate frames attached to a rgd body, I a and I b be the generalzed nertas of the body correspondng to the two frames. Usng 11, 15, and the fact that the knetc energy of the body should reman under change of coordnate frame, the followng transformaton rule between the generalzed nertas can be obtaned: I b = Ad T ab I a Ad Tab 21 where T ab SE3 : A} B}. If the mass, m, and the nerta matrx n a center of mass frame 3, I c R 3 3, are gven, one can get the generalzed nerta n an arbtrary body frame from 21, rather than usng 20, as [ ] RI I = c R T +m[p] T [p] m[p] m[p] T 22 m1 where R, p SE3 represents the orentaton and poston of the center of mass frame wth respect to the body frame. The generalzed momentum of a rgd body s defned as L = IV 23 where I and V are the generalzed nerta and velocty of the body expressed n a coordnate frame attached to the body. L a and L b be the generalzed momentum of a rgd body expressed n dfferent body frames, A} and B}, respectvely. Usng 11 and 21 one can derve the followng transformaton rule for generalzed momentums: L b = Ad T ab L a 24 whch s same to that of generalzed forces, and ndeed, the generalzed momentum s also known as dse Tme Dervatves of se3 and dse3 Recall that the tme dervatve of a 3-dmensonal vector x = 3 =1 x ê, expressed n a movng coordnate frame ê }, can be obtaned as d dt x = 3 =1 } d d dt x ê + x = ẋ + w x 25 dtê where ẋ = 3 d =1 dt x ê = dx 1 dt, dx2 dt, dx3 dt s the component-wse tme dervatve of x and w s the angular velocty of the movng frame. 3 A coordnate frame whose orgn s located on the center of mass of the body. 4

5 The tme dervatves of se3 and dse3 whch are expressed n a movng frame have more generalzed form. A smple approach to obtan the dervatves s to transform se3or dse3 to a statonary coordnate frame, dfferentate t there, and transform the dervatve back to the orgnal movng coordnate frame wth the correct assumpton that the dervatve of se3or dse3 can be transformed wth the rule of se3or dse3. 4 Note that dfferentatng a vector n a statonary coordnate frame wth respect to tme s just gettng the component-wse dervatve of t. Lemma 1. Let X se3 be expressed n a movng frame attached to a rgd body. Then the tme dervatve of X can be obtaned by d dt X = Ẋ + ad V X 26 where Ẋ s the component-wse tme dervatve of X and ad V : se3 se3 s a lnear transformaton defned as [ ] [w] 0 ad V = 27 [v] [w] where V = w, v se3 s the generalzed velocty of the body. Proof. Let T = R, p SE3 denote the orentaton and poston of the movng frame wth respect to an nertal frame whch s statonary n the space. By transformng X to the nertal frame, dfferentatng t there, and then transformng the result back to the orgnal body frame, one can get d dt X = Ad d T dt Ad T X = Ẋ + Ad d T dt Ad T X where d dt d represents the component-wse dfferentaton and Ẋ = dtx. Usng 7 and V = w, v = as follows: R T Ṙ, R T ṗ, one can show Ad T d dt Ad T = ad V d [ ] [ ] R Ad T dt Ad T 0 Ṙ 0 T = R T [p] R T [ṗ] R + [p] [ ] Ṙ Ṙ [w] 0 = [v] [w] Lemma 2. Let Y dse3 be expressed n a movng frame attached to a rgd body. Then the tme dervatve of Y can be obtaned by d dt Y = Ẏ ad V Y 28 where Ẏ s the component-wse tme dervatve of Y and ad V : dse3 dse3 s a lnear transformaton defned as [ ] T ad V = ad T [w] 0 V = 29 [v] [w] where V = w, v se3 s the generalzed velocty of the body. Proof. Smlarly to the proof of Lemma 1, the dervatve of Y dse3 can be obtaned by d dt Y = d Ad T dt Ad T Y = Ẏ + d Ad T dt Ad T Y. 4 In [1] the tme dervatve of a spatal velocty whch s smlar to the generalzed velocty n ths artcle was obtaned wth ths approach. 5

6 Usng 8 and V = w, v = R T Ṙ, R T ṗ, one can show Ad T d dt Ad T = ad V as follows: Ad d [ ] [ ] R T dt Ad T = T R T [p] Ṙ [ṗ] R + [p] Ṙ 0 R T 0 Ṙ [ ] [w] [v] = = ad T 0 [w] V = ad V 1.5 Geometrc Dynamcs of a Rgd Body Equatons of moton of a rgd body can be wrtten as F = d dt L 30 where F represents the net sum of the generalzed forces actng on the rgd body and L s the generalzed momentum of the body. Usng 23 and 28, the equatons of moton of the rgd body can be wrtten as F = I V ad V IV 31 where V s the component-wse tme dervatve of the generalzed velocty of the body. Note that L = İV + I V = I V because the components of I doesn t vary. The dynamcs equaton of a rgd body s coordnate nvarant,.e., the structure of the equaton stll remans under change of coordnate frame. Let A} and B} be coordnate frames attached to a body, V a and V b be the generalzed veloctes, F a and F b be the generalzed forces, and I a and I b be the generalzed nertas correspondng to A} and B} respectvely. Usng the transformaton rules, 11, 13, and 21, one can easly transform the dynamcs equatons wth respect to A}, F a = I a V a ad V a I a V a, to the equatons n B}, F b = I b V b ad V b I b V b, and ths shows the coordnate nvarance of 31 under change of coordnate frame. 2 Dynamcs of Open Chan Systems Let q R n denote the set of coordnates of all jonts n a system, and for open chan systems, n s equal to the degree-of-freedom of the system. The dynamcs equatons of the system can be wrtten as Mq q + bq, q = τ 32 where Mq R n n s a symmetrc mass matrx of the system, bq, q R n represents Corols, centrfugal, and gravty terms, and τ R n denotes torqueor force vector correspondng to the system coordnates q. Calculatng the jont torqueor force τ wth gven q, q, q s called nverse dynamcs. It s typcally used to obtan the requred jont torques whch make the system move along a gven jont trajectory. On the other hand, calculatng q wth gven q, q, τ s called forward dynamcs, and t s frequently used to smulate the system evoluton n tme by ntegratng q to get q, q at the next tme step. In general, the command nput on a jont can be ether torque or acceleraton durng the smulaton and the command type does not have to be same for all jonts. The equatons of moton can be rewrtten as M qa q p + b = τa τ p where the subscrpt a s for the acceleraton-prescrbed jonts and the subscrpt p s for the jonts wth gven, or known, torques. We can compute τ a, q p wth known q a, τ p from the equatons and 33 6

7 Inverse dynamcs q, q, q τ Forward dynamcs q, q, τ q Hybrd dynamcs q a, q a, q a, q p, q p, τ p τ a, q p Table 1: Input and output of dynamcs we call ths hybrd dynamcs 5 One possble soluton for hybrd dynamcs s to rearrange 33 and solve t wth a drect matrx nverson. For example, the acceleratons [ of the unprescrbed jonts can be obtaned by q p = Mpp Maa M τ p b p M pa q a where M = ap M pa M pp ], b = ba bp, and q = qa q p. The method, however, s not effcent for a complex system because t requres buldng the mass matrx and nvertng the submatrx correspondng to the unprescrbed jonts, whch leads to an On 2 + On 3 p algorthm where n and n p denote the number of all coordnates and the number of unprescrbed coordnates respectvely. In Table 1 the nput and output of nverse, forward, and hybrd dynamcs are summarzed. Note that hybrd dynamcs s a generalzaton of tradtonal forward and nverse dynamcs,.e., they can be regarded as the extreme cases of hybrd dynamcs when all of the jonts have gven, or known, torques and when all of the jonts are acceleraton-prescrbed respectvely. 2.1 Recursve Inverse Dynamcs A recursve Newton-Euler nverse dynamcs algorthm usng the geometrc notatons shown n Secton 1 was presented n [3]. Here the algorthm s slghtly modfed to support mult-degree-offreedom jonts n the formulaton. Let 0} be an nertal frame whch s statonary n the space, I} be a coordnate frame attached to -th rgd body of the open chan system, and λ} be a coordnate frame attached to the parent body of the -th rgd body. Also, let T SE3 : 0} }, T λ SE3 : 0} λ}, and T SE3 : λ} }. From T = T λ T and 6, the generalzed velocty of the -th body can be rewrtten as V = T T 34 = T T λ T λ T + T λ T 35 = Ad T V λ + S q 36 where S q = T T se3 represents the relatve velocty of the -th body wth respect to ts parent. S = S q se3 n s called the Jacoban of the jont connectng the -th body and ts parent and q R n represents the coordnate vector of the jont. As shown n 31, component-wse tme dervatves of the generalzed veloctes of all bodes n the system are needed to buld the dynamcs equatons for each body. Recallng that A = A AA for an arbtrary matrx A, and ad ξ1 ξ 2 = ξ 1 ξ 2 ξ 2 ξ 1 for arbtrary ξ 1, ξ 2 se3, one can derve the followng formula for V, the component-wse tme dervatve of V : V = d T dt V λt + Ṡ q + S q 37 = T T T V λt + T V λ T + T V λ T + Ṡ q + S q 38 = Ad T V λ + ad AdT V λ S q + Ṡ q + S q 39 = Ad T 5 We follow [1] for the termnology. V λ + ad V S q + Ṡ q + S q 40 7

8 36 and 40 are well suted to calculate the generalzed velocty and ts component-wse tme dervatve of each body n a open chan system from the ground to the end of the system recursvely, as the velocty and acceleraton of the ground are known n most cases. 6 Note that Ṡ 0 as the jont Jacoban S s a functon of q n general. Let F dse3 be the generalzed force transmtted to -th body from ts parent through the connectng jont, and F ext Both F and F ext be wrtten as dse3 be a generalzed force actng on the -th body from envronment. are expressed n }. From 31, the equatons of moton for the -th body can F + F ext Ad F T k = I V ad V I V 41,k k µ where the left hand sde of the equatons represents the net force actng on the body, µ s the set of chld bodes of the -th body, and Ad F T k s the generalzed force, transmtted from k-th,k chld body, expressed n }. It should be noted that the generalzed force, F, for each body can be calculated by 41 from the ends to the ground recursvely, as the end bodes have no chld. A recursve nverse dynamcs algorthm for open chan systems s shown n Table 2, and the followng s a lst of symbols for the geometrc nverse dynamcs: = ndex of the -th body. λ = ndex of the parent body of the -th body. µ = set of ndexes of the chld bodes of the -th body. q R n τ R n = coordnates of the -th jont whch connects the -th body wth ts parent body. = torqueor force exerted by the -th jont. T SE3 : λ} }, a functon of q. V se3 = the generalzed velocty of the -th body, vewed n the body frame }. V se3 = component-wse tme dervatve of V. S se3 [ n = Jacoban of T ] vewed n }. S = T T,, T T q 1 q n, where q k R denotes the k-th coordnate of the -th jont,.e., q = q 1,, qn. I = the generalzed nerta of the -th body, vewed n }. F dse3 = the generalzed force transmtted to the -th body from ts parent through the connectng jont, vewed n }. F ext }. dse3 = the generalzed force actng on the -th body from envronment, vewed n Table 2: Recursve Inverse Dynamcs 6 One can assume that V 0 = 0 and V 0 = 0, g where g R 3 denote the gravty vector, vewed n the nertal frame, wth approprate drecton and magntude. 8

9 whle forward recurson do T = functon of q V = Ad T V λ + S q V = Ad T V λ + ad V S q + Ṡ q + S q end whle whle backward recurson do F = I V ad V I V F ext τ = S T F end whle 2.2 Recursve Forward Dynamcs + k µ Ad F T k,k Featherstone [1] found that the dynamcs equatons of the -th body can be reformulated to have the followng form, F = Î V + ˆB, 42 where Î s called as the artculated body nerta of the body and ˆB s an assocated bas force. He also showed that the artculated body nerta and bas force correspondng to each body n open chan systems can be calculated recursvely, and by usng these new quanttes, forward dynamcs can be solved wth an On algorthm. A Le group formulaton of the artculated body nerta method was reported n [4]. Here, a more general form of the geometrc formulaton supportng mult-degree-of-freedom jont models s presented. Startng from 42, we ll show that the same form of dynamcs equatons stll holds for the parent of the -th bodyλ-th body, and Îλ and ˆB λ can be calculated from Î and ˆB, whch leads a backward recurson process for them. Let s assume that the equatons of moton for -th body can be wrtten as 42. By substtutng 40 for V n 42, one can get F = Î Ad T V λ + ad V S q + S q + Ṡ q + ˆB, 43 and from S T F = τ, the unknown q can be wrtten as q = S T ÎS τ S T Î Ad T V λ + ad V S q + Ṡ q S T ˆB }. 44 From 41 the dynamcs equatons for the λ-th body becomes F λ = I λ Vλ ad V λ I λ V λ Fλ ext + and by substtutng 43 for F k n 45, one can get k µλ Ad F T k, 45 λ,k F λ = I λ Vλ ad V λ I λ V λ Fλ ext + Ad Î T k Ad λ,k T V λ + ad Vk S k q k + S k q k + Ṡk q k + ˆB k }. λ,k k µλ 46 By substtutng 44 for the unknown q k n 46 and arrangng the equatons n terms of V λ, one can have the dynamcs equatons for the λ-th body wth the followng desred form: F λ = Îλ V λ + ˆB λ 47 9

10 where Î λ = I λ + k µλ k µλ } Ad Î T k ÎkS k S Tk ÎkS k S Tk Î k Ad λ,k T 48 λ,k ˆB λ = ad V λ I λ V λ Fλ ext + Ad ˆB T k + ad Îk Vk S k q k + Ṡk q k λ,k + ÎkS k S T k ÎkS k τ k S T k Îkad Vk S k q k + Ṡk q k S T k ˆB k }. 49 In summary, the On algorthm for forward dynamcs of open chan systems conssts of the followng three man recurson process: 1. Forward recurson: recursvely calculates V for each body wth Backward recurson: recursvely calculates Î and ˆB for each body wth 48 and Forward recurson: recursvely calculates q and V for each body wth 44 and 40. Table 3 shows the forward dynamcs algorthm for open chan systems wth a few addtonal ntermedate varables, such as η, Ψ, Π, and β, for the smplcty and effcency of the equatons. Table 3: Recursve Forward Dynamcs whle forward recurson do T = functon of q V = Ad T V λ + S q η = ad V S q + Ṡ q end whle whle backward recurson do Î = I + k µ Ad T,k ˆB = ad V I V F ext Π k Ad T,k + k µ Ad T,k Ψ = S TÎS Π = Î ÎS Ψ S TÎ β = ˆB + η Î + S Ψ τ S T Î η + ˆB } end whle whle forward recurson do } q = Ψ τ S TÎ Ad T V λ + η S T ˆB V = Ad T V λ + S q + η F = Î V + ˆB end whle 2.3 Recursve Hybrd Dynamcs A geometrc recursve algorthm for hybrd dynamcs of open chan systems was reported n [5], but the formulaton was based on 1-dof jonts. Here, more general form of the geometrc hybrd dynamcs s presented to support mult-degree-of-freedom jonts more convenently. One can derve the hybrd dynamcs for open chan systems wth a smlar way as n 2.2 for forward dynamcs. Let s go back to 46. Unlke the case of forward dynamcs, as q k n 46 β k 10

11 correspondng to an actve jont s already known, one can arrange 46 n terms of Vλ by substtutng 44 nto 46 for q k of passve jonts only as follows: F λ = Îλ V λ + ˆB λ 50 where Î λ = I λ + + k µλ a} k µλ p} k µλ p} Ad Î T k Ad λ,k T λ,k } Ad Î T k ÎkS k S Tk ÎkS k S Tk Î k Ad λ,k T λ,k ˆB λ = ad V λ I λ V λ Fλ ext + Ad ˆB T k + ad } Îk Vk S k q k + Ṡk q k + S k q k λ,k k µλ a} + Ad ˆB T k + ad Îk Vk S k q k + Ṡk q k λ,k + ÎkS k S T k ÎkS k τ k S T k Îkad Vk S k q k + Ṡk q k S T k ˆB k } where a and p denote the sets of the acceleraton-prescrbed and unprescrbed jonts n the system respectvely. Table 4 shows the hybrd dynamcs algorthm for open chan systems. Table 4: Recursve Hybrd Dynamcs whle forward recurson do T = functon of q V = Ad T V λ + S q η = ad V S q + Ṡ q end whle whle backward recurson do Î = I + k µ Ad T,k ˆB = ad V I V F ext f a then Π = Î β = ˆB + Î η + S q else Ψ = S TÎS Π = Î ÎS Ψ S TÎ Π k Ad T,k + k µ Ad T,k β = ˆB + η Î + S Ψ τ S T Î η + ˆB } end f end whle whle forward recurson do f a then V = Ad T V λ + S q + η F = Î V + ˆB τ = S TF else q = Ψ τ S TÎ Ad T V λ + η S T β k ˆB } 11

12 V = Ad T V λ + S q + η F = Î V + ˆB end f end whle 3 Dfferentaton of the Geometrc Dynamcs 3.1 Basc Dervatves It s useful to have the followng dervatves for dfferentatng the recursve dynamcs algorthms wth the chan rule. Lemma 3. Let p R be an arbtrary scalar varable and T SE3 be a functon of p. Then Ad T = ad T T Ad T. 53 Proof. Let ξ be an arbtrary se3. If Ad T s regarded as a 6 6 matrx, then Ad T ξ = Ad T ξ + Ad T ξ. 54 Ad T can also be regarded as a lnear mappng, Ad T : ξ T ξt, and n ths case, Ad T ξ = T ξt 55 = T ξt + T ξ T T + T ξ 56 = T T T ξt T T ξt T + T ξ T 57 = ad T T Ad T ξ + Ad T ξ. 58 As ξ se3 s arbtrary, one can see Ad T = ad T T Ad T from 54 and 58. Corollary 1. Let p R be an arbtrary scalar varable and T SE3 : λ} }. Then Ad T = ad h Ad T Ad = Ad ad T T h where h se3 s defned as h = T T. 61 Corollary 2. If p = q k where q k denotes the k-th coordnate of the -th jont, then h = Sk 62 where S k se3 denotes the k-th column of the -th jont Jacoban, S se3 n. If p / q = q 1,, qn }, then h =

13 3.2 Dervatves of the Dynamcs By applyng chan rule wth 59 and 60, the recursve algorthm for nverse, forward, and hybrd dynamcs can be dfferentated wth respect to an arbtrary scalar varable p R. Table 5 shows the dervatve of the recursve nverse dynamcs, and t can solve τ wth gven q, q, q. In Table 6, the dervatve of the recursve forward dynamcs, whch calculates q τ wth gven, s gven. The dervatve of the recursve hybrd dynamcs whch solves s presented n Table 7. τ a, qp Table 5: Dervatve of the Recursve Inverse Dynamcs whle forward recurson do T T h V V = Ad T = Ad T + ad V V λ V λ ad h Ad T V λ + S q q + S V λ + S ad h Ad T S S q + ad V q q + S end whle whle backward recurson do F = I V + I V ad V τ end whle + k µ Ad T,k = S T F + S T F Fk q q + S + Ṡ q q + Ṡ I V ad I V V V + I ad h k F k wth gven qa, qa, qa F ext Table 6: Dervatve of the Recursve Forward Dynamcs whle forward recurson do T T h V η = Ad T V λ ad h Ad T = ad S V S q + ad V q q + S end whle whle backward recurson do Î ˆB Ψ Π β Π k V λ + S q q + S + Ṡ q q + Ṡ = I + k µ Ad T Π kad h k Π k ad h k,k = ad V I V ad I V V ext V + I F + k µ Ad βk T ad h k β k,k = Ψ S T Î S + TÎ S S S + TÎ } T S Ψ = Î Ψ Î S ST Î + Î S Ψ S TÎ Î + S Ψ S TÎ } T S + Î Ψ S TÎ S + Î Ψ S T Î = ˆB + Î η + S Ψ τ S T Î η + ˆB } + Î η + S Ψ Ψ + S τ S T Î η + ˆB T } Ad T,k T 13

14 + S Ψ τ S end whle whle forward recurson do q V F = Ψ τ S Î T AdT S τ + Ψ S TÎ Ad T S T ˆB S T ˆB = Ad V λ T = Î end whle V + Î V TÎ η + ˆB S T Î η η + Î + ˆB } V λ + η S T ˆB } TÎ + S T Î AdT V λ } ad h Ad T V λ + η V λ + η ad h Ad T V λ + S q q + S + η + ˆB Table 7: Dervatve of the Recursve Hybrd Dynamcs whle forward recurson do T T h V η = Ad T V λ ad h Ad T = ad S V S q + ad V q q + S end whle whle backward recurson do Π k Î = I + k µ Ad T Π kad h k,k ˆB = ad V I V ad I V V V + I + k µ Ad βk T ad h k β k,k f a then else Π β Ψ Π β = Î = ˆB V λ + S q q + S + Ṡ q q + Ṡ F ext Π k ad h k + Î η + S q + Î η + S q q + S S T Î TÎ S + = Ψ S + S = Î Ψ Î S ST Î + Î S TÎ } T S Ψ T } Ad T,k S Ψ S TÎ Î + S Ψ S TÎ } T S + Î Ψ S TÎ S + Î Ψ S T Î + Î η + S Ψ τ S T Î η + ˆB } + Î η + S Ψ Ψ + S τ S T Î η + ˆB = ˆB + S Ψ τ end f end whle whle forward recurson do f a then S T TÎ η + ˆB S T Î η η + Î + ˆB } 14

15 V = Ad V λ T ad h Ad T V λ + S q q + S + η F V + Î V τ = Î = S T F + S T F + ˆB else q = Ψ τ S Î T AdT τ + Ψ S V F end f end whle S T Î Ad T S T ˆB S T ˆB = Ad V λ T = Î V + Î V TÎ + S T Î V λ } V λ + η S T ˆB } AdT ad h Ad T V λ + η V λ + η ad h Ad T V λ + S q q + S + η + ˆB To obtan the dervatves of the dynamcs, t s needed to run the assocated recursve dynamcs n advance, and n addton, the followng quanttes S, Ṡ ext F,, I, and h 64 for each body should be set properly n the ntalzaton step. The algorthms for the dervatves of the dynamcs are so general that they can be appled to dfferentatng the equatons of moton wth respect to any arbtrary scalar varable. It should be noted that, by mplementng the algorthms cleverly, the calculaton speed can be much faster than ts nave mplementaton, because, n some cases, many of the quanttes n 64 are zero. For example, f p = q k where q k R denotes the k-th coordnate of system, then I = 0 S = Ṡ f p = q k, then S Ṡ = I f p = q k, then Ṡ References = h = h = 0 = 0 when qk / q = S = I = 0 when qk / q = h = 0. [1] Roy Featherstone, Robot Dynamcs Algorthms, Kluver Academc Publshers, [2] Rchard M. Murray, Zexang L and and S. Shankar Sastry, A Mathematcal Introducton to Robotc Manpulaton, CRC Press,

16 [3] F. C. Park, J. E. Bobrow and S. R. Ploen, A Le group formulaton of robot dynamcs, Internatonal Journal of Robotcs Research, vol. 14, no. 6, pp , [4] S. R. Ploen and F. C. Park, Coordnate-nvarant algorthms for robot dynamcs, IEEE Transactons on Robotcs and Automaton, vol. 15, no. 6, pp , [5] Garett A. Sohl and James E. Bobrow, A recursve multbody dynamcs and senstvty algorthm for branched knematc chans, Journal of Dynamc Systems, Measurement, and Control, vol. 123, pp ,