Static Analysis of Large-Scale Multibody System Using Joint Coordinates and Spatial Algebra Operator

Initial transient oscillations inhibited in the dynamic simulations responses of multibody systems can lead to inaccurate results, unrealistic load prediction, or simulation failure. These transients could result from incompatible initial conditions, initial constraints violation, and inadequate kinematic assembly. Performing static equilibrium analysis before the dynamic simulation can eliminate these transients and lead to stable simulation. Most exiting multibody formulations determine the static equilibrium position by minimizing the system potential energy. This paper presents a new general purpose approach for solving the static equilibrium in large-scale articulated multibody. The proposed approach introduces an energy drainage mechanism based on Baumgarte constraint stabilization approach to determine the static equilibrium position. The spatial algebra operator is used to express the kinematic and dynamic equations of the closed-loop multibody system. The proposed multibody system formulation utilizes the joint coordinates and modal elastic coordinates as the system generalized coordinates. The recursive nonlinear equations of motion are formulated using the Cartesian coordinates and the joint coordinates to form an augmented set of differential algebraic equations. Then system connectivity matrix is derived from the system topological relations and used to project the Cartesian quantities into the joint subspace leading to minimum set of differential equations.


Introduction
Virtual development procedures became the most economical venue in product design and optimization including earth-moving equipment and automotive systems. These systems are typically large and complex and composed of heterogeneous physical subsystems. Simulation of such systems may require multidomain modeling, for example, rigid and flexible bodies, nonlinear contact and friction force modules, terrain interaction, electrical and hydraulic subsystems, control systems, and nonholonomic constraints. Accurate multibody solver will be required in the heart of the analysis of such systems. As per Giampiero and Manfred [1], the computation of static equilibrium is often the first step in the analysis of multibody system models. The static equilibrium position defines the working point for the linearization of the system equation of motion and provides initial values for the dynamical simulation.
Large earth-moving equipment, agricultural machinery, tracked vehicles, tractors, bulldozers, motor graders, wheeled loaders, and forestry machines are some of the equipment that may work on slopped terrain. Stability assessment of such machinery on the slopes under different loading conditions and different articulation configurations requires accurate modeling of the machine and its interaction with the terrain. The behavior of existing terrain models, in most cases [2][3][4][5], is highly nonlinear and history dependent. Performing static equilibrium is very crucial step for accurate linear analysis and stability assessment of the multibody systems.
Another example would be when the multibody dynamics approach is used to model and simulate the startup and shut down of reciprocating engines and turbines which are supported by journal bearings. Journal bearings are typically nonlinear components and their dynamic characteristics (stiffness, damping, etc.) are history dependent [6,7]. In such systems, the crankshaft and the rotor could be modeled as flexible (deformable bodies). The static equilibrium position of rotor center with respect to the bearing geometric center needs to be accurately determined in order to perform transient simulation or normal mode analysis. When 2 The Scientific World Journal the multibody system contains contacting bodies that may undergo slipping, static equilibrium cannot be obtained from force balance. In this case, the traditional static equilibrium solution will not converge to the true static equilibrium position.
In general, analysis and simulation scenarios of dynamic multibody systems could be summarized as follows: (1) performing transient dynamic simulation of typical machine working cycle or operation; (2) performing eigenvalue analysis to calculate the natural frequencies and normal modes and assess the system stability; (3) determining the static equilibrium of an articulated system that may include rigid and flexible bodies connected with joints.
Three traditional approaches have been used to formulate the multibody system equations of motion: the Cartesian body coordinates (absolute coordinates), the joint coordinates, and velocity transformation method. The Cartesian body coordinates formulations are very popular and reported to be a simpler method to construct the equations of motion leading to a large set of differential algebraic equations [8,9]. The configuration of a rigid body is described by a set of translational and rotational coordinates. Algebraic constraints are introduced to represent kinematic joints connecting bodies and then the Lagrange multiplier technique is used to describe joint reaction forces. The system of differential algebraic equations has to be solved simultaneously. Newton-Raphson iterations or similar techniques could be used to satisfy the applied constraints. Kinematic assembly of the system is necessary for starting a successful simulation.
Featherstone [10,11] used spatial vectors to study the dynamics of articulated bodies. Featherstone and Orin [12] and Featherstone [13] presented an efficient approach for utilizing spatial algebra to model multibody systems and efficiently factor the inertia matrix for rigid body systems. Wehage and Haug [14] used a similar approach to develop set of automated procedures for robust and efficient solution of overconstrained multibody dynamics. Wehage and Belczynski [15] proposed structuring the kinematic and dynamic equations into block matrix structure and developed procedures to enable real-time simulation of multibody system. Rodriguez et al. [16,17] presented a spatial operator based on the spatial algebra for developing multibody dynamic equations of motion. Determining the static equilibrium position using these formulations was not explicitly explained.
Traditional approach of solving for the static equilibrium is based on minimizing the potential energy of the system [1,18]. This approach requires explicit expression for potential energy of all the bodies and the force elements in the system. This could be practically impossible for the abovementioned heterogeneous system simulations. Other approaches [19,20] assume the system will be under static equilibrium when the first and second derivatives of the system generalized coordinates are set to zero. The differential equations of motion of the system then become a set of algebraic equations that could be solved along with the constraint equations. The resulting solution yields a set of initial values for the system states. Linear modes analysis could then be performed by carrying out Taylor expansion while only the linear terms are retained [1,19]. In the abovementioned approaches, false equilibrium position could be reported. Also, violation of the constraints in the velocity level and acceleration level may exist.
Recently, Yang et al. [19] presented a general approach for solving the eigenvalue problem after calculating static equilibrium. The approach is similar to many existing solution approaches and could be summarized by [19] as follows. The system equations of motion could be derived from where x ∈ R is the vector of general coordinates, ∈ R is the vector of Lagrange multipliers, is the Lagrange function associated with (x,ẋ ), f is the vector of nonconservative forces which are function of (x,ẋ , ), Φ is the vector of constraints associated with (x), and ( Φ/ x) is the vector of generalized constraint forces. The general approach to solve for the static equilibrium position starts by writing the objective function as follows: (2) Then, the system equations could be written as The equilibrium state (x 0 , 0 , 0 ) satisfies the following conditions: With a small vibration about the equilibrium position (ẍ ,ẋ , x 0 + x, 0 + , ), then Using Taylor expansion and reserving the linear terms, we get whereM The Scientific World Journal 3 Writing the solution as y = V, where is one of the eigenvalues, V is the corresponding eigenvector and the eigenvalue problem becomes This equation represents a standard eigenvalue problem that could be solved using standard solver. The frequency shift could be used to insure that K is nonsingular.
Avilés et al. [21] presented a procedure for the solution of the nonlinear static equilibrium problem in complex multibody mechanical systems, including rigid and elastic elements. The error function was simple based on the potential function and set of nonlinear constraints. Lagrange multipliers along with various versions of the augmented Lagrange multipliers were used to form the system dynamic equations. A Newton-Raphson second-order method is used for seeking function minima for equilibrium positions. Wang et al. [22] used the bond graph to improve the efficiency for the kintostatic analysis of complex multibody systems. They proposed an effective decoupling method to simplify the resulting complexity of the equation of motion. As per Giampiero and Manfred [1], the linearized equation of motion will lead to nonlinear equations that can be solved using Newton's method.
Similarly, commercial multibody dynamic simulation solvers like SIMPACK use Newton-Raphson based solver for nonlinear equations and find the system equilibrium positions and/or calculate the preload in the springs and bushing elements. This solution approach has been used for trains, automotive, and airplane systems. Another approach to determine the static equilibrium is the quasistatic approach. In multibody systems, this approach may not be very efficient if there is significant difference in the inertia of the bodies in the system or if there are dynamic oscillators in the system. This paper describes a general purpose formulation and implementation for modeling rigid and flexible body in multibody system based on the joint coordinates formulation. A new approach for solving static equilibrium will be presented in this paper. The presented static solver utilizes a recursive transient solver to evaluate the system equations of motion. An energy dissipative damping is introduced to eliminate the system oscillations around the equilibrium position. The system states are dampened using weighted damping factor that is function of the state velocity and the state error. The attributes associated with this method are the ability to handle large heterogeneous systems and ability to linearize the system in terms of arbitrary user-defined coordinates in a straightforward implementation. The spatial algebra operators are used to formulate the kinematic and dynamic equations of motion.
This paper is organized as follows. In the following section, the structure of the equation of motion of the multibody system using joint coordinate formulation is introduced for rigid body system based on the spatial algebra operator. In Section 3, the flexible body kinematic and dynamic equations of motion are presented. In Section 4, the equations of motion of the constrained multibody systems with closed loops will be presented. Section 5 outlines the constraint enforcement technique based on Baumgarte stabilization approach. Section 6 presents the proposed static equilibrium technique based on the energy drainage. Section 7 outlines the recursive algorithm implemented to solve the proposed multibody system equations of motion. Section 8 presents sample results of the proposed static solution approach for multibody systems. Finally, this paper is summarized and some conclusions are drawn in Section 9.

Multibody Formulation and System
Equation of Motion Figure 1 shows an illustration of the typical structure of articulated earth-moving equipment, a wheel loader. The major dynamic components of the machine, as shown in the figure, include the rear frame which contains the engine and drive train, the right and left rear wheels which are connected to the rear frame, the front frame which articulates with respect to the rear frame to steer the machine, the boom which is connected to the front frame and is driven by hydraulic cylinders, the bucket which is connected to the boom and is controlled by other hydraulic cylinders, and the left and right front wheels which are connected to the front frames. This machine model will be used to relate the theoretical multibody equations to physical model for clarification. The proposed formulation in this investigation utilizes a joint coordinates based multibody approach. The kinematic tree or connectivity graph is developed to describe the system topology based on the connectivity between the different bodies in the system. The ground body (the inertial body) is considered the root (base) of the kinematic tree. Any dynamic body in the system is connected or referenced to one parent body through an arc joint that allows one or more DoF. While each body can have only one parent (ancestor body), it could have one or more child bodies (descendant bodies). In the kinematic tree the root body is numbered as 0 while the descendant bodies are numbered consecutively from 1 to . The body and the joint connecting it to its parent are given the same number [10,23]. It should be mentioned that the kinematic tree is not unique. Using the parent-child relation, a parent-child list could be developed and stored to be used later in the recursive calculations [13,24,25]. Any arbitrary body in the system could be modeled as rigid or flexible body. Figure 2 shows the kinematic tree of the wheel loader under investigation. The dynamic bodies are represented by the circles and the joints are represented by solid arrows. The rear frame is considered the base of our kinematic tree and is referenced to the ground with free joint (joint with 6-DoF). The rear wheels and the front frame are considered descendants of the rear frame and connected to their parent by revolute joints. The remaining bodies in the physical model are shown in the kinematic tree. It should be mentioned that, although the bucket and the boom are driven by two hydraulic cylinders each, only one cylinder is shown in the kinematic tree for simplicity, The Scientific World Journal  In the proposed formulation the child body is joined to its parent through two markers; as shown in Figure 3, one marker is attached to the parent side called connector while the other marker is on the child side called the output marker. The output marker is the reference frame of the child body. The body kinematic quantities and dynamic matrices are expressed with respect to the body reference frame. The position and orientation of the connectors are measured relative to the parent reference marker through the spatial transformation matrix, as shown in Figure 3. The joint degrees of freedom, representing the joint variables, are defined as the allowed relative motion between the two connecting markers. Relative motion between the markers could be translation or rotation along one of the connector marker axes. Massless markers could be inserted between the two bodies to represent joints with more than one DoF. The joint variables displacement, velocities, and accelerations are used as the body states. The Cartesian displacements, velocities, acceleration vectors, and the joint reaction forces are considered as augmented algebraic variables.
The position vector of body is defined in the global coordinate system by position vector of the origin of the body reference marker 0 0 , , as shown in Figure 3, while the orientation of the body could be described by the 3 × 3 rotation matrix 0 which is a function of set of spatial rotational angles. The spatial velocity vector of body , k 0 0 , = [ 0 0̇0 0 , ] , could be obtained by differentiating the position vector and the body orientation parameters. As the recursive formulation is based on relative motion, the relative spatial velocity vector of the reference frame of a child body with respect to its parent rigid body observed at the origin of the local coordinate system of body can be written as The Scientific World Journal where is the time derivative of the orientation parameters of body with respect to body anḋ, is the derivative of the position vector of body with respect to body . The relative velocity across the joint between body and body can be written as follows: where h is a 6 by joint influence coefficient matrix or partial velocity matrix corresponding to particular columns of the identity matrix depending on which primitive degrees of freedom are represented by q , as shown in Figure 3, and q andq are the vector of joint variables and its time derivatives.
The spatial velocity vector can be transformed from the CS at into the CS at using spatial transformation matrix as follows [9,10,24]: where is the spatial transformation matrix and and indicate the coordinate systems. The general transformation matrix is the result of spatial rotation followed by a spatial translation as follows: where is the 6 × 6 spatial translation matrix, is the 3 × 1 translational displacement vector of CS relative to CS defined in the CS ,̃is 3 × 3 skew symmetric matrix representing the cross-product operation of , is the 6 × 6 spatial rotation matrix, and is the 3 × 3 transformation matrix relating the coordinate frames and .
The spatial velocity vector of body defined in the global coordinate system could be computed as follows: The spatial velocity vector of a descendant body could be calculated recursively using its parent spatial velocity as follows: Assuming the system shown in Figure 3 is modeled using rigid bodies, the velocity of body could be written in a similar way as follows: Rearranging the terms in the velocity equations, the recursive form of the system velocity could be expressed as follows [13]: The velocity recursive equations in (16) could be written in a compact form as follows: where ℓℓ represents the assembled system topology or connectivity matrix which could be constructed from the connectivity graph, the superscript ℓ refers to local matrices and the superscript refers to the system assembly matrix, and H ℓ is the assembly influence coefficient matrices grouped in blocks corresponding to the joints' DoF.

6
The Scientific World Journal It should be mentioned that ℓℓ is a lower triangular matrix and thus has simple inverse that maintains the topological structure of the original matrix and becomes an upper triangular matrix as follows [26]: Similarly the spatial acceleration vector of body could be obtained by differentiating the velocity vector defined in (14) as follows: Assuming that the quadratic velocity term could be expressed as 0 0 ,0 =k 0 0 ,0 h 0 ,0q =k 0 0 ,0 k 0 0 ,0 , the global acceleration vector could written as follows: The recursive form to calculate the system local accelerations could be written as follows: Equations (17) and (21) could be used to recursively calculate the velocity and acceleration of the system bodies marching outward from the root to the descendant bodies. Similarly, the system topological matrix could be used to project the joint forces in the joint subspace as follows: The kinematic equations derived earlier could be used to express the rigid body equations of motion from the momentum equations. The spatial momentum vector of the rigid body can be written as follows: where P , is the spatial momentum of body , , is the spatial mass matrix defined at the body center of mass marker , and k 0 , is the global velocity of the marker located at the body center of mass. The centroidal spatial mass matrix is defined as follows: where is the mass body , is 3 × 3 identity matrix, and , is the 3 × 3 matrix representing the body moment of inertia tensor defined at a marker located at the body center of mass, . This rigid body momentum can be transformed into the global coordinate system and differentiated with respect to time to obtain the body equation of motion as follows: where the symbol 0 refers to global coordinate system, ] is the spatial force vector including the gravitational forces, 0 is the sum of all external forces acting on the body , and 0 ,0 contains the sum of all torques and the moments of all forces about the origin of frame 0.
The equation of motion of any two connected bodies and could be driven from the free body diagram as follows. For body , the equation of motion in its local coordinate system could be written as where F , is the vector of reaction forces of the joint connecting body to its parent. While for body , the equation of motion in its local coordinate system could be written as The equations could be rearranged in matrix form as follows: Similar to the velocity and acceleration equations (14), (15), (16), and (17), it could be shown that the connectivity matrix could be used to simplify the expression of the system equations of motion. The system equation of motion could be written for the whole system assembly in a more compact form as follows: The system of equations in (29) contains the unknown Cartesian acceleration of the bodies as well as the joint reaction forces. In order to be able to solve the equation of motion, the kinematic relations from (21), the constraint equations from (22), and the system dynamic equation (29) are rearranged in matrix form as follows: where ℓℓ is a block diagonal inertia matrix expressed in the Cartesian form, ℓℓ is the body connectivity matrix, ℓ is a block diagonal joint influence coefficient matrix, ℓ is the body Cartesian accelerations, F ℓ is a vector of the Lagrange multipliers,q ℓ is the second time derivative of joint variables, G ℓ is a vector of the externally applied loads, ℓℓ is the quadratic constraint derivatives, and ℓ is the generalized The Scientific World Journal 7 forces. Since the inertia matrix ℓℓ is invertible, (30) could be solved to get the unknown joint accelerationsq which can be used to calculate the joint Cartesian accelerations, a ℓ , and the joint reaction forces F ℓ as follows: Equation (31) could be written in a compact form as follows: where M is the generalized mass matrix projected in the joint subspace and can be calculated as follows: and Q is the generalized force vector projected in the joint subspace and can be calculated as follows: This equation of motion (32) represents minimum set of differential equations that are function of the system generalized coordinates. However, this form is limited to open-loop unconstrained mechanisms and therefore is suitable only for robotic type mechanisms.

Equation of Motion of Rigid and Flexible Body System
The flexible body dynamics can be modeled in the multibody system as a reduced form of the finite element model. The finite element can be reduced using nodal approach or modal approach. Modal formulation uses a set of kinematically admissible modes to represent the deformation of the flexible body [27,28]. Component mode synthesis approach is widely used in the multibody dynamics codes to accurately simulate the flexible body dynamics in the multibody system. The main advantage of the modal formulation is that fewer elastic modes can be used to accurately capture the flexible body dynamics at reasonable computational cost [29,30]. Craig-Bampton approach was introduced to account for the effect of boundary conditions and the attachment joints of the flexible body [31,32]. The structure of the reduced flexible body equation-of-motion can be written as follows [23]: where refers to the flexible body's local reference frame, refers to flexible or elastic coordinates, represents the 6 × 6 inertia matrix associated with the reference motion, , are the inertia coupling terms between the reference motion and the elastic coordinates, is the inertia matrix associated with the elastic coordinates, is the reference frame acceleration in the Cartesian space, represents the elastic displacements measured relative to that frame, is the vector of external forces including centrifugal and Coriolis forces [1,23,27], and is the vector of elastic forces projected into the modal space. It should be mentioned that the matrices , , and depend on the elastic coordinates and should be updated every time step. To improve the computation efficiency of the flexible body simulation, a set of inertia shape invariants (ISI) are identified and calculated before the simulation starts. The ISI are used during the dynamic simulation to update the inertia terms. The ISI depend on the kinematic expression of the system equations.
A general form of the equation of motion of multibody system with flexible bodies could be written in a compact form as follows [23]: where the block diagonal matrix M is composed of six by six inertia matrices that represent the Cartesian inertia matrices associated with the reference frame for the rigid and flexible bodies in the system, M is the inertia coupling terms between the modal elastic coordinates and Cartesian reference accelerations, M is a block diagonal matrix containing the flexible body inertias, T is a lower triangular topology matrix, H is a block matrix of the joint influence coefficient matrices, a is the vector of Cartesian accelerations, F is the vector of joint reaction forces,q is the vector of joint accelerations with the appended elastic coordinates, G is the vector of external forces, is a column matrix of 6 × 1 spatial quadratic acceleration vectors, and Q is the vector of joint and elastic forces.
The system of equations given by (36) can be easily solved for the joint accelerations and the modal coordinates accelerations. Then the Cartesian accelerations and the reaction forces could be easily computed. Since the inverse of system topology matrix T is just its transpose, manipulating the terms in (36), we can get the joint and modal accelerations as follows:

(M + H (T M T) H + H T M + M TH)q
This equation could be written in a compact form as follows: The Cartesian accelerations could be calculated as follows: and the Cartesian joint reaction forces could be obtained from The Scientific World Journal The sequence of evaluating the terms in (37) to (40) could be optimized in order to minimize the computational efforts and to avoid repeated calculations. Both (32) and (38) represent the minimum set of differential equations that can completely describe the system dynamics.

Equation of Motion of Constrained System
The abovementioned development could be efficiently used to model open-loop systems like robots and human body systems. In the open-loop systems, each body is connected with one joint only. This open-loop systems approach will not be adequate to model the wheel loader system shown in Figure 1. In the kinematic tree, shown in Figure 2, the solid arrows represent a joint where the dotted lines represent a joint that will form a closed kinematic loop. According to this description, the kinematic tree of the wheel loader contains two closed loops formed of the following list of bodies: Γ 1 = {2, 8, 11, 9} and Γ 2 = {2, 9, 12, 10, 7}. Each of those two loops could be represented by a set of constraint equations that are function of the joint variable of all the bodies forming the loop as follows: Φ 1 (q 2 , q 8 , q 11 , q 9 ) = 0 and Φ 2 (q 2 , q 9 , q 12 , q 10 , q 7 ) = 0. The first and second derivatives of the constraint equations could be easily evaluated from the kinematic equations. The multibody system's equations of motion with appended constraints could be formed as follows: where M is the generalized mass matrix from (32) and (38), J is the Jacobian matrix, and the other column matrices are similar to those described earlier.
The coefficient matrix in the left hand side of (41) is often singular, and general-purpose sparse matrix solver algorithms are routinely used to analyze, factor, and solve the equations. An alternative approach based on generalized coordinate partitioning [14] may be employed at this stage. The factored forms of (33) and (34) used to obtain (32) play a significant role in the solver efficiency. The generalized coordinate partitioning method [14] first analyzes the model topology and decomposes it into a number of kinematically uncoupled superelements, each with a small Jacobian matrix [2]. The uncoupled acceleration constraint equations could be arranged as follows: ] . (42) The Jacobian matrices in (42) are factored into LU form using complete pivoting to partition the variables into dependent and independent sets. The results are then combined into a revised set of constraint equations [33] =̈+ , .

(44)
The coefficient matrix in the upper-left part of (44) is structurally nonsingular, so it can be reduced tö

Constraint Enforcement
The equations of motion in (41) and (44) are index-2 differential algebraic equations (DAE) [1]. This set of DAE are traditionally linearized and solved by implicit numerical integrators [34][35][36]. The generalized coordinate partitioning approach [14] is used to avoid the complexity and cost of implicit solvers, but this approach does not guarantee continued satisfaction of the system constraints. Baumgarte [37] introduced a constraint stabilization method based on the control theory. In this approach, the original acceleration level constraints are replaced witḧ where and are often chosen to appropriately achieve critical damping. The Baumgarte stabilization is implemented in the following form: where is a user-specified gain (natural frequency). Unfortunately, the value of the parameter is problem dependent, and no general procedure exists for its determination. If Φ = Φ =Φ = 0 represents the constraint equations associated with (41), then substituting (48) into (41) leads to A similar modification would be made to the right hand side of (44). The value set for is critical and affects the integrator performance. If is too large, constraint violations will be very small, but the equation of motion will tend to be stiff and the integrator will be slow. If is too small, the stabilization algorithm will not efficiently reduce the constraint violations. From experience, the maximum value of best is probably near the highest system frequencies in the system. To this end, the developed set of dynamic equations of motion is sufficient to perform the dynamic simulation of open-and closed-loop systems. It is also sufficient for modeling rigid and flexible bodies in the multibody system. The Scientific World Journal 9

Static Equilibrium Analysis
The mechanical system can be considered at static equilibrium if the kinetic energy is zero or constant target value and the potential energy is minimum. Many methods have been proposed to solve static equilibrium in nonlinear multibody system models. Most of those proposed techniques require minimizing potential functions and/or linearizing the equations of motion [36] as mentioned earlier. Writing potential functions for arbitrary nonlinear force modules or linearizing them is a daunting task. The problem becomes more challenging when models contain friction and other history-based force models because unique iterative static solutions may not exist. The proposed approach uses the dynamic model itself to solve the static equilibrium position. This approach may have some drawbacks because convergence can be slow in lightly damped systems. In order to improve the solver efficiency, an energy-drainage mechanism is used.
In the energy-drainage approach, a technique similar to the Baumgarte equation was implemented into the solver. After solving (45) to obtain the accelerationsq , the solver imposes the additional requirement on the second derivatives where is a natural damping frequency and and −1 represent the system states at times and −1 . Choosing is tricky as it was in the constraint stabilization. If is too small, the damping will be weak and the model will oscillate too long. And if is too large, damping will be strong and the model will drift slowly toward equilibrium. With this damping method, equilibrium is approached as ‖ − −1 ‖ gets acceptably small, not necessarily when the kinetic energy gets small.
Another feature of energy-drainage damping is its ability to inhibit the effects of bad initial conditions that often cause models to fly apart and the numerical integrator to fail. Extremely high forces from the offending contact models can cause unrealistic accelerations, which are then integrated to give unrealistic velocities and displacements. When energy-drainage damping is active, the resulting large system velocities are immediately fed back through the above damping equation, and this generates large opposing acceleration terms, canceling those from the model. With energy-drainage damping, the contacting bodies will tend to drift slowly apart until the forces and accelerations become reasonable.

Recursive Algorithm
At the beginning of the dynamic simulation, the structure of the equation of motion is determined based on a preliminary analysis of the system topology. The dependent and independent variable sets are determined using the generalized coordinate portioning approach. The solver integrates both dependent and independent variable sets and the kinematic constraints are enforced using Baumgarte stabilization, as explained in the previous section. The input states to the integrator represent the first and second time derivative of the joint variables and the output states are the joint displacements and the joint velocities. The recursive algorithm of the multibody dynamic simulation can be summarized as follows.
(1) Using the joint variables which returned from the integrator, the solver calculates the Cartesian displacements, velocities, and accelerations. Forward evaluation scheme is utilized (starting from the root body to the descendant/branch bodies).
(2) Update and factor the Jacobian matrix.
(3) Apply generalized coordinate portioning technique to determine the quality of the independent and dependent set of variables.
(4) Calculate the constraint violations and apply the Baumgarte stabilization.
(5) Calculate the internal and external forces and apply them to the different bodies (interaction forces between bodies, driver forces, soil/terrain forces, etc.).
(6) Transform the inertia matrices into global coordinate system.
(7) Calculate the inertia forces and centrifugal and Coriolis forces in Cartesian space.
(8) Propagate the external and inertia forces from the descendant bodies into their parents.
(9) Project the Cartesian forces into the body joint space.
(10) Factor the mass matrix based on the current independent and dependent variables selection.
(11) Calculate second derivatives of the joint variables.
(13) Send the states to the integrator.
The solver utilizes a predictor-corrector integrator with variable-order interpolation polynomials and variable time step. This explicit integrator insures the stability of the solution and the ability to capture the high-speed impacts between the different machine parts.

Examples
This section presents some examples of the simulation results multibody system ranging from simple pendulum to a full wheeled machine. The first example represents a uniform beam connected to the ground with a revolute joint to form a single pendulum, as shown in Figure 4. The link has mass of 1.7595 Kg. The beam center of mass is located in the middle. The pendulum was originally horizontal and is allowed to fall under the effect of gravity. The static equilibrium position of this pendulum is known to be vertical. The proposed multibody formulation and solver were used to model the pendulum and compute the static equilibrium position. The transient simulation is run for a few iterations to allow the system to pick up some kinetic energy. Once the pendulum starts to move the energy draining mechanism is applied. A stopping criterion is set to be a target value of the kinetic energy. When the pendulum reaches this target value, it would have reached the static equilibrium. Figure 5 shows the value of the pendulum angle as it falls under the effect of gravity till the system reaches equilibrium position. The kinetic energy is drained out from the system as shown in Figure 6.
The change in the pendulum potential energy from the reference is shown in Figure 7. As the pendulum starts moving, the potential energy of the pendulum decreases It should be mentioned that time in the plots is only a representation of the number of iteration to achieve solution convergence. Also, it should be mentioned that this example represents an extreme case where the pendulum initial position was extremely far from the equilibrium position. This example shows that the solver is stable and can achieve realistic solutions even in extreme cases.
The second example represents a system that might have minor assembly errors or constraint violations. The system represents a chain of 50 links connected with revolute joints. The chain links had same properties of the link in first example. In this example, the first link is attached to the ground and it has initial angle of 5 ∘ from the vertical position. Every following link has an initial deviation of 1 ∘ relative to its parent. The static equilibrium analysis was performed on the system. The kinetic energy of the system is shown in Figure 8. The potential energy of the system is shown in Figure 9 while the angles of all the links are shown in Figure 10. To demonstrate the completeness of the proposed approach a full machine with closed kinematic loops will be presented in the following example. Forestry equipment is typically required to work on different terrains ranging from swampy, compactable, and hard soil. It may be required to work on slopes dragging wood logs and tree. During operation and articulations, the location of the center of mass of such machine may change and causes the machine to slip, flip over, or fall. Design of such machines requires accurate prediction of the machine stability under the different loading conditions and at different articulation angles. Also, power requirements analysis of such machine requires the simulation to start from a stable static equilibrium position. A grapple skidder machine, shown in Figure 11, will be used as a demonstration example.
The kinematic tree of the machine could be developed, as shown in Figure 12, in order to establish the parent child list. It could be seen that the kinematic tree of grapple skidder is very similar to that of the wheel loader shown in Figure 2. The solver algorithm analyzes the kinematic tree to identify the closed loops of the mechanism and define the constraint equations associated with the closedloop joints. The constraint equations of the two kinematic closed loops could be written as Φ 1 (q 2 , q 8 , q 11 , q 9 ) = 0 and Φ 2 (q 2 , q 9 , q 12 , q 10 , q 7 ) = 0.
The next step for the solver is to analyze the closed loops and merge any two loops that share any joint variable into one larger superelement. This operation could be iterative and may lead to very large superelements. In our example, since the two closed loops are sharing the variables of body 9, they have to be merged into a large superelement that contains all the bodies of the two loops. The Jacobian matrix of this superelement could be calculated as follows: ] . (51) The first raw block of the Jacobian matrix represents the partial derivatives of the first kinematic loop constraint equations with respect to the joint variables of the bodies in the first loop while the second row block represents the partial derivatives of the second loop constraint equations. The system topology matrix will be generated by the solver and should be as follows:    The system assembly influence coefficient matrix could be written as follows:  ] .

(53)
When the solver starts, the Cartesian velocities and accelerations are calculated using forward substitution from the root body to the descendant bodies. The Jacobian matrices are updated based on the new values of the spatial displacements and velocities. The external forces acting on the system, the contact forces between the different bodies, and the interaction forces between the system and terrain are calculated and applied to each body's force vector. The system topology matrix is used to project the Cartesian quantities into the joint subspace and define the minimum set of the system variables as shown in (32) to (34). The system Jacobian matrix is then appended to the inertia matrix to form the augmented constrained multibody equation of motion. The solver then utilizes the generalized coordinate portioning approach to identify the dependent and independent variables by factoring the Jacobian matrix into LU form. The independent variable is identified, as shown in (45). The constraint violation penalty is applied to the system equation of motion using Baumgarte equation, as shown in (48). The entries to the mass matrix are permuted based on the order in the LU factorization order.
To perform the static analysis, at this point, the system accelerations are modified using the energy drainage function as shown in (50), and both the dependent and independent variables are sent to the integrator. The integrated states are used to calculate the Cartesian position, velocities, and accelerations of all bodies. The Cartesian velocities are used to calculate the kinetic energy of the system and the global position of the bodies is used to calculate the potential energies. If the value of the kinetic energy of the system reached the target, the solver stops and reports the current state as the body equilibrium position. Figure 13 shows a successful simulation to achieve the static equilibrium of wheeled machine. The machine is dropping from a small height over a washboard surface. As the machine drops, the rear tires fall on the inclined plane of the surface causing the machine to roll down and backward. As the machine contacts the opposite surface of the washboard ditch, the machine comes to static equilibrium.

Conclusions
This paper presented an approach for evaluating the static equilibrium position of multibody systems. The multibody system can include rigid and flexible bodies. The proposed approach is implemented in a joint-based multibody dynamics formulation that uses the spatial algebra to derive the equation of motion of the multibody system. The kinematic equation of the closed-loop systems was optimized to employ Baumgarte constraint stabilization approach to eliminate the constraint violation while at the same time avoiding using iterative constraint enforcement schemes. In order to determine the system static equilibrium position, an energy drainage mechanism was introduced to modify the system states before integrating it. The static equilibrium is achieved by running a transient simulation of the dynamics system while the integrated states were dampened with the energy drainage operator.