Linear Complementarity Problem Simulation of The Meter Stick Trick

Size: px

Start display at page:

Download "Linear Complementarity Problem Simulation of The Meter Stick Trick"

Garey Stewart
6 years ago
Views:

1 Liear Complemetarity Problem Simulatio o Te Meter Stick Trick Yoke Peg Leog, Ze Lv Abstract I tis paper, we ocus o aturally icorporatig cotact modes estimatio i a dyamical simulatio o a object wit multiple cotacts. More speciically, tis paper presets a algoritm to simulate a meter stick trick usig a Liear Complemetarity Problem (LCP) ormulatio. A meter stick, modeled as a plaar uiorm orizotal rod i gravity, is supported by two poit cotacts or igers. We developed a LCP-based algoritm to simulate motio o te meter stick we oe iger moves toward te oter at a ixed speed. Te simulatio matces observatios o pysical meter stick trick experimets. Overall, tis paper sows tat a LCP-based algoritm is a robust optio to aturally capture te switcig cotact modes i a dyamical simulatio o a object wit multiple cotacts. I. INTRODUCTION Robotic maipulatio plays a sigiicat role i may applicatios icludig te mauacturig ad ealt care idustry. Maipulatio plaig is a callegig task because o te complexity i uderstadig ad modelig te relatiosip betwee a maipulator ad its eviromet. Eve we te matematical model o te robotic system is assumed to be completely kow, ully uderstadig ad simulatig te iteractios betwee a maipulator ad its eviromet is ot trivial. Oe o te diiculties is to estimate cotact modes betwee a maipulator i cotact wit a object. Cotact mode estimatio is importat or maipulatio plaig because ote time cotact modes will determie te amout o orce to apply or te ext course o actio to be take by te robot. For example, we a robotic ad is graspig a object, te cotact modes o te igers o te object diers depedig o te amout o orces applied. I te orces applied by eac iger is too small, te object slides. Kowig tat te cotacts are i slidig mode is elpul or te robotic ad to respod accordigly to acieve a irm grasp. I tis paper, we ocus o aturally icorporatig cotact modes estimatio i a dyamical simulatio o a object wit multiple cotacts. More speciically, tis paper presets a algoritm to simulate a meter stick trick usig a Liear Complemetarity Problem (LCP) ormulatio. A meter stick, modeled as a plaar uiorm orizotal rod i gravity, is supported by two poit cotacts or igers. We developed a LCP-based algoritm to simulate motio o te meter stick we oe iger moves toward te oter at a ixed speed. Te simulatio sows tat at slow speed, te meter stick always stays balaced betwee te igers. Te meter stick alterates betwee stickig ad slidig modes util bot igers reac te ceter o mass o te meter stick. At ig speed, te simulatio sows tat te meter stick alls. Bot cases matc observatios o pysical meter stick trick experimets. Overall, tis paper sows tat a LCP-based algoritm is a robust optio to aturally capture te switcig cotact modes i a dyamical simulatio o a object wit multiple cotacts. Te rest o te paper is laid out as ollows: i Sectio II, we preset a review o Berard ad colleagues work i usig LCP to deie cotact modes; i Sectio III, we discuss te matematical backgroud o te LCP ormulatio ad dyamic model i te plae; i Sectio IV, we ormulate a discretized dyamic model i LCP orm; i Sectios V, we discuss te meter stick trick ad results rom te LCP-based simulatio o te trick; ad i Sectio VI, we coclude wit remarks o uture works. II. RELATED WORKS Recetly, Berard et. al. [] used a liear complemetarity problem ormulatio o rigid body dyamics to motivate deiitios o cotact modes. Extedig a metod to compute te set o all wreces guarateed to acieve a particular cotact state or a tree-dimesioal system is diicult. I te plae, possible iteractios at a cotact are slide let, slide rigt, roll, ad separate. However or a cotact i a tree-dimesioal space, tere is a iiite umber o directios tat a cotact could slide. Te autors used a liear complemetarity problem ormulatio to obtai deiitios o cotact modes or a plaar system. Te, tey exteded te deiitios to a tree-dimesioal system. I te paper, tey sowed te equivalece betwee ituitively motivated cotact modes ad tose implied directly by te LCP. Te paper begis by presetig te backgroud o dyamic model o a rigid body i te plae, wic icludes Newto-Euler equatios, kiematic equatios, ormal complemetarity ad rictio law. It s a pretty detailed orm o backgroud, sice i we ave all te equatios metioed above, we ca caracterize a system matematically so as to perorm te ext step calculatio. Geeral maipulatio tasks ca be viewed as a sequece o cotact modes leadig rom a iitial state to a goal state. So te paper also explais dieret cotact modes i te plae. To elp solidiy te ideas o cotact modes, te paper gives a example sowig a disc iitially at rest ad i cotact wit a small ixed disc, wic illustrates ituitive cotact modes or a object clearly. From tis part, we ca better uderstad ow te cotact orces ad acceleratios cage correspodig to dieret types o cotact modes. Te, te paper sows a cocise deiitio o LCP ad rewrites te istataeous dyamic model ito LCP ormulatio or two dimesioal system. I additio, te paper sows te ituitively-motivated cotact modes are idetical to tose obtaied by te LCP

2 ormulatio. Oe cotributio o te paper is tat it exteds LCP-motivated cotact modes ito tree-dimesioal, wic is a great callege or previous metods. Overall, tis paper presets te equality o ituitive cotact modes or rigid bodies wit te coes geerated rom a liear complemetarity ormulatio o te dyamics. Besides, te paper sows te system as a LCP ad sows ow tis ormulatio easily reduces to polyedral covex coes by usig multiple rictio models. I additio to [], we reerred to [2] wic provides a iterative ramework to ormulate a dyamical system simulatio i a mixed complemetarity problem (MCP) orm. Aoter paper [3] icludes te metod to rewrite te MCP ito a LCP orm wic ca te be solved usig a Lemke algoritm. III. BACKGROUND Tis sectio explais te matematical backgroud related to te LCP-based simulatio. Te LCP ormulatio, dyamic model i te plae ad dyamic time-steppig are discussed. A. LCP Formulatio Te stadard liear complemetarity problem is deied as ollows. Give te costat matrix B R m m ad vector b R m, id vectors z R m, y R m suc tat y = Bz + b y z () were elemets o y ad z are greater ta. Tis costrait o y ad z requires te use o slack variables. For every pair o elemets i y ad z tat ca be bot positive ad egative, a pair o slack variables are deied to replace te origial elemet. y i = y + i z i = z + i y i z i were y + i is te positive compoet o y i, ad y i is te egative compoet o y i. I oter words, we y i is positive, y + i > ad y i =. Likewise, we y i is egative, y + i = ad y i >. Hece, a origial vector o [y y 2... y ] T is rewritte as [y y + 2 y 2... y ] T i y 2 is allowed to ave eiter a positive value or a egatives value. Te same aalysis is also applied to z i. B. Dyamic Model I Te Plae A dyamic model is required to describe te dyamics o a system. I tis work, a dyamic model i a plae is cosidered, ad it icludes bot te dyamics o a body ad cotact costraits. Te otatio ad ormulatio o te model is aalogous to [], [2]. ) Newto-Euler Equatios: Dyamics o a body i a plae is described usig te Newto-Euler equatio: M v = W λ + W λ + w ext (2) were M = diag(m, m, J) is te geeralized mass matrix o te body, w ext = [ x y τ z ] T is te exteral wrec applied to te body. Te momet o iertia, J, o a slader rod is 2 ml2. I additio, λ ad λ are vectors cotaiig all te ormal ad tagetial compoets o te cotact orces applied to te body. Te distace betwee a iger ad te ceter o mass o a body is deied as r i, te ormal cotact orces o te body is deied as i, ad te tagetial cotact orces o te body is deied as t i. Te x ad y compoets o r i ad i are r ix, r iy, ix, ad iy respectively. W ad W are Jacobia matrices tat map te cotact orces to teir equivalet wreces i te body-ixed rame. Tey are deied as:... i... W =... r i i ti... W = (3)... r i ti... were r i ti is deied as r ix t iy r iy t ix. Te velocity o te body i te coiguratio space is related to te system velocity based o te relatiosip: q = Gv (4) were q represets te coiguratio o te body ad G is te represetatio Jacoba relatig te system velocity v to te time-derivative o te system coiguratio q. 2) Normal Cotact Costraits: For eac cotact, we deie te distace alog te ormal directio betwee a body ad a statioary cotact as Ψ i, ad te ormal cotact orce as λ i,. We te cotact orce is greater ta zero te te ormal distace equals to zero. Oterwise, we te ormal distace is greater ta zero te te cotact orce equals to zero. Hece, te costrait ca be writte as: Ψ λ (5) were te symbol idicates ormality (i.e., Ψ T λ = ). 3) Tagetial Cotact Costraits: For eac cotact, we also deie te distace alog te tagetial directio betwee a body ad a cotact as Ψ i, ad te tagetial cotact orce as λ i,. Combiig te tagetial orce vectors ad relative slip velocity vectors at a cotact ito sigle vectors, maximum dissipatio or all cotacts ca be writte compactly as: λ W T v + Eσ + Ψ (6) σ Uλ E T λ (7) were U is te diagoal matrix wit te i t diagoal elemet equal to µ i, σ i approximates te slidig speed at cotact i, ad E is te block diagoal matrix wit i t block o te mai diagoal [2]. IV. DISCRETIZED DYNAMIC MODEL IN LCP FORM To simulate te dyamics o a body wit multiple cotacts, we utilize te complemetarity betwee cotact orces ad te relative motios betwee cotacts ad te body wic is discussed i Sectio III. Because o te complemetarity, LCP is suitable or computig te dyamics o a body wit multiple cotacts. Usig dyamic time-steppig, we

3 simulate a system s dyamics iteratively. Te rest o tis sectio described te dyamic time-steppig equatios ad te metod to ormulate te system s dyamics i LCP orm. A. Dyamic Time-steppig Equatios Simulatig te dyamics o a system iteratively requires discretizatio o te cotiuous dyamic model. Usig te Euler s metod, derivatives are approximated as: q = ql+ q l (8) were time step is a costat. Te Newto-Euler equatio (2) ad velocity kiematic equatio (4) are rewritte i a discrete time orm as ollows: Mv l+ = Mv l + (λ l+ ext + W λ l+ + W λ l+ ) q l+ = q l + v l+ (9) were λ represets a orce wrec. All applied or ocostrait orces are icluded i λ ext. Te discrete orms o te o-peetratio ad rictio costraits (5), (6), ad (7) are: λ l+ Ψ l + Ψl q q + Ψl t () λ l+ Eσ l+ + Ψl q q + Ψl t () σ l+ Uλ l+ E T λ l+ (2) were q = q l+ q l, t =, Ψl q = W T, ad Ψl q = W T. Note tat Ψl t represets te lateral positio cage o te rictioal surace i oe time step. Rewrite (), () ad (2) i term o te geeralized velocity vector v ad te cotact impulse i term o p (.) = λ (.). We arrive at te ollowig dyamic time-steppig equatios: p l+ Ψl Eσl+ + W T v l+ + Ψl ] p l+ + W T v l+ + Ψl σ l+ Uλ l+ E T λ l+ (3) were te equatios are i complemetarity orms similar to (). Tey are supplemeted by te discretized dyamic equatios i (9) wic is rewritte as: Mv l+ = Mv l + p ext + W p l+ + W p l+ q l+ = q l + v l+ (4) B. Dyamics i LCP Form Equatios (3) ad (4) costitute a MCP (5). I order to solve (5) to simulate te dyamics o a body wit multiple cotacts, we rewrite it ito LCP orm by substitutig away te v l+ term based o te relatiosip rom (4). Rewritig (4) to obtai v l+ = M W p l+ + M W p l+ + v l + M p ext (6) F y x r q y C Fig.. Coiguratio o a meter stick trick. C deotes te body-ixed rame, ad F deotes te world rame. From te MCP, we ave te ollowig equatios: ρ l+ x = W T v l+ + Ψl ρ l+ r2 + Ψl = W T v l+ + Ψl s l+ = Up l+ E T p l+ (7) Substitutig (6) ito (7) ad rewritig te equatios i matrix orm, te MCP is coverted ito a LCP (8). Te LCP is solved usig te Lemke algoritm [4]. Te resultig variable values at eac iteratio are used to update te variables at te ext iteratio. V. THE METER STICK TRICK LCP-based dyamics simulatio described i Sectio IV is used to simulate a meter stick trick. Tis sectio presets te model o te meter stick trick ad discusses te results rom te simulatios. A. Model o a Meter Stick Trick A meter stick trick ivolves balacig a meter stick usig two igers wile movig oe o te iger towards te oter statioary iger. At relatively low velocity, te igers meet eac oter at te ceter o te mass o te meter stick. Fig. sows te coiguratios o te meter stick trick system. Te legt o meter stick is L = m, te tickess is t =.5 cm, ad te mass is m =.2 kg. Additioally, te vector poits rom te origial poit o te world rame to te ceter o mass is q, wic correspods to te coiguratios o te meter stick. Hece, te relevat compoets i te LCP (8) or te meter stick trick are as ollows:.2 M =.2 p ext = 6 r = rx r2x r.25 2 =.25 W = r x r 2x mg

4 ρ l+ ρ l+ s l+ M W W = W T W T E U E T v l+ p l+ p l+ σ l+ Mv l + p ext Ψ + l + Ψl Ψ l (5) ρ l+ ρ l+ s l+ W T M W W T M W = W T M W W T M W E E T p l+ p l+ σ l+ W T vl + W T M p ext + Ψl + W T vl + W T M p ext + Ψl + Ψl (8) W = r y r y r 2y r 2y Ψ = U = µ µ E = Ψ = v m v m were µ is te coeiciet o rictio wic ca be eiter static or kietic depedig o te cotact modes o te igers, ad v m is te costat velocity o te movig iger. I simulatios, te let iger is allowed to move at a costat velocity, ad te rigt iger stays statioary at all time. Two cases are ivestigated i tis paper: (a) low iger velocity ad (b) ig iger velocity. Te rest o tis sectio discusses te results or bot simulatio cases. B. Low Figer Velocity We te movig iger velocity is relatively low, te meter stick always stays balace betwee te two igers. Fig. 2 ad Fig. 3 sow te result o a simulatio o a meter stick trick wereby te two supportig igers started o at.4 m away rom te ceter o mass o te meter stick. Figer is movig at a velocity o. m/s. Fig. 2 illustrates tat te meter stick is alteratig betwee stickig ad slidig modes wit respect to te igers. Tis result is observed i a pysical meter stick trick. Note tat i Fig. 3, te distace betwee te two igers at te time o switcig ca be aalytically solved or based o a simple orce balace relatiosip. Reerrig to Fig. 4, te positios o bot igers satisy te ollowig relatiosip at all time: W x = W 2 x 2 (9) were W i is te ormal orce applied by iger i o te meter stick. We iger starts to slide ad iger 2 stops slidig, iger as to overcome static rictio to start slidig, ad Positio, m COM o meter stick.5 Figer Figer Fig. 2. Positios o te ceter o mass o te meter stick ad te two supportig igers. Relative positio rom COM o meter stick, m Figer Figer Fig. 3. Relative positios o te two supportig igers rom te ceter o mass o te meter stick. iger 2 is uder kietic rictio we it is slidig. Geerally, kietic rictio, µ k, ad static rictio, µ s, are uctios o te ormal orce, N, applied at te cotact. Tis paper assumes a simple rictio model wereby rictio orces, F, ollow te ollowig equatios: F s = µ s N F k = µ k N. (2) Combiig bot (9) ad (2), we arrive at (2) wic

5 W W μsw x x2 2 mg μkw2 Fig. 4. Force balace o a meter stick supportig by two igers we iger is stickig to te meter stick ad iger 2 is slidig relative to te meter stick. describes te relatiosip betwee te coeiciet o rictio ad te positios o igers we cotact mode switcig occurs. µ k x start = µ s x stop (2) were x start is te positio o te iger tat starts to slide, ad x stop is te positio o te iger tat stops slidig. C. Hig Figer Velocity At ig iger velocity, te meter stick trick ails because te cotact modes do ave eoug time to switc accordigly. Fig. 5 sow te result o a simulatio o a meter stick trick wereby te two supportig igers started o at.4 m away rom te ceter o mass o te meter stick. Velocity o iger is varied. We te velocity is icreased, te cotact mode switcig becomes less distict, ad evetually, te meter stick trick ails. VI. CONCLUSIONS We ave preseted a dyamics simulatio metod tat captures cotact modes aturally i te dyamic time-steppig iteratio usig a LCP-based algoritm. Te metod is used to simulate a meter stick trick. A meter stick i gravity is supported by two igers wereby oe o te iger is movig at a costat velocity towards te oter statioary iger. Te simulatio results matc a pysical meter stick trick. We te velocity is relatively low, te meter stick alterates betwee slidig mode ad stickig mode util bot igers meet at te ceter o mass o te meter stick. We te velocity is icreased, te cotact mode switcig becomes less distict, ad evetually, te meter stick trick ails. Based o tis work, uture directios ca ivolve extedig te LCP-based dyamics simulatio ito a more complex system. Oter ta tat, derivig te aalogy o tis simulatio metod i tree dimesioal space will ope up a broader class o practical applicatios. REFERENCES [] S. Berard, K. Ega, ad J. Trikle, Cotact modes ad complemetary coes, i Robotics ad Automatio, 24. Proceedigs. ICRA IEEE Iteratioal Coerece o, vol. 5, april- may 24, pp Vol.5. [2] S. Berard, Cookig wit complemetarity: A recipe guide or complemetarity based rigid-multi-body dyamics simulatio, 26. [3] J. C. Trikle, Formulatio o multibody dyamics as complemetarity problems, ASME Coerece Proceedigs, vol. 23, o. 3733, pp , 23. [Olie]. Available: ttp://lik.aip.org/lik/abstract/asmecp/v23/i3733/p36/s [4] P. L. Fackler ad M. J. Mirada, Matlab lemke algoritm, 997. Positio, m Positio, m Positio, m Positio, m COM o meter stick.5 Figer Figer (a) v =.5 m/s COM o meter stick.7 Figer Figer (b) v = m/s COM o meter stick.7 Figer Figer (c) v = 5 m/s COM o meter stick.7 Figer Figer (d) v = m/s Fig. 5. Te velocity o te movig iger is varied. We te velocity is icreased, te cotact mode switcig becomes less distict, ad evetually, te meter stick trick ails.

Cooking with Complementarity: A Recipe Guide for Complementarity Based Rigid-Multi-Body Dynamics Simulation

Cooking with Complementarity: A Recipe Guide for Complementarity Based Rigid-Multi-Body Dynamics Simulation Cookig wit Complemetarity: A Recipe Guide or Complemetarity Based Rigid-Multi-Body Dyamics Simulatio Stepe Berard August 25, 26 1 Itroductio Te ocus o tis report is to clariy te complemetarity based ormulatio