Dynamics of Linked Hierarchies. Constrained dynamics The Featherstone equations

Transcription

1 Dynm o Lnke Herrhe Contrne ynm The Fethertone equton

2 Contrne ynm pply ore to one omponent, other omponent repotone, rom ner to r, to ty tne ontrnt F

3 Contrne Boy Dynm Chpter 4 n: Mrth mpule-be Dynm Smulton o Rg Boy Sytem Ph.D. ertton, Berkeley, 996 R.Prent, CSE788 OSU

4 Pmnre Lnk numbere 0 to n Fxe be: lnk 0; Outermot t lke: lnk n Jont numbere to n Lnk h nbor jont, Jont Eh jont h DoF Vetor o jont poton: qq,q 2, q n T 2 n R.Prent, CSE788 OSU

5 The Problem Gven: the poton q n velote o the n jont o erl lnkge, the externl ore tng on the lnkge, n the ore n torque beng pple by the jont tutor Fn: The reultng elerton o the jont: q& & q& R.Prent, CSE788 OSU

6 Frt Determne equton tht gve bolute moton o ll lnk Gven: the jont poton q, velote n elerton Compute: or eh lnk the lner n ngulr veloty n elerton tve to n nertl rme R.Prent, CSE788 OSU

7 Notton globl vrble v Lner veloty o lnk v α Lner elerton o lnk ngulr veloty o lnk ngulr elerton o lnk R.Prent, CSE788 OSU

8 Jont vrble q jont poton q& q u r jont veloty Unt vetor n reton o the x o jont vetor rom orgn o F - to orgn o F vetor rom x o jont to orgn o F R.Prent, CSE788 OSU

9 B term F boy rme o lnk Orgn t enter o m xe lgne wth prnple xe o nert Frme t enter e o m F r F - u [ q, q& ] Nee to etermne: q& & R.Prent, CSE788 OSU x o rtulton Stte vetor

10 From be outwr Velote n elerton o lnk re ompletely etermne by:. the velote n elerton o lnk - 2. n the moton o jont R.Prent, CSE788 OSU

11 Frt etermne velote n elerton From veloty n elerton o prevou lnk, etermne totl globl veloty n elerton o urrent lnk v Compute rom be outwr,,,, α Moton o lnk - R.Prent, CSE788 OSU v To be ompute v,,, α, Moton o lnk,,,,, α Moton o lnk rom lol jont

12 Compute outwr ngulr veloty o lnk ngulr veloty o lnk - plu ngulr veloty nue by rotton t jont Lner veloty lner veloty o lnk - plu lner veloty nue by rotton t lnk - plu lner veloty rom trnlton t jont R.Prent, CSE788 OSU v v r v

13 Compute outwr ngulr elerton propgton α α & Lner elerton propgton α r r& v& Rewrtten, ung r& v v tve veloty n r & r v v r v v rom prevou le α r r v v& R.Prent, CSE788 OSU

14 Compute outwr ngulr elerton propgton α α & Lner elerton propgton α r r v v& Nee & v& n term o jont x moton q& q&& R.Prent, CSE788 OSU u

15 Dene w n v n ther tme ervtve Jont veloty vetor x tme prmetr veloty prmt q& u Jont elerton vetor x tme prmetr elerton ξ q& revolute u unkown v 0 v R.Prent, CSE788 OSU

16 Veloty propgton ormule revolute lner v v r v v v v r ngulr R.Prent, CSE788 OSU

17 Tme ervtve o v n w revolute Jont elerton vetor Chnge n jont veloty vetor & ξ v & 2 ξ From jont elerton vetor From hnge n jont veloty vetor From hnge n hnge n vetor rom jont to CoM R.Prent, CSE788 OSU

18 q& u Dervton o & revolute v& & q&& u qu & & ξ qu & & u & u & ξ t v & t & v & 2 ξ & R.Prent, CSE788 OSU

19 elerton propgton ormule lner revolute α r r& α 2 ξ v & r & r v α r ξ r 2 v& Prevouly erve ngulr R.Prent, CSE788 OSU & α α ξ α ξ & α

20 Sptl ormulton o l t t elerton propgton revolute r v v r v v r r ξ α ξ α α 2 r r ξ α But remember u q& & ξ n unknown R.Prent, CSE788 OSU

21 Frt tep n orwr ynm q, q& Ue known ynm tte: Compute bolute lner n ngulr velote: v, Remember: elerton propgton equton nvolve unknown jont elerton But rt nee to ntroue notton to ltte equton wrtng Sptl lgebr R.Prent, CSE788 OSU

22 Sptl lgebr Sptl veloty Sptl elerton v v α R.Prent, CSE788 OSU

23 Sptl Trnorm Mtrx r oet vetor R rotton v v R 0 G G F F G F ~ rr R G G F F ro prout opertor R.Prent, CSE788 OSU

24 Sptl lgebr Sptl ore Sptl trnpoe τ b [ T T x b ] Sptl jont x u u Sptl nner prout x y ue n lter R.Prent, CSE788 OSU

25 ComputeSerlLnkVelote revolute, v, α, For to N o 0 en R rotton mtrx rom rme - to r ru vetor rom rme - to rme n rme oornte R v Rv q& q u r v v q& u Spe to revolute jont R.Prent, CSE788 OSU

26 Sptl ormulton o l t t elerton propgton revolute Prevouly: r r ξ α ξ α α 2 r r ξ α && Wnt to put n orm: q u R 0 R.Prent, CSE788 OSU u R rr F G ~ Where:

27 Sptl Corol ore Sptl Corol ore revolute ξ α α 2 r r ξ α ξ α α q && 2 u q Thee re the term nvolvng u u q& & ξ 2 r R.Prent, CSE788 OSU

28 Fethertone lgorthm O Sptl elerton o lnk Sptl ore exerte on lnk through t nbor jont Sptl ore exerte on lnk through t outbor jont ll expree n rme Fore expree tng on enter o m o lnk R.Prent, CSE788 OSU

29 Serl lnkge rtulte moton Sptl rtulte nert o lnk ; rtulte men entre ubhn beng onere Sptl rtulte zero elerton ore o lnk nepenent o jont elerton; ore exerte by nbor jont on lnk, lnk not to elerte Develop equton by nuton R.Prent, CSE788 OSU

30 Be Ce Coner lt lnk o lnkge lnk n Fore/torque pple by nbor jont grvty nert*elerton o lnk n Newton-Euler equton o moton m g n n m n τ α n n n n n n R.Prent, CSE788 OSU

31 Ung ptl notton Lnk n n nbor jont τ n m n g 0 M n τ n 0 n R.Prent, CSE788 OSU n n α n n n n m n n n g n n

32 nutve e nutve e O O ume prevou true or lnk ; oner lnk - outbor jont O τ O Lnk - nbor jont m g τ 0 0 O O g m M α τ τ R.Prent, CSE788 OSU τ τ

33 nutve e nutve e 0 O g m M α 0 O τ τ O O The eet o jont on lnk - equl n O The eet o jont on lnk equl n oppote to t eet on lnk Subttutng R.Prent, CSE788 OSU

34 nutve e nutve e nvokng nuton on the enton o R.Prent, CSE788 OSU

35 nutve e nutve e Expre n term o - n rerrnge q && ] [ q && Nee to elmnte rom the rght e o the equton q& & R.Prent, CSE788 OSU

36 nutve e nutve e ] [ q && Mgntue o torque exerte by revolute jont tutor Q u τ ore n torque τ pple to lnk t the nbor jont gve re to ptl nbor ore reolve th b τ n the boy rme o τ u u u u u Q τ τ τ R.Prent, CSE788 OSU Moment o ore Moment o ore

37 nutve e nutve e u Q τ Q q && prevouly n q && Premultply both e by ubttute Q or, n olve Q q && R.Prent, CSE788 OSU

38 n ubttute n ubttute Q Q q && ] [ q && ] [ ] ] [ [ ] [ Q R.Prent, CSE788 OSU ] [

39 n orm & term n orm & term ] [ ] [ ] [ Q ] ] [ [ Q To get nto orm: ] [ Q R.Prent, CSE788 OSU ] ] [ [ Q

40 Rey to put nto oe Ung Loop rom ne out to ompute velote prevouly evelope repete on next le Loop rom ne out to ntlze,, n vrble Loop rom oute n to propgte, n upte Loop rom ne out to ompute q& & ung,, R.Prent, CSE788 OSU

41 ComputeSerlLnkVelote revolute // Th oe rom n erler le loop ne out to ompute velote, v, α, For to N o en R rotton mtrx rom rme - to r ru vetor rom rme - to rme n rme oornte R v Rv q& u r v v q& R.Prent, CSE788 OSU u Spe to revolute jont

42 ntserllnk ntserllnk revolute // loop rom ne out to ntlze,, vrble For to N o m g 0 M 0 0 M 2 r R.Prent, CSE788 OSU en

43 SerlForwrDynm Cll ompserllnkvelote Cll ntserllnk // new oe wth ll to 2 prevou routne For n to 2 o ] [ // loop oute n to orm n or eh lnke ] [ ] ] [ [ Q ] ] [ [ Q 0 0 // loop ne out to ompute lnk n jont elerton For to n o 0 0 Q q && R.Prent, CSE788 OSU q &&

44 n tht ll there to t! R.Prent, CSE788 OSU

45 v r v v α α & & & α r r v α r r & ξ v & ξ t v & & t & v & 2 ξ v r& v v r & r v v& q& & u ξ q& & u ξ v & v v r R.Prent, CSE788 OSU

46 2 r r ξ α 2 ξ α α R.Prent, CSE788 OSU

47 v α R F G ~ 0 v R rr F G ~ τ [ ] T T b b x u y x u u R.Prent, CSE788 OSU