Implicit Constraint Enforcement for Rigid Body Dynamic Simulation

Transcription

1 Implicit Constaint Enfocement fo Rigid Body Dynamic Simulation Min Hong 1, Samuel Welch, John app, and Min-Hyung Choi 3 1 Division of Compute Science and Engineeing, Soonchunhyang Univesity, 646 Eupnae-i Shinchang-myeon Asan-si, Chungcheongnam-do, , Koea Min.Hong@UCHSC.edu Depatment of Mechanical Engineeing, Univesity of Coloado at Denve and Health Sciences Cente, Campus Box 109, PO Box , Denve, CO 8017, USA {Sam, jtapp}@cabon.cudenve.edu 3 Depatment of Compute Science and Engineeing, Univesity of Coloado at Denve and Health Sciences Cente, Campus Box 109, PO Box , Denve, CO 8017, USA Min-Hyung.Choi@cudenve.edu Abstact. he pape pesents a simple, obust, and effective constaint enfocement scheme fo igid body dynamic simulation. he constaint enfocement scheme teats the constaint equations implicitly poviding stability as well as accuacy in constained dynamic poblems. he method does not equie ad-hoc poblem dependent paametes. We descibe the fomulation of implicit constaint enfocement fo both holonomic and non-holonomic cases in igid body simulation. A fist ode vesion of the method is compaed to a fist ode vesion of the well-known Baumgate stabilization. 1 Intoduction Rigid body simulation is highly impotant in the modeling of vaious physical systems and has been compehensively studied in electical and mechanical engineeing. Rigid body dynamics involves a vaiety of holonomic constaints such as ball-andsocket, hinges, slides, univesal joints, and contact constaints. In addition, nonholonomic constaints have been used in simulations of obots and cas. In igid body dynamics, constaint enfocement is citical to guaantee successful simulations since small but accumulating constaint dift could cause instabilities. Constaint satisfaction though the use of Lagange multiplies epesents a challenging poblem numeically as the esulting system of equations is a mixed ODE algebaic system. Ealy wokes solved this poblem by diffeenting the constaint equations eplacing the algebaic equation with an ODE but the solutions exhibited numeical dift in the constaint eo. Baumgate s stabilization method [1] was intoduced to educe this numeical dift using paamete dependent stabilization tems and is still widely used today due to it s simplicity. In this pape, we popose an implicit holonomic and non-holonomic constaint enfocement method fo igid body dynamics that povides numeical stability and accuacy without equiing ad-hoc stabilization paamete tems while poviding the same asymptotic computational cost as the Baumgate method. he appoach is to implicitly expand the algebaic constaint in a aylo seies to the same ode as the ode of V.N. Alexandov et al. (Eds.): ICCS 006, Pat I, LNCS 3991, pp , 006. Spinge-Velag Belin Heidelbeg 006

2 Implicit Constaint Enfocement fo Rigid Body Dynamic Simulation 491 the numeical method used to integate the ODE system. he aylo expansion is implicit in the state vaiables thus the method has desiable stability chaacteistics. Related Wok he seminal wok of Baumgate [1] included second ode feedback tems to stabilize constaints and has been used successfully fo many applications [, 3, 4, 6]. Numeous impoved methods have appeaed ove the decades since Baumgate pesented his method but Baumgate s stabilization is widely used due to its simplicity and the ease with which it is undestood. he main dawback to Baumgate stabilization is that the equied paametes must be chosen in an ad-hoc manne. Anothe appoach is poststabilization in which a coection is made to the state vaiables in ode to minimize the constaint eo. An example of this is Cline and Pai [5] who poposed a poststabilization appoach fo igid body simulation. Post-stabilization equies additional computational load to einstate the accuacy in the constaint and it can cause eo in the motion of objects unde complicated situations since the constaint dift eduction is pefomed independently fom the confoming dynamic motions [7]. 3 Implicit Holonomic Constaint Enfocement In this section a simple implicit constaint enfocement method is pesented and compaed to Baumgate s stabilization. It will be shown that a cetain selection of Baumgate paametes will esult in the two methods being nealy identical fo the holonomic case. A stability analysis will be pesented and used as a guide to select paametes fo Baumgate s stabilization technique. Let Φ be the vecto of holonomic constaints. he equations of motion fo the holonomically constained system ae witten as π M +Φ λ = F (1) J ω +Φ λ = ωjω () ( e) Φ, = 0 (3) 1 e = G () e ω (4) Whee ae the cente of mass coodinates, e ae the Eule paametes associated with the oientation of the igid bodies, Φ and Φπ ae the constaint jacobians associated with and the oientation, espectively and λ is a vecto containing the Lagange multiplies. M and J ae inetia matices and ω is the skew-symmetic matix associated with the angula velocity ω. he matix defining the time deivative of the Eule paametes and the angula velocity is given by: -e1 e0 e3 -e G(e) = -e -e3 e0 e1 -e3 e -e1 e0 (5)

3 49 M. Hong et al. Equations (1), () and (4) ae discetized to fist ode as: 1 t ( +Δ t) = t + ΔtM F ΔtM Φ λ (6) t ( +Δ t) = t + Δ tt ( +Δt) (7) ω( t+δ t) = ω() t +ΔtJ ΔtJ ω() t Jω() t ΔtJ Φπ λ (8) Δt e( t+δ t) = e( t) + G( e) ω( t+δ t) (9) he basic philosophy of the implicit constaint enfocement technique we use is to expand the algebaic constaint function in a aylo seies to the same ode as the discete equations. he esulting expansion is: Φ ( t+δ t) = Φ ( t) + Φ ( t+δ t) +Φ ( t+δt) Δ t = 0 (10) { π ω } Equations (6) though (8) may be used to eliminate the new time velocity tems to obtain the linea system that must be solved fo the Lagange multiplie: Φ 1 { } Φ M Φ +Φ π J Φ π λ = + Φ +Φ M F J J t t π ω + Φ +Φ π ω ω (11) Δ Δ In this equation and in all equations that follow tems without explicit time dependence indicated ae evaluated at old time. Once this system is solved fo the Lagange multiplies equations (6) though (9) ae used to update the othe vaiables. Note that this linea system is symmetic positive definite. Baumgate s stabilization technique may be used fo this system in the following way. Fist the constaint is stabilized by adding two tems to the second deivative with paametes α and β as follows: Φ+ αφ+ β Φ= 0 (1) A simila pocedue is used to obtain the linea system to be solved fo the Lagange multiplies: { Φ M Φ +Φπ J Φ π} λ = β Φ+ α( Φ +Φ πω ) +ΦM F (13) +Φ J ( ωjω) + Φ + Φ ω ω ( ) π π π As with the simple implicit method, once this system is solved fo the Lagange multiplies equations (6) though (9) ae used to update the othe vaiables. Baumgate stabilization is often citicized because selection of the paametesα and β is ad-hoc and poblem dependent. Geenwood [9] suggests that both paametes should be popotional to the time step and a cusoy examination of equations (1) and (10) indicates that the choices α = 0.5Δ t and β =Δt would esult in ou method and Baumgate s stabilization diffeing by only the last two tems in equation (1). In what follows we analyze the stability of these schemes and use the esult to guide us in a selection fo the paametesα and β. In ode to analyze the stability the schemes descibed above we lump the cente of gavity and oientation vaiables into a single vecto q and we conside a lineaized system with lineaized constaints with constaint gadient given by

4 Implicit Constaint Enfocement fo Rigid Body Dynamic Simulation 493 Φ q ( q ) = Β (14) he dynamic system will have only constaint foces as we wish to isolate the effect of constaint enfocement on stability. he discetized homogeneous equations of motion may be witten as: ( +Δ ) = Δ Β ( +Δ ) = +Δ ( +Δ ) q t t q t tm λ (15) q t t q t tq t t (16) Note that the mass matix and inetia matix ae now lumped togethe. he linea system that must be solved fo the Lagange multiplie becomes. 1 1 ΒM Β λ = Β q() t + Βq () t (17) Δt Δt he Lagange multiplies may now be eliminated and the esulting linea state-space fom of the equations of motion is whee whee qt ( +Δt) qt =Χ qt ( +Δt) qt Ι M Β Α Β Δt Ι M Β Α Β Χ = M Β Α Β Ι M Β Α Β Δt Α=ΒM Β. he condition fo stability of this system of equations is (18) (19) ρ ( Χ) 1 (0) whee ρ (X) is the spectal adius of the matix X. A simila esult may be deived fo the Baumgate method with the esult Whee qt ( +Δt) qt =Λ qt ( +Δt) qt ( α ) t β M t t M Λ = tβ M tαm he condition fo stability fo the Baumgate method is Ι Δ Β Α Β Δ Ι Δ Β Α 1 Β Δ Β Α Β Ι Δ Β Α Β (1) () ρ ( Λ) 1 (3) Note that the choices α = 0.5Δ t and β =Δ t would esult in the Baumgate stabilization method having the same spectal adius as ou implicit method as the diffeing

5 494 M. Hong et al. tems wee discaded in the lineaization. In ode to investigate the stability of the methods futhe we conside the simple poblem of a plana double pendulum with links modeled by point masses. he equations of motion ae witten using Catesian coodinates thus we have two constaints that foce the link lengths to be constant. he link lengths consideed ae unity as ae the point masses. he constaints ae lineaized about a co-linea position and the esulting constaint jacobian fo this lineaized system is Β= (4) 0 0 Fo this case, ρ(x) 1 fo all Δt thus the implicit constaint enfocement method is unconditionally stable. he stability behavio of the Baumgate s stabilization depends on the selection of the paametes α and β. Fo example, if we select α,β = 10 the esulting stability limit is Δt If we select paametes α =Δt and β =Δt which give us a citically damped esponse with time constant popotional to Δt we will find that the system is unstable. heein lies the difficulty in paamete selection fo Baumgate s stabilization technique. Impope choices fo the paametes α and β often esults in time step limitations due to stability o in the time constants associated with constaint satisfaction being too slow. One of the advantages of the simple implicit constaint enfocement scheme descibed in this pape is that the seach fo a good choice of paametes need not be undetaken and, as we will show, the method poduces accuate esults. Fo the compaisons in the est of this pape we will use the paamete selections α = 0.5Δ t and β =Δt fo the easons cited above. We note that these paamete selections specify a damping coefficient of 0.5 and a time constant on the ode of the time step fo constaint satisfaction. 4 Implicit Non-holonomic Constaints Enfocement Non-holonomic constaints typically estict velocities fo points on objects. In the development that follows the implicit constaint enfocement technique is developed using functional dependences on the cente of gavity coodinates only. Extension to dependence on oientation coodinates follows identical development. he equations of motion fo the non-holonomic constaint can be witten as N Φ = v (5) Following ou ealie philosophy the constaint witten at new time is N o Φ ( t ( +Δ t)) t ( +Δ t) = v( t+δt) (6) Next a aylo seies expansion is witten to the same ode as the discete equations: N ( t t ) t t t Φ ( t ( +Δ t)) t ( +Δ t ) = Φ ( t ) t + Φ ( t ) t ( t ( +Δt) t ) (7) + Φ (())() (( +Δ ) ()) N N N o

6 Implicit Constaint Enfocement fo Rigid Body Dynamic Simulation 495 whee the subscipt indicates patial diffeentiation with espect to. Afte simplification we have: Φ (( t+δ t))( t +Δ t) = Φ (())() t t (( t+δt) t ()) + Φ (())( t t +Δt) (8) N N N Equation (7) may be used to eliminate t ( +Δt) to obtain Φ ( t ( +Δ t)) t ( +Δ t) =Δt Φ ( t ) t t ( +Δ t) + Φ ( t ) t ( +Δt) (9) N N N he fist tem may be witten to second ode eplacing t ( +Δt) with t esulting in Φ ( ( t+δ t)) ( t+δ t) = Δt Φ ( ( t)) ( t) ( t) + Φ ( ( t)) ( t+δt) (30) N N N Equation (6) may be used to eliminate t ( +Δt) and the esult is substituted into Equation (3) to obtain the linea system that must be solved fo the Lagange multiplie: 1 1 v ( t+δt) ΦM Φ λ = Φ + Φ M F + Φ (31) Δt Δt N N N N N o Note that we obtain the desiable symmetic positive definite linea system as we did with the holonomic case. he development fo the case with oientation vaiable dependence of the constaints is identical. Once this system is solved fo the Lagange multiplies equations (6) though (9) ae used to update the othe vaiables. Baumgate s stabilization fo the non-holonomic case begins with the constaint function witten as g(t) = 0. Baumgate s stabilization may be expessed as (Geenwood [8]). Fo ou case we wite and Baumgate s stabilization becomes t o g + αg + β gdt = 0 (3) N gt () = Φ v = 0 t Φ M Φ λ = v + Φ + Φ M F+ α Φ v + β Φ v dt (34) N 1 N N N 1 N N o o o 0 Note again the appeaance of exta tems in when using Baumgate s stabilization and in paticula the appeaance of the integal that must be calculated. In the esults that follow we evaluate the integal with a fist-ode method consistent with the method used to integate the ODE s. 5 Expeiments o veify the accuacy of the poposed holonomic and non-holonomic implicit constaint enfocement, we pefomed a thee-link igid ba simulation in Fig. 1. hee sepaate igid bas ae connected by thee spheical joints. he non-holonomic o (33)

7 496 M. Hong et al. (a) Initial state of igid ba (b) he motion of igid-ba duing simulation Fig. 1. he thee-link igid ba simulation with holonomic and non-holonomic constaints. he geen cicle indicates the tajectoy of the point whee the non-holonomic constaint is applied. (a) Implicit constaint eo using Δ t = 0.01 (b) Baumgate constaint eo using Δ t = 0.01 (c) Implicit constaint eo using Δ t = (d) Baumgate constaint eo using Δ t = Fig.. Compaison of holonomic and non-holonomic constaint eos fo thee-link igid ba simulation

8 Implicit Constaint Enfocement fo Rigid Body Dynamic Simulation 497 constaint is applied on the top end of the fist link. he point in the end of this link has a cicula velocity pescibed. he constaint is integable but we teat it as a nonholonomic constaint fo illustation puposes. he pescibed velocity esults in the cicula movement of the end point of the link in the x-y plane. We ecoded the holonomic and non-holonomic constaint eos ove a time duation of 50 seconds and the esults ae shown in Fig. he Baumgate stabilization paametes used aeα = 0.5Δ t and β =Δ t. he esults shown indicate somewhat bette esults fo the non-holonomic constaint eos with ou method and simila esults fo the holonomic constaint eos. hese esults ae typical of what we have obseved and we note that if we do not include the integal in the Baumgate method the esults ae less accuate. 6 Conclusion his pape descibes an implicit constaint enfocement method fo igid body simulations with both holonomic and non-holonomic constaints. Ou implicit constaint enfocement method has seveal advantages. his method is implicit thus it has desiable stability chaacteistics. he method does not equie poblem dependent paametes and does not equie an integal computation fo the non-holonomic case. he method is simple to implement, equiing fewe tems than Baumgate stabilization. he method esults in a symmetic positive definite linea system identical to that in Baumgate stabilization. he method is easily extended to second ode and the second ode method will appea in futue wok. Refeences 1. J. Baumgate, Stabilization of constaints and integals of motion in dynamical systems, Compute Methods in Applied Mechanics, pp. 1-16, D. Baaff, Linea-time dynamics using Lagange multiplies, In poceedings of SIGGRAPH 1996, ACM Pess / ACM SIGGRAPH, Compute Gaphics Poceedings, pp , R. Bazel, and A. Ba, A modeling system based on dynamic constaints, Compute Gaphics, (4), pp , J. C. Platt, and A. Ba, Constaint methods fo flexible models, Compute Gaphics, (4), pp , M. B. Cline, and D. K. Pai, Post-stabilization fo igid body simulation with contact and constaints, In poceedings of the IEEE Intenational Confeence on Robotics and Automation, pp , H. Jeon, M. Choi, and M. Hong, Numeical stability and convegence analysis of geometic constaint enfocement in dynamic simulation systems, In poceedings of the Intenational Confeence on Modeling, Simulation and Visualization Methods, pp , M. Hong, M. Choi, S. Jung, S. Welch, and J. app, Effective constained dynamic simulation using implicit constaint enfocement, in Poceedings of the 005 IEEE Intenational Confeence on Robotics and Automation, pp , D. Geenwood, Advanced Dynamics, Cambidge, 003