Orthogonal Complement Based Divide-and-Conquer Algorithm (O-DCA) for Constrained Multibody Systems

Transcription

1 Orthogonal Complement Based Divide-and-Conquer Algorithm (O-DCA) for Constrained Multibody Systems Rudranarayan M. Mukherjee, Kurt S. Anderson Computational Dynamis Laboratory Department of Mehanial Aerospae and Nulear Engineering. Rensselaer Polytehni Institute 110 8th Street. Troy NY USA Marh 15, 2006 Abstrat Keywords: Closed Kinemati Loops, Orthogonal Complement, Singular Configurations, Logarithmi Computational Complexity, Divide and Conquer. A new algorithm is presented in this paper for alulating the forward dynamis of multi-rigid bodies onneted together by kinemati joints to form single or oupled losed loop topologies. The algorithm is exat and non-iterative. The onstraints are imposed at the aeleration level by utilizing a kinemati relation between the joint motion subspae (or partial veloities) and its orthogonal omplement. Sample test ases indiate exellent onstraint satisfation and robust handling of singular onfigurations. Sine the present algorithm does not use either a redution or augmentation approah in the traditional sense for imposing the onstraints, it does not suffer from the assoiated problems for systems passing through singular onfigurations. The omputational omplexity of the algorithm is expeted to be O(n + m) and O(log(n + m)) for serial and parallel implementation respetively, where n is the number of generalized oordinates and m is the number of independent algebrai onstraints. 1 Introdution Computer simulation and assoiated analysis of the dynami behavior of multibody systems is an essential tool for engineers and researhers working in various fields. The involved appliations inlude, but are not limited to, terrestrial and spae vehiles, bio-mehanial systems, materials modelling, robotis and manufaturing proesses. For suh model-based engineering to be effetive, it is essential that the simulation tools used be omputationally effiient, aurate, and robust. Thus, the development of algorithms to model multibody system dynamis has been an ative area of researh. Several algorithms of various omputational omplexities have been presented in the literature. The earliest algorithms for artiulated body systems were of O(n 3 ) omplexity [1] (the number of omputational operations inrease as a ubi funtion of n, per integration step), with n being the number of degrees of freedom of the system. In the late 1970s through the early 1990s emphasis was plaed by a number of researhers on the development of lower omputational order (ost) algorithms [2]-[5]. Several algorithms were independently derived and developed by various authors for solving the multi-rigid body and multi-flexible dynamis problem in O(n) omplexity [6]-[13]. A brief review and the underlying similarities between many of these different algorithms is disussed in referene [14]. Many multibody systems of sientifi and engineering interest inlude topologies with losed kinematial loops. In these situations, the losed kinematial loops are most often modelled by produing a set of n equations of motion assoiated with the unonstrained system, and a ompanion set of m independent algebrai onstraint equations whih must be satisfied throughout the solution of the equations of motion. These onstraint equations may then be used to either: i) Redue out exessive degrees of freedom produing a 1

2 r Position vetor ω i Absolute angular veloity of referene frame i on body i,a3 1 matrix v i Absolute translational veloity of a point i,a3 1 matrix α i Absolute angular aeleration of referene frame i on body i,a3 1 matrix a i Absolute translational aeleration of a point i,a3 [ 1 matrix ] V i ω i Spatial veloity of point i assoiated with frame i = v i [ 6 1] A i α i Spatial aeleration of point i assoiated with frame i = a i 6 1 H k/(k+1) Joint motion map of the kinemati joint between bodies k and k+1 D k/(k+1) Orthogonal Complement of the kinemati joint between bodies k and k+1 J k+ Joint onneting bodies k-1 and k J k Joint onneting bodies k and k+1 q Generalized relative oordinate u Generalized relative speed u Time derivative of generalized relative speed ζ Inertia oupling terms for individual body or subassembly Υ Inertia oupling terms for assembly i τ Constraint torque at joint i,3 1 olumn matrix f i i τ i f F i F i b n m bb and tb sys asm C T U Z Ŵ X Ŷ X Y K D Constraint fore at joint i,3 1 olumn matrix i Measure numbers of onstraint torque at joint i, i.e. ordered list of non-zero elements of τ Measure numbers of onstraint fore at joint i, i.e. ordered list of non-zero elements of f i [ ] i τ Spatial onstraint fore at joint i = f i 6 1 Measure numbers i.e ordered list of non-zero elements of F i = [ ] i τ f i (6 dof) skew symmetri matrix for ross produt of any vetor b The number of generalized oordinates in the system The number of independent algebrai onstraints Base and Terminal joints through whih the four bar linkage shown in Figure (3) onnets to ground Supersript representing the whole system as a single body Supersript representing the bodies 2,4,5 and 6 from Figure(4) as a single body Transpose of any arbitrary matrix C Identity matrix Zero matrix Useful Intermediate Quantity Useful Intermediate Quantity Useful Intermediate Quantity Useful Intermediate Quantity Useful Intermediate Quantity Useful Intermediate Quantity Useful Intermediate Quantity All bold faed symbols and letters represent matrix quantities Table 1: The Nomenlature minimum dimension system of n m equations; or ii) Augment the equations of motion produing a larger n + m dimension system of equations involving m redundant state variables. The first O(n) methods for 2

3 systems with losed kinematial loops were independently presented in referenes [15][16] and were of the augmentation ategory, while oordinate partitioning [17] and Reursive Coordinate Redution [18][19] were of the redution type. Whihever type of proedure is used, there are two prinipal problems enountered when dealing with systems with losed kinematial loops, viz. (1) the saddle point problem originating from onstraint equations beoming linearly dependent and (2) the aumulation of integration errors leading to signifiant drift in onstraint satisfation. The saddle point problem is typially enountered when the system passes through a singular onfiguration whereby the onstraint Jaobian beomes rank defiient. A related problem may additionally our for redution approahes when the dependeny matrix relating the dependent state derivative(s) to the independent state derivative(s), whih is neessary for suh a formulation, loses rank. By omparison, the problem of onstraint violation error drift an be traed bak to the fat that unless some form of onstraint stabilization approah is used, the onstraint are most often imposed at the aelerations, or possibly the veloity level. Imposing the onstraints at the aeleration level results in an eventual unstable growth in onstraint violation whih an grow exponentially for a given simulation. The error in onstraint violation ours due to the aumulation of round-off errors from finite preision arithmeti and the introdution of two zero eigenvalues (one assoiated with eah onstant of temporal integration) for eah aeleration level onstraint. To overome this problem some form of onstraint stabilization is often introdued into the equations. Constraint stabilization tehniques have been used in various forms for a number of years. The most ommon of these an be found in referene [20]-[26]. Although introduing a onstraint stabilization tehnique an redue the drift in the onstraint violation signifiantly, these methods do not generally provide full onstraint satisfation, and ome with their own (potentially signifiant) omputational ost. Unfortunately, in many situations involving stiff systems and/or systems repeatedly passing near and through singular onfigurations, the onstraint violation errors an grow rapidly and result in a signifiant loss of auray, making some form of stabilization essential. Another onern whih may arise when dealing with omplex systems is the onsiderable additional expense inurred when dealing with heavily onstrained systems (m n). With so-alled O(n) omplexity algorithms, the simulation turn around times sale linearly with the inrease in system size (number of generalized oordinates n) and hene are more effiient than the traditional O(n 3 ) approahes when dealing with artiulated body systems where n 1. Unfortunately, these algorithms do not perform quite as well when one is dealing with systems involving many kinemati loops. In suh instanes these so-alled O(n) algorithms atual perform as O(n + nm 2 + m 3 ). Other strategies exist [18][19][27][28] whih offer O(n + m) overall performane, but these proedures are signifiantly less easy to implement and are not as a rule appliable to all system topologies. If one wishes to exploit the potential advantages in redued simulation turnaround time through the use parallel omputing, then the theoretial lower limit on effetive ost per integration step (turn around time per temporal integration step) is O(log(n)). Thus, if the omputations are suffiiently oarse grain parallel and inter-proessor ommuniations osts (e.g. ommuniations lateny and data transfer) are adequately low, then substantial gains may be potentially realized through the use of parallel omputing. The first parallel algorithm whih was both time optimal O(log(N)) turn around time per temporal integration step) and proessor optimal (theoretially ahieves this O(log(N)) turn around with only O(N) proessors) was presented in [29], but was limited to hain systems of N bodies. In [30][31] a Divide and Conquer Algorithm is presented that an ahieve O(log(n)) omplexity when implemented on O(N) proessors in parallel and is appliable for general topologies. The extension of the algorithm for systems with losed kinematial loops uses a onstraint stabilization method together with a formulation utilizing Lagrange multipliers. The method an degenerate to O(n 3 ) omplexity (if solved sequentially) in the worst ase. Moreover a neessary matrix used for dealing with loops within the proedure an beome rank defiient and in suh ases the method requires an alternate formulation. In this paper, an algorithm is proposed for handling systems with losed kinematial loops. The proposed method uses a Divide and Conquer formulation similar to that in [30]. This formulation is time and proessor optimal, but does not inlude oordinate redution or Lagrange multipliers for onstraint imposition. The proedure implements the spatial Newton-Euler formulation in a Divide and Conquer sheme and imposes the onstraints at the aeleration level by using a kinemati relationship involving the orthogonal omplement of the joint motion subspae. The method proposed an easily handle systems in truly singular onfigurations, and is appliable for general systems ontaining either single losed loops or multiple oupled losed loops. 3

4 2 Analytial Preliminaries This setion presents a brief review of the Divide and Conquer algorithm for serial hains as found in [30]. This bakground is essential to understand the handling of non-terminal bodies in the losed loop topologies beause this treatment is effetively similar to the method presented in [30] for hain systems. In setion (3) the new proedure is presented for handling loop losure onstraints at the terminal bodies. The basi unit of the DCA sheme is the two-handle representation of a body. A handle is any seleted point on the body whih is used in modelling the interations of the body with the environment. The handles on a body an orrespond to a joint loation, a enter of mass or any desired referene point. The two handles an even oinide. A body an have any number of handles on it. For the algorithm presented here, the joint loations are hosen as the handles on the body. Consider two representative bodies Body k and Body k +1of the artiulated body system as shown in Figure (1). The two handles on Body k orrespond to the joints J k+ and J k. Similarly, the two handles on Body k +1orrespond to the joints J k+1+ and J k+1. Using a spatial Newton-Euler formulation, the equations of motion of a representative body Body k of the system an be written at the two handles as below A k+ = ζ k 11F k+ A k = ζ k 21F k+ + ζ k 12F k + ζ k 13 (1) + ζ k 22F k + ζ k 23 (2) Here A k+ and A k are the spatial aelerations of the body at the handles at J k + and J k, respetively. The terms ζ k ij (i =1, 2 j =1, 2) are the inverse of the spatial inertia terms while the terms ζ k i3 (i =1, 2) are the inertia dependent bias terms. The bias terms also ontain ontributions from any fores applied to the body whih are determinable diretly from the system state. These ative fores inlude body fores, atuator fores, spring-damper fores et. The terms F k+ and F k are the unknown onstraint fores ating on the body at the joint loations. The interations of the body with the rest of the system are ahieved through these onstraint fores. At the beginning of the simulation, the inertia dependent terms viz. ζ k ij as well as the ative fores for eah body are either known or an be easily alulated from the state of the system. The above equations then redue to two sets of equations in two sets of unknowns viz. the spatial aelerations (A k+, A k ), and the onstraint fores (F k+, F k ). This set of equations are heneforth referred to as the two handle equations of motion of a Body k. Similarly the two handle equations of motion for Body k+1 an be written in the form A k+1+ = ζ k+1 11 F k+1+ + ζ k+1 12 F k+1 + ζ k+1 13 (3) A k+1 = ζ k+1 21 F k+1+ + ζ k+1 22 F k+1 + ζ k+1 23 (4) There are two main proesses in the DCA approah, a hierarhi assembly proess and a hierarhi disassembly proess. In the hierarhi assembly proess, the equations of motion of eah body are written in terms of the aelerations at eah of its two handles (the proedure may be easily generalized to bodies with more than two handles). The two handle equations of motion of two suessive bodies are then oupled together to form the two handle equations of motion of the resulting assembly using a reursive set of formulate derived in setion (2.1) A k+ = Υ 11 F k+ A k+1 = Υ 21 F k+ + Υ 12 F k+1 + Υ 13 (5) + Υ 22 F k+1 + Υ 23 (6) The two handles of the resulting assembly are the inward joint of the Body k (viz. J k+ ) and the outward joint on the Body k+1 (viz. J k+1 ) and the onstraint fores are those ating on the resulting assembly at those handles. The inertia oupling terms, Υ ij, for the resulting assembly are alulated using a reursive set of formulae as disussed in setion (2.1) of this paper. This proess begins at the level of individual bodies of the system. Adjaent bodies of the system are hierarhially assembled to onstrut a binary tree as shown in Figure (2). Individual bodies that make up the system form the leaf (base) nodes of the binary tree. The equations of motion of a pair of bodies are oupled 4

5 Joint Motion Subspae H k / k+1 F k -- F k+1+ J k -- J k+1+ Body K Body K+1 J k + J k+1 -- F k+ F k+1 -- Figure 1: Two Handle Artiulated Body together using the reursive set of formulae to form the two handle equations of motion of the resulting assembly. The resulting assembly now orresponds to a node of the next level in the binary tree. Working up the binary tree in this hierarhi assembly proesses, only a single assembly is left as the root (top) node of the binary tree. The root (top) node orresponds to the two-handle representation of the entire artiulated system modelled as a single assembly. The two handles on this body orrespond the boundary joints of the artiulated system. The proedure for solving the equations of motion of the top node using the boundary onditions is disussed in detail for systems with kinematially losed loops in setion (3). Using the proedure outlined in there, the spatial aelerations and the onstraint fores on the top node at the terminal handles an be generated. The hierarhi disassembly proess begins with the solution of the two-handle equations of motion of the top node. From this solution, the spatial aelerations of and fores on the two handles of the single assembly are known. The spatial aeleration and onstraint fores generated by solving the two handle equations of an assembly are identially the values of the spatial aelerations and onstraint fores on one handle on eah of the two onstituent assemblies. From these known quantities, the two handle equations of motion of the onstituent assemblies an be easily solved to obtain the onstraint fore and spatial aeleration at the onneting joint. For example, for a representative assembly made from Body k and Body k+1, the equations of motion are given by equations (5-6). On solving these equations the quantities A k+,a k+1,f k+ and F k+1 are generated. These quantities are then substituted into the two-handle equations of the onstituent sub-assemblies say for Body k and Body k+1. Thus knowing the values of A k+,f k+, equations (1-2) an be easily solved, while from A k+1 and F k+1 equations (3-4) an be solved. This proess is repeated in a hierarhi disassembly of the binary tree where the known boundary onditions are used to solve the two-handle equations of motion of the immediate subassemblies, until spatial aeleration and onstraint fores on all bodies in the system are alulated. Similar to the sheme in [30], this algorithm works in four sweeps, traversing the system topology like a binary tree. The first and the third sweep work upwards from the leaf (base) nodes of the binary tree to the top node while the seond and the fourth sweep work downwards. The input to this algorithm is omprised of the mass properties of the bodies, joint generalized oordinates and speeds. The first two sweeps generate 5

6 Single Assembly Top Node HIERARCHIC DISASSEMBLY Resulting Assemblies Higher Level Nodes Resulting Assemblies Higher Level Nodes HIERARCHIC ASSEMBLY Constituent Bodies Base Nodes Figure 2: The Hierarhi Assembly and Disassembly Proess using Binary Tree Struture the position and veloity of eah handle on eah node by using an assembly-disassembly proess similar to that desribed in [30]. On ompleting the two sweeps, on eah base node, the oordinate transformations, the state dependent aelerations, the ative joint fores are alulated. The ative fores are state dependent and inlude atuator fores on the joints, damping fores and body fores like gravity. The final two sweeps orrespond to the hierarhi assembly and the hierarhi disassembly proesses respetively. In the analytial treatment presented here, diretion osine matries and transformation between different basis are not shown expliitly. Appropriate basis transformations have to be taken into aount for an implementation of this algorithm. Also, this algorithm uses a redundant mixed set of oordinates, viz. Cartesian oordinates and relative oordinates, throughout the derivation. Mixed set of oordinates has been used in [32]-[33] for rigid body dynamis. 2.1 Reursive Formulae The relative aeleration at the joint onneting Body k and Body k +1is given by the following equation N A k+1+ N A k = H k/(k+1) u + Ḣ k/(k+1) u (7) In this equation, H k/(k+1) is the joint motion subspae matrix, u and u are the relative generalized speeds, and relative generalized aelerations (translation and/or rotation), respetively, at the joint. The olumn vetor(s) of the joint motion subspae matrix are the basis of the spae whih ontains the ative fores at the joint and the joint degrees of freedom (dof). It an be interpreted as the 6 dof matrix that maps the dof generalized speeds at the joint into a 6 1 olumn matrix of spatial relative veloity at the joint. Matrix D k/(k+1) is defined to be the orthogonal omplement of the joint motion subspae matrix H k/(k+1). While H k/(k+1) is a 6 dof matrix orresponding to the dof 1 olumn matrix of joint degrees of freedom, D k/(k+1) is a 6 (6 dof ) matrix that maps the onstrained degrees of freedom of the joint. The olumn vetor(s) of D k/(k+1) are the basis of the spae in whih the onstraint fores ating on the joint lie (i.e. the olumn matries are the basis of this spae where the joint an support onstraint fores). For example, in a spherial joint, the translational degrees of motion are onstrained while the rotational degrees of freedom 6

7 are maintained. Hene the orresponding maps maybe given by H k/(k+1) = D k/(k+1) = (8) By definition of the orthogonal omplement H k/(k+1) and D k/(k+1) satisfy the following relation H k/(k+1)t D k/(k+1) = D k/(k+1)t H k/(k+1) =0 (9) Newton s Third Law requires the onstraint fore on joint J k+1+ viz. F k+1+ and joint J k viz. F k are equal in magnitude and opposite in diretion. Using this relation and substituting the expressions for N A k and N A k+1+ from equation (2) into equation (3) into equation (7), an expression for F k+1+ is obtained as [ζ k ζ k 22]F k+1+ =[ζ k 21F k+ ζ k+1 12 F k+1 + ζ k 23 ζ 13 k+1 + H k/(k+1) u + Ḣ k/(k+1) u] (10) F k+1+ =[ζ k ζ k 22] 1 [ζ k 21F k+ ζ k+1 12 F k+1 Premultiplying equation (10) by D k/(k+1 )T gives +ζ k 23 ζ k H k/(k+1) u + Ḣ k/(k+1) u] (11) D k/(k+1)t [ζ k ζ k 22]F k+1+ = D k/(k+1)t [ζ k 21F k+ + ζ k 23 ζ k+1 13 ζ k+1 12 F k+1 +Ḣ k/(k+1) u]+d } k/(k+1)t {{ H k/(k+1) } u (12) 0 From the definition of the orthogonal omplement of joint motion subspae, the onstraint fore F k+1+ be expressed in terms of the measure numbers of the onstraint torques and onstraint fores as an F k+1+ = D k/(k+1) F k+1 + (13) where the onstraint fore and onstraint moment measure numbers f k+1 + represented as [ ] k+1 + τ F k+1+ = f k+1 + and τ k+1 +, respetively, are (14) Substituting relation (13) into equation (12) yields D k/(k+1)t [ζ k ζ k 22]D k/(k+1) F k+1 + = D k/(k+1)t [ζ k 21F k+ ζ k+1 12 F k+1 +ζ k 23 ζ k Ḣ k/(k+1) u]) (15) The term D k/(k+1)t [ζ k ζ k 22]D k/(k+1) appearing in (15) is a Symmetri Positive Definite (SPD) matrix and hene there is no problem assoiated with its inversion. Defining the quantity X as F k+1+ may be determined as X D k/(k+1)t [ζ k ζ k 22]D k/(k+1) (16) 7

8 F k+1+ = X 1 D k/(k+1)t [ζ k 21F k+ ζ k+1 12 F k+1 + ζ k 23 ζ k Ḣ k/(k+1) u]. (17) The above expression (17) is then premultiplied by D k/(k+1) to get the desired expression for the spatial onstraint fore F k+1+, F k+1+ = D k/(k+1) F k+1 + = D k/(k+1) X 1 D k/(k+1) T [ζ k 21F k+ ζ k+1 12 F k+1 + ζ k 23 ζ k Ḣ k/(k+1) u] (18) The above expression for F k+1+ an be ompatly written as below F k+1+ = Ŵζk 21F k+ Ŵζk+1 12 F k+1 + Ŷ (19) where Ŵ = D k/(k+1) X 1 D k/(k+1)t (20) and Ŷ = Ŵ [ζk 23 ζ k Ḣ k/(k+1) u] (21) This expression for F k+1+ is substituted in equations (1) and (4), and after some algebrai manipulation, the two equations an be obtained as A k+ =[ζ k 11 ζ k 12Ŵζk 21]F k+ + ζ k 12Ŵζk+1 F k+1 + ζ k 13 ζ k 12Ŷ (22) and A k+1 = ζ k+1 21 Ŵζ k 21F k+ +[ζ k+1 22 ζ k+1 21 Ŵζ k+1 12 ]F k+1 + ζ k ζ k+1 21 Ŷ (23) Equations (22) and (23) an be onsidered as the two handle equations of motion of the resulting assembly of Body k and Body k +1. The two handles on this assembly are the joints J k+ and J k+1. Colleting terms in above equations, the two handle equations of motion of the assembly an be written as A k+ = Υ 11 F k+ A k+1 = Υ 21 F k+ +Υ 12 F k+1 +Υ 13 (24) +Υ 22 F k+1 +Υ 23 (25) where now Υ ij are the omposite inertia of the assembly. From the above, a reursive set of formulae for Υ ij an be obtained as Υ 11 =[ζ k 11 ζ k 12Ŵζk 21] (26) Υ 22 =[ζ k+1 22 ζ k+1 21 Ŵζ k+1 12 ] (27) Υ 12 = Υ T 21 = ζ k 12Ŵζk+1 (28) Υ 13 = ζ k 13 ζ k 12Ŷ (29) Υ 23 = ζ k ζ k+1 21 Ŷ (30) In the assoiated manipulations, the two bodies are oupled together to form an assembly by expressing the intermediate (ommon) joint onstraint fore in terms of the onstraint fores at the other two handles. This proess an now be repeated for all bodies in the system where the two handle equations of motion of two suessive bodies or assemblies are oupled together using the reursive formulae to obtain the two handle equations of the resulting assembly. This proess works hierarhially exploiting the same struture as that of a binary tree. At the end of the hierarhi assembly proess, the whole artiulated system may be modelled in terms of the two handle equations of motion of a single assembly. The methodology outlined here is effetively idential to the proedure outlined in [30], though some intermediate manipulations may appear to be different. The primary import of this setion is that an artiulated hain system an be modelled as a single assembly with handles at the base and terminal joints of the system. In the next setion, a new methodology is outlined that explains how the two handle equations of motion of the resulting assembly an be solved when the base and terminal joints are suh that the system redues to a kinematially losed loop. 8

9 q 4 q q 3 2 q 2 Figure 3: Simple Single Loop System 3 Proedure for Dealing with Kinemati Loops In this setion, a proedure is presented for the effiient, aurate and robust treatment of systems with kinemati loops. The proedure will first be demonstrated for a single loop system, and then will be generalized to systems ontaining multiple loops. 3.1 Single Loop Case Consider a N body system onneted together by kinemati joints. The base body and the terminal body of the system are onneted to the inertial frame, thus forming a loop system. Figure (3) shows the general topology of this system. Other than the kinemati joints linking the base and the terminal body to the inertial frame, there is no differene between this system and a hain system. Hene, proeeding in a manner as explained in setion 2, the n-body system an be modelled as a single assembly. The resulting two handle equations of motion of the assembly are obtained as shown below. A bb = ζ sys 11 F bb + ζ sys 12 F tb + ζ sys A tb = ζ sys 21 F bb + ζ sys 22 F tb 13 (31) + ζ sys 23 (32) Here bb and tb denote the joints at the base body and the terminal body by whih the system is onneted to the inertial frame. Also, sys implies the inertia oupling terms representing the whole system. Sine the system is attahed to an inertial frame, the aelerations at the two ends an be given by the following kinemati relation. A bb = H bb u bb + Ḣ bb u bb (33) A tb = H tb u tb + Ḣ tb u tb (34) where H bb and H tb represent the joint motion subspae of the joints bb and tb by whih the system is onneted to the inertial frame. Similarly, u bb and u tb represent the generalized relative aelerations at the joint degrees of freedom at bb and tb. The terms Ḣ bb u bb and Ḣ tb u tb represent the state dependent aeleration terms whih an be kinematially alulated before solving the equations of motion. Substituting the equations (33-34) into equations (31-32) and absorbing the state dependent aeleration 9

10 terms into the bias term, one obtains, H bb u bb = ζ sys 11 F bb + ζ sys 12 F tb + ζ sys H tb u tb = ζ sys 21 F bb + ζ sys 22 F tb 13 (35) + ζ sys 23 (36) The two above equations (35-36) ontain four unknowns F bb, F tb, u bb and u tb. To eliminate two unknowns, u bb and u tb, the kinemati relationship of the joint motion subspae and its orthogonal omplement as explained by equation (9) is exploited. Multiplying the above equations by (D bb ) T and (D tb ) T respetively, where D bb and D tb represent the orthogonal omplement of the respetive joint motion subspae, one obtains 0 {}}{ (D bb ) T H bb u bb = (D bb ) T [ζ sys 11 F bb + ζ sys 12 F tb (D tb ) T H tb }{{} 0 u tb = (D tb ) T [ζ sys 21 F bb + ζ sys 22 F tb + ζ sys 13 ]=0 (37) + ζ sys 23 ]=0 (38) The above are two equations in two unknowns, viz. F bb and F tb. But the matries (D bb ) T ζ sys ij and (D tb ) T ζ sys ij are rank defiient and hene these equations in their present form annot be solved. In order to solve these equations, the onstraint fores are expressed in terms of the measure numbers F bb tb and F assoiated with these fores as in equation (13) above. F bb = D bb F bb and F tb = D tb F tb (39) Substituting these expressions for the onstraint fores into the equations (37-38) one obtains (D bb ) T ζ sys 11 Dbb bb F +(D bb ) T ζ sys 12 Dtb tb F +(D tb ) T ζ sys 13 =0 (40) (D tb ) T ζ sys 21 Dbb bb F +(D tb ) T ζ sys 22 Dtb tb F +(D tb ) T ζ sys 23 =0 (41) In these equations, the terms (D bb ) T ζ sys 11 D bb and (D tb ) T ζ sys 22 D tb are symmetri positive definite (SPD) matries and there is no problem assoiated with their inversion. For notational onveniene, the above equations an be represented ompatly in matrix form as [ ] [ ] bb χ11 χ 12 ][ F χ13 χ 21 χ 22 F tb = (42) χ 23 where the orresponding χ ij an be derived from above equation. The matrix in (42) is also SPD with χ 12 = χ T bb tb 21. Having solved the above equations for the values of F and F, the orresponding expression for F bb and F tb an be obtained by pre-multiplying the orresponding expressions by (D bb ) and (D tb ) respetively as shown in equation (39). At this point, both onstraint fores on terminal and base joints are known. Consequently, the two handle equations of motion of the single assembly an be solved to obtain the spatial aelerations at the orresponding joints. This initiates the hierarhi disassembly proess disussed above whih suessively alulates the spatial aelerations and onstraint fores of sub-assemblies. This disassembly results in the spatial aelerations and onstraint fores alulated on eah physial body in the system. 3.2 Multiple Closed Loops The treatment of oupled losed loops is presented in this setion. The methodology presented here is appliable for ases where there are multiple loops as well as for ases when a single loop is onneted to a hain struture. Consider the double loop system shown in figure (4). As seen in the figure, bodies 1, 2, 3 form the upper loop while bodies 4, 5, 6 form the lower loop with body 2 being the ommon body shared between the two loops. The objetive is to redue the lower loop into a single assembly by oupling together the two handle equations of motion of bodies 2, 4, 5 and 6. This single assembly and the bodies in the upper loop then form a single loop system 10

11 (a) (b) Figure 4: Coupled Loop System Proeeding in a manner similar to a hain system, the bodies 4, 5 and 6 an be oupled together to form the two handle equations of motion of the resulting system as below. In this ase the joints 4 + and 6 are the base and terminal joints for the system sys representing the assembly of bodies 4,5, and 6 while ζ sys ij represent the inertia oupling terms of the assembly. A 4+ = ζ sys 11 F 4+ + ζ sys 12 F 6 + ζ sys 13 (43) A 6 = ζ sys 21 F 4+ + ζ sys 22 F 6 + ζ sys 23 (44) Similarly, the two handle equations of motion of the body 2 an be written as below. A 2+ = ζ 2 11F 2+ A 2 = ζ 2 21F 2+ + ζ 2 12F 2 + ζ 2 13 (45) + ζ 2 22F 2 + ζ 2 23 (46) where 2 + is the joint between bodies 2 and 1 while 2 is the joint between bodies 2 and 3. For body 2, the onstraint fores F 2+ and F 2 an be further analyzed as F 2+ = Fin 2+ + F ext 2+ (47) F 2 = Fin 2 + F ext 2 (48) Here Fin represents the fore ating on body 2 whih originates from the lower loop. This fore is an internal fore when the lower loop is onsidered as a single system. F ext is the fore ating on body 2 due to the interations with bodies 1 and 3. Thus if the lower loop were to be represented as a single assembly with handles at joints 2+ and 2, the terms represented by Finwould disappear and the terms F ext would represent the onstraint fores at the two handles of the resulting assembly. The objetive thus is to ouple 11

12 the equations of the resulting assembly of bodies 4, 5, 6 with the equations of body 2 to get the two handle equations of the resulting assembly. This assembly be referred to as asm heneforth. Consider the kinemati expression for the aelerations of the two handles of the assembly of bodies 4, 5, 6. These an be written as Also note that from Newton s seond law, A 4+ = A 2+ + H 4+ u 4+ + Ḣ 4+ u 4+ (49) A 6 = A 2 + H 6 u 6 + Ḣ 6 u 6 (50) F 4+ = Fin 2+ (51) F 6 = Fin 2 (52) Substituting the expressions of A 4+ from equation (49) and A 6 from equation (50) as well as the expressions of onstraint fores from equation (51-52) into the orresponding two handle equations of motion (43-44), we obtain A 4+ = A 2+ + H 4+ u 4+ + Ḣ 4+ u 4+ = ζ sys 11 Fin2+ ζ sys 12 Fin2 + ζ sys 13 (53) A 6 = A 2 + H 6 u 6 + Ḣ 6 u 6 = ζ sys 21 Fin2+ ζ sys 22 Fin2 + ζ sys 23 (54) Subtrating equation (53) from equation (45) and similarly equation (54) from equation (46), an expression for the relative joint aeleration an be obtained as H 4+ u 4+ = ζ 2 11F ext 2+ ζ 2 12F ext 2 H 6 u 6 = ζ 2 21F ext 2+ ζ 2 22F ext 2 +[ζ ζ sys 11 ]Fin2+ + +[ζ ζ sys 12 ]Fin2 +[ζ 2 13 ζ sys 13 + u Ḣ4+ 4+ ] (55) +[ζ ζ sys 21 ]Fin2+ + +[ζ ζ sys 22 ]Fin2 +[ζ 2 23 ζ sys 23 + u Ḣ6 6 ] (56) The orthogonal omplement D 4+ and D 6 are orthogonal to the joint motion subspae matries H 4+ and H 6. Hene premultiplying above equations (55) and (56) by (D 4+ ) T and (D 6 ) T and using the relation from equation (9), the following an be arrived at. (D 4+ ) T H 4+ u 4+ = 0=(D 4+ ) T ζ 2 11F ext 2+ (D 6 ) T H 6 u 6 = 0=(D 6 ) T ζ 2 21F ext 2+ (D 4+ ) T [ζ ζ sys ]F +(D 4+ ) T ζ 2 12F ext 2 (D 4+ ) T [ζ ζ sys 6 12 ]F +(D 4+ ) T [ζ 2 13 ζ sys 13 + u Ḣ4+ 4+ ] (57) (D 6 ) T [ζ ζ sys ]F +(D 6 ) T ζ 2 22F ext 2 (D 6 ) T [ζ ζ sys 6 22 ]F +(D 6 ) T [ζ 2 23 ζ sys 23 + u Ḣ6 6 ] (58) Further note that the onstraint fores an be expressed in terms of the measure numbers and the orthogonal omplement of the joint motion subspae i.e. F 4+ = D 4+ F 4 + and F 6 = D 6 F 6 (59) From these, equations (57) and (58) an be manipulated to generate an expression for the internal fores viz. Fin 2+ and Fin 2 as shown below. 12

13 (D 4+ ) T [ζ ζ sys ]F (D 4+ ) T ζ 2 11F ext 2+ +(D 4+ ) T [ζ ζ sys 6 12 ]F = +(D 4+ ) T ζ 2 12F ext 2 (D 4+ ) T [ζ ζ sys 11 ]D4+ F 4 + +(D 4+ ) T [ζ ζ sys 12 ]D6 F 6 = (D 4+ ) T ζ 2 11F ext 2+ (D 6 ) T [ζ ζ sys ]F (D 6 ) T ζ 2 21F ext 2+ +(D 4+ ) T ζ 2 12F ext 2 +(D 6 ) T [ζ ζ sys 6 22 ]F = +(D 6 ) T ζ 2 22F ext 2 (D 6 ) T [ζ ζ sys 21 ]D4+ F 4 + +(D 6 ) T [ζ ζ sys 22 ]D6 F 6 = (D 6 ) T ζ 2 21F ext 2+ +(D 6 ) T ζ 2 22F ext 2 +(D 4+ ) T [ζ 2 13 ζ sys 13 + Ḣ4+ u 4+ ] (60) +(D 4+ ) T [ζ 2 13 ζ sys 13 + Ḣ4+ u 4+ ] (61) +(D 6 ) T [ζ 2 23 ζ sys 23 + Ḣ6 u 6 ] (62) +(D 6 ) T [ζ 2 23 ζ sys 23 + Ḣ6 u 6 ] (63) In matrix format, the above equations (60-63) an be expressed as [ ] F 4 + = [ X ][ Y ] [ F ext 2+ ] F 6 F ext 2 + [ X ][ K ] (64) [ F 4+ F 6 ] = [Fin 2+ Fin 2 ] where [ X ] = and [ Y ] = and [ K ] = and [ D ] = ] = [ D ][ X ][ Y ] [ F ext 2+ F ext 2 + [ D ][ X ][ K ] (65) [ ] 1 (D 4+ ) T [ζ ζ sys 11 (D ]D4+ 4+ ) T [ζ ζ sys 12 ]D6 (D 6 ) T [ζ ζ sys 21 (D ]D4+ 6 ) T [ζ ζ sys 22 [ ] ]D6 (66) (D 4+ ) T ζ 2 11 (D 4+ ) T ζ 2 12 (D 6 ) T ζ 2 21 (D 6 ) T ζ 2 22 (67) ) T [ζ 2 (D4+ 13 ζ sys 13 + u Ḣ4+ 4+ ] (68) (D 6 ) T [ζ 2 23 ζ sys 23 + u Ḣ6 6 ] [ ] D 4+ Z Z D 6 (69) and Fin 2 in the two handle equations of motion for body 2, the Substituting the expression for Fin 2+ two handle equations of motion of the entire assembly of bodies 4,5,6 and 2 an be obtained as where [ A 2+ A 2 [ ζ asm 11 ζ asm 12 ζ asm 21 ζ asm 22 and [ ζ asm 13 ζ asm 23 ] ] ] = = = [ ζ asm 11 ζ asm 12 ζ asm 21 ζ asm 22 ][ ] F ext 2+ F ext 2 + [ ζ asm 13 ζ asm 23 ] (70) [ ζ 2 11 ζ 2 ] 12 ζ 2 21 ζ 2 ( [ U ] [ D ][ X ][ Y ] ) (71) 22 [ ] ζ 2 13 [ D ][ X ][ K ] (72) ζ 2 23 This body asm now represents a single body in the upper loop. The upper loop now is made up of bodies 1, asm and 3. This upper loop now redues to a single loop and an be solved exatly as in the above setion. Having solved for the aelerations at the handles of eah body in the upper loop, the same proedure an be applied for the lower loop to generate the aeleration of eah handle on eah body of the lower loop. 3.3 Singular Configurations Multibody systems with losed kinematial loops an undergo motion suh that the system passes through a singular onfiguration. Singular onfigurations are typially observed when system enters some form of a toggle position. Example of suh ases an be a four bar linkage as shown in figure (5). 13

14 q 3 q 4 A O A L q 3 q 4 B O B P g q 1 q 2 C q 2 Figure(a): Augmentation Method Problem q 1 Figure(b) Redution Method Problem Figure 5: Four Bar Linkages for simulating Singular Configurations When using an augmentation approah for formulating the equations of motion, a ommon problem enountered with the system in a singular onfiguration is that the onstraint Jaobian beomes rank defiient. A rank defiient onstraint Jaobian redues the system of equations to unsolvable, rendering the formulation inapable of handling suh onfigurations. A similar problem is seen with a redution type of approah where the dependeny matrix loses rank when the system enters a singular onfiguration. In either ase, in a true singular onfiguration, the system of equations annot be solved. Even if the system does not enter a true singular onfiguration, but passes near singular states, onstraint violation errors an grow signifiantly due to the ill onditioned onstraint Jaobian or dependeny matrix. This is often enountered as the integration steps aross a singular onfiguration during a simulation and an result in signifiant errors in the simulation. The algorithm presented in this paper is able to simulate systems with singular onfigurations without running into these problem. This is beause neither does the formulation onstrut a onstraint matrix nor does it use dependent and independent oordinates. Thus, sine the dimensionality of the problem never hanges, the algorithm is free from rank defiieny issues with all matries to be inverted remaining positive definite. The ODCA algorithm presented uses a redundant set of generalized oordinates, but does not arry along a ompanion set of algebrai onstraint equations. There is thus no onstraint Jaobian to lose rank, but individual joint onstraints are enfored impliitly through the joint spae map H (??q (8)). This manner in whih this method avoids singularities appears similar in some regards to that with Euler parameters. With Euler parameter, one deals with a redundant four member set of generalized oordinates (parameters) for the global and nonsingular desription of three degree of freedom spatial rotation. The onstraint between these four oordinates being impliitly enfored. If the onstraint were expliitly used to redue out the extra generalized oordinate (parameter), the representation again may beome singular. 4 Numerial Examples In this setion numerial results obtained from implementing sample test ases are presented. These test ases were run with the intent of assessing the basi harateristis of the presented ODCA approah, relative to those obtained using more traditional methods. For this reason simulations were run using: 1) the 14

15 presented ODCA method; 2) onstraint enforement at the aeleration level, via Lagrange multipliers; and 3) Constraint enforement at the veloity level by removing the redundant variables at both the aeleration and veloity levels [34]. Thus, the following sample ase were all run without the benefit of any form of supplemental onstraint stabilization. In this manner the relative merit of these methods with regard to auray and robustness might be more learly seen. All the test ases presented here were implemented in Matlab TM and the temporal integration of the equations of motion were arried out using the ode45 integrator featured in Matlab TM. The absolute and relative toleranes were set to All bodies in the test ases are modelled as having length L = 1, with mass of 1kg and inertia about an axis perpendiular to the page of the paper as 1kgm 2. All kinemati joints in the test ases are revolute. 4.1 Single and Coupled Loops Consider the four bar linkage as shown in figure (3). The following figure (6) shows the results obtained from a simulation of this system when moving under the effet of gravity. Figure (6) shows the variation in the angles with respet to time while figure (7) shows the variation of the onstraint violation error with time. The onstraint violation error refers to the absolute position error in the satisfation of the loop loser equation. As an be seen in the error plot, the error is of the order of i.e. up to mahine auray. A similar test ase is shown for systems with oupled loops. Consider the seven bar linkage as shown in figure(4). The results of a 10 seond simulation of the system released from rest under the effet of gravity are shown in figure (9). The onstraint violation is plotted in figure(8). The onstraint violation in this ase is also of the order of i.e. up to mahine auray. Analysis of this error time history indiates that after the first few seonds of the simulation, the onstraint violation error grows as t 0.7, without any additional onstraint stabilization strategy being used. This represents a onsiderable improvement in performane over onventional onstraint enforement methods. Speifially, when this same system is modelled through the enforement of the loop losure onstraints at the aeleration level alone via Lagrange multipliers, the the error inreases as t 2. Similarly, if the onstraints for this system are enfored a the veloity level, through the elimination of redundant variables, then the onstraint violation error grows linearly with t. In the single loop, and multiple loop onfigurations investigated, the presented ODCA proedure onsistently showed suh redued onstraint violation error growth relative to more traditional methods. 4.2 Singular Configurations Two test ases are simulated to ompare the performane of this algorithm for systems repeatedly passing through singular onfigurations. These test ases are shown in Figure (5). For eah of these systems 20 seond simulations were arried out, with the systems being ated on by gravity, and released from rest from a non equilibrium position. In eah ase, the system equilibrium position, about whih the systems osillate produes a numerial singularity. As suh these test ases offer a hallenge to traditional onstraint enforement methods. When the funtion evaluations our near the singular onfiguration, a signifiant loss of auray is inurred due to the ill onditioning of the system. If the funtion evaluation ours on (or too near) a singular onfiguration, then these traditional methods will fail ompletely. The results are shown in Figure (10). For the system shown in Figure (5-a), the simulation results obtained with the present algorithm are ompared with a referene simulation generated using an augmented Lagrange approah. When using the augmented Lagrange approah, the onstraint Jaobian for this system loses rank when the oordinate q 1 beomes zero i.e. q 1 =0. In this onfiguration there are three possible solutions i.e. a) the system passes though the vertial and remains in a parallelogram onfiguration b) the systems jumps to the anti-parallelogram (rossed) iruit of motion or ) bar A beomes kinematially loked in the vertial diretion as bars B and C swing as a unit as a pendulum about hinge O B. Similarly, for the system shown in Figure(5-b), the simulation using the O-DCA is ompared against a referene simulation generated using a redution approah. In the redution approah q 1 is seleted as the independent aeleration variable with q 2, q 3, q 4 as the dependent aeleration variables. With this system, the dependeny matrix presribing q 2, q 3, q 4 as a funtion of q 1 beomes singular as the system enters the 15

16 toggle position at q 4 =90. In this onfiguration the dependeny equations offer two solutions with a further inrease in q 1 i.e. a) hinge point P will move to the left or b) move to the right. In all ase the presented ODCA method signifiantly out performed the traditional Lagrange multiplier and redution approahes. The onstraint violation error time histories for eah of these are presented in figures 10. The relative robustness and auray of the presented ODCA was partiularly evident when for eah of these mehanisms, the systems was started with an nonzero initial veloity in a truly singular onfiguration. The ODCA was unaffeted and performed the simulation without diffiulty, while the more traditional onstraint enforement methods all failed immediately and ompletely. 4.3 Disussion When simulating dynamis of systems with onstraints, the onstraint are most often imposed at the aelerations, or possibly the veloity level. The onstraint imposition at aeleration or veloity level introdues zero eigenvalues assoiated with eah onstant of temporal integration. This problem is further exaerbated with aumulation of round-off errors from finite preision arithmeti. Thus, imposing the onstraints at the aeleration or veloity level results in an eventual unstable growth in onstraint violation whih an grow exponentially for a given simulation. This problem an be alleviated by introduing some form of onstraint stabilization into the equations. Although introduing a onstraint stabilization tehnique an redue the drift in the onstraint violation signifiantly, these methods do not generally provide full onstraint satisfation, and ome with their own (potentially signifiant) omputational ost. The onus is thus on the underlying formulation to keep the onstraint violation at a minimum. The results obtained from the implementation of the algorithm for sample test ases are indiative of the exellent performane of this algorithm for systems with single or oupled loops. Although the onstraints are imposed at the aeleration level, the onstraint violation error growth ahieved with the ODCA was onsistently superior to that obtained using even veloity level onstraint enforement with more traditional onstraint enforement methods. Even though there is a small growth in the error, the magnitude of the error is far smaller than what may be expeted for a omparable length simulation using aeleration level onstraint imposition with either augmented or redution approah. The error an be further redued with the use of a onstraint stabilization tehnique or by imposing the onstraint at the veloity level. For systems whih pass through singular onfiguration, the performane of the present algorithm is far superior to that of a true augmentation approah as well as a redution type approah with the onstraint imposition at the aeleration level. For the test ases simulated, the aeleration level augmented approah failed to onverge, while the redution approah showed a large error in onstraint violation. The performane of the present algorithm is omparable with the redution approah with a veloity level onstraint imposition. In fat, for the system simulated, the present algorithm gives slightly better results. Simulations run with both the redution and augmentation approahes failed when the system entered a true singular onfiguration. The results shown here for these approahes are for simulations when the system does not enter a true singular onfiguration but passes through it. By omparison, the method presented here ontinued to run orretly even when it entered a truly singular onfiguration (not just near singular). 5 Conlusion In this paper, a new algorithm is presented for alulating the forward dynamis of multibody systems with losed kinematial loops. The algorithm is simple to implement and the omputational expense is O(n) when implemented in serial. Due to the binary-tree struture of the formulation, the algorithm would ahieve a logarithmi omplexity for parallel implementation. The algorithm is exat and non-iterative. The implementation results indiate exellent onstraint satisfation for systems with single as well as oupled loops, even when the onstraint satisfation is enfored at the aeleration level. The performane of the algorithm is better than omparable algorithms for modelling systems whih pass through singular onfigurations. The algorithm an be extended for flexible body systems modelled using a omponent mode synthesis formulation. The implementation of the onstraints at the veloity level using this algorithm as well as performane measures for larger systems are of urrent researh interest for the authors. 16

17 q1 Angles in radians q2 q3 q4 Time in seonds Figure 6: Results for Single Loop 6 Aknowledgement This work was funded by the NSF NIRT Grant Number The authors would like to thank the funding ageny for their support. Referenes [1] W. W. Hooker and G. Margulies. The dynamial attitude equations for an n-body satellite. Journal of the Astronautial Sienes, 7(4): , Winter [2] J. S. Y. Luh, M. W. Walker, and R. P. C. Paul. On-line omputational sheme for mehanial manipulators. Journal of Dynami Systems, Measurements, and Control, 102:69 76, Jun [3] M. W. Walker and D. E. Orin. Effiient dynami omputer simulation of roboti mehanisms. Journal of Dynami Systems, Measurement, and Control, 104: , Sep [4] D. E. Rosenthal and M. A. Sherman. High performane multibody simulations via symboli equation manipulation and Kane s method. Journal of the Astronautial Sienes, 34(3): , Jul. Sep [5] P. E. Neilan. Effiient Computer Simulation of Motions of Multibody Systems. PhD thesis, Stanford University, [6] A. F. Vereshhagin. Computer simulation of the dynamis of ompliated mehanisms of robotmanipulators. Engineering Cybernetis, 12(6):65 70, November Deember

18 [7] W. W. Armstrong. Reursive solution to the equations of motion of an n-link manipulator. In Fifth World Congress on the Theory of Mahines and Mehanisms, volume 2, pages , [8] R. Featherstone. The alulation of roboti dynamis using artiulated body inertias. International Journal of Robotis Researh, 2(1):13 30, Spring [9] H. Brandl, R. Johanni, and M. Otter. A very effiient algorithm for the simulation of robots and similar multibody systems without inversion of the mass matrix. In IFAC/IFIP/IMACS Symposium, Vienna, Austria, [10] D. S. Bae and E. J. Haug. A reursive formation for onstrained mehanial system dynamis: Part I, Open loop systems. Mehanisms, Strutures, and Mahines, 15(3): , [11] D.E. Rosenthal. An order n formulation for roboti systems. The Journal of the Astronautial Sienes, 38(4): , [12] K. S. Anderson. Reursive Derivation of Expliit Equations of Motion for Effiient Dynami/Control Simulation of Large Multibody Systems. PhD thesis, Stanford University, [13] K. Kreutz-Delgado, A. Jain, and G. Rodriguez. Reursive formulation of operational spae ontrol. International Journal of Robotis Researh, 11(4): , August [14] A. Jain. Unified formulation of dynamis for serial rigid multibody systems. Journal of Guidane, Control, and Dynamis, 14(3): , May Jun [15] R. Featherstone. Robot Dynamis Algorithms. Kluwer Aademi Publishing, New York, [16] D. S. Bae and E. J. Haug. A reursive formation for onstrained mehanial system dynamis: Part II, Closed loop systems. Mehanisms, Strutures, and Mahines, 15(4): , [17] R. A. Wehage. Appliation of matrix partitioning and reursive projetion to O(n) solution of onstrained equations of motion. Amerian Soiety of Mehanial Engineers, Design Engineering Division (Publiation) DE, v15-2, pages [18] K. S. Anderson and J. H. Crithley. Improved order-n performane algorithm for the simulation of onstrained multi-rigid-body systems. Multibody Systems Dynamis, 9: , [19] J. H. Crithley and K. S. Anderson. A generalized reursive oordinate redution method for multibody system dynamis. Journal of Multisale Computational Engineering, 1(2 & 3): , [20] P. Lötstedt. On a penalty funtion method for the simulation of mehanial systems subjet to onstraints. Report TRITA-NA-7919, Royal Institute of Tehnology, Stokholm, Sweden, [21] P. Lötstedt. Mehanial systems of rigid bodies subjet to unilateral onstraints. SIAM Journal of Applied Mathematis, 42(2): , Apr [22] J. Baumgarte. Stabilization of onstraints and integrals of motion in dynami systems. Computer Methods in Applied Mehanis and Engineering, 1:1 16, [23] J. W. Baumgarte. A new method for stabilization of holonomi ontraints. Journal of Applied Mehanis, 50: , De [24] K. C. Park and J. C. Chiou. Stabilization of omputational proedures for onstrained dynamial systems. Journal of Guidane, Control and Dynamis, 11(4): , Jul. Aug [25] E. Bayo, J. Garia de Jalon, and M. A. Serna. Modified Lagrangian formulation for the dynami analysis of onstrained mehanial systems. Computer Methods in Applied Mehanis and Engineering, 71(2): , Nov [26] K. S. Anderson. An order-n formulation for motion simulation of general onstrained multi-rigid-body systems. Computers and Strutures, 43(3): ,