research article static analysis of large-scale multibody...

Research ArticleStatic Analysis of Large-Scale Multibody System Using JointCoordinates and Spatial Algebra Operator

Mohamed A. Omar

Mechanical Engineering Department, Taibah University, Almadinah Almonawwarah 42353, Saudi Arabia

Correspondence should be addressed to Mohamed A. Omar; [email protected]

Received 15 March 2014; Accepted 28 March 2014; Published 19 June 2014

Academic Editor: Belal F. Yousif

Copyright © 2014 Mohamed A. Omar.This is an open access article distributed under the Creative Commons Attribution License,which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

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, initialconstraints violation, and inadequate kinematic assembly. Performing static equilibrium analysis before the dynamic simulationcan eliminate these transients and lead to stable simulation. Most exiting multibody formulations determine the static equilibriumposition by minimizing the system potential energy. This paper presents a new general purpose approach for solving the staticequilibrium in large-scale articulated multibody. The proposed approach introduces an energy drainage mechanism based onBaumgarte constraint stabilization approach to determine the static equilibrium position. The spatial algebra operator is used toexpress the kinematic and dynamic equations of the closed-loop multibody system. The proposed multibody system formulationutilizes the joint coordinates andmodal elastic coordinates as the system generalized coordinates.The recursive nonlinear equationsofmotion are formulated using theCartesian coordinates and the joint coordinates to forman augmented set of differential algebraicequations. Then system connectivity matrix is derived from the system topological relations and used to project the Cartesianquantities into the joint subspace leading to minimum set of differential equations.

1. Introduction

Virtual development procedures became the most econom-ical venue in product design and optimization includingearth-moving equipment and automotive systems. Thesesystems are typically large and complex and composedof heterogeneous physical subsystems. Simulation of suchsystems may require multidomain modeling, for example,rigid and flexible bodies, nonlinear contact and frictionforce modules, terrain interaction, electrical and hydraulicsubsystems, control systems, and nonholonomic constraints.Accurate multibody solver will be required in the heartof the analysis of such systems. As per Giampiero andManfred [1], the computation of static equilibrium is oftenthe first step in the analysis of multibody system models. Thestatic equilibrium position defines the working point for thelinearization of the system equation of motion and providesinitial 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 equipmentthatmaywork on slopped terrain. Stability assessment of suchmachinery on the slopes under different loading conditionsand different articulation configurations requires accuratemodeling of the machine and its interaction with the terrain.The behavior of existing terrain models, in most cases [2–5],is highly nonlinear and history dependent. Performing staticequilibrium is very crucial step for accurate linear analysisand stability assessment of the multibody systems.

Another example would be when the multibody dynam-ics approach is used to model and simulate the startup andshut down of reciprocating engines and turbines which aresupported by journal bearings. Journal bearings are typicallynonlinear components and their dynamic characteristics(stiffness, damping, etc.) are history dependent [6, 7]. Insuch systems, the crankshaft and the rotor could be mod-eled as flexible (deformable bodies). The static equilibriumposition of rotor center with respect to the bearing geometriccenter needs to be accurately determined in order to per-form transient simulation or normal mode analysis. When

Hindawi Publishing Corporatione Scientific World JournalVolume 2014, Article ID 409402, 14 pageshttp://dx.doi.org/10.1155/2014/409402

2 The Scientific World Journal

the multibody system contains contacting bodies that mayundergo slipping, static equilibrium cannot be obtained fromforce balance. In this case, the traditional static equilibriumsolution will not converge to the true static equilibriumposition.

In general, analysis and simulation scenarios of dynamicmultibody systems could be summarized as follows:

(1) performing transient dynamic simulation of typicalmachine working cycle or operation;

(2) performing eigenvalue analysis to calculate the nat-ural frequencies and normal modes and assess thesystem stability;

(3) determining the static equilibrium of an articulatedsystem that may include rigid and flexible bodiesconnected with joints.

Three traditional approaches have been used to formulatethe multibody system equations of motion: the Cartesianbody coordinates (absolute coordinates), the joint coordi-nates, and velocity transformation method. The Cartesianbody coordinates formulations are very popular and reportedto be a simpler method to construct the equations of motionleading to a large set of differential algebraic equations [8, 9].The configuration of a rigid body is described by a set of trans-lational and rotational coordinates. Algebraic constraintsare introduced to represent kinematic joints connectingbodies and then the Lagrange multiplier technique is usedto describe joint reaction forces. The system of differentialalgebraic equations has to be solved simultaneously. Newton-Raphson iterations or similar techniques could be used tosatisfy the applied constraints. Kinematic assembly of thesystem is necessary for starting a successful simulation.

Featherstone [10, 11] used spatial vectors to study thedynamics of articulated bodies. Featherstone and Orin [12]and Featherstone [13] presented an efficient approach forutilizing spatial algebra to model multibody systems andefficiently factor the inertia matrix for rigid body systems.Wehage and Haug [14] used a similar approach to developset of automated procedures for robust and efficient solutionof overconstrained multibody dynamics. Wehage and Bel-czynski [15] proposed structuring the kinematic and dynamicequations into block matrix structure and developed proce-dures to enable real-time simulation of multibody system.Rodriguez et al. [16, 17] presented a spatial operator based onthe spatial algebra for developing multibody dynamic equa-tions of motion. Determining the static equilibrium positionusing these formulations was not explicitly explained.

Traditional approach of solving for the static equilibriumis based on minimizing the potential energy of the system[1, 18].This approach requires explicit expression for potentialenergy of all the bodies and the force elements in the system.This could be practically impossible for the abovementionedheterogeneous system simulations. Other approaches [19, 20]assume the system will be under static equilibrium whenthe first and second derivatives of the system generalizedcoordinates are set to zero. The differential equations ofmotion of the system then become a set of algebraic equationsthat could be solved along with the constraint equations. The

resulting solution yields a set of initial values for the systemstates. Linear modes analysis could then be performed bycarrying out Taylor expansion while only the linear termsare retained [1, 19]. In the abovementioned approaches, falseequilibrium position could be reported. Also, violation of theconstraints in the velocity level and acceleration level mayexist.

Recently, Yang et al. [19] presented a general approach forsolving the eigenvalue problem after calculating static equi-librium. The approach is similar to many existing solutionapproaches and could be summarized by [19] as follows. Thesystem equations of motion could be derived from

𝑑

𝑑𝑡(𝜕𝐿

𝜕x) + (

𝜕𝐿

𝜕x) = (

𝜕Φ

𝜕x)

𝑇

𝜆 + f ,

Φ (x) = 0,(1)

where x ∈ R𝑛 is the vector of general coordinates, 𝜆 ∈ R𝑚 isthe vector of Lagrange multipliers, 𝐿 is the Lagrange functionassociated with (x, x), f is the vector of nonconservativeforces which are function of (x, x, 𝑡), Φ is the vector ofconstraints associated with (x), and (𝜕Φ/𝜕x)𝑇𝜆 is the vectorof generalized constraint forces.

The general approach to solve for the static equilibriumposition starts by writing the objective function as follows:

F (x, x, x, 𝑡) = (− 𝑑𝑑𝑡(𝜕𝐿

𝜕x) +

𝜕𝐿

𝜕x+ f) (x, x, x, 𝑡) ,

G (x) = (𝜕Φ (x)𝜕x

)

𝑇

.

(2)

Then, the system equations could be written as

F (x, x, x, 𝑡) + G (x)𝜆 = 0,

Φ (x) = 0.(3)

The equilibrium state (x0,𝜆0, 𝑡0) satisfies the following condi-

tions:

F (0, 0, x0, 𝑡0) + G (x

0)𝜆0= 0,

Φ (x0) = 0.

(4)

With a small vibration about the equilibrium position(𝛿x, 𝛿x, x

0+ 𝛿x,𝜆

0+ 𝛿𝜆, 𝑡), then

F (𝛿x, 𝛿x, x0+ 𝛿x, 𝑡) + G (x

0+ 𝛿x) (𝜆

0+ 𝛿𝜆) = 0,

Φ (x0+ 𝛿x) = 0.

(5)

Using Taylor expansion and reserving the linear terms, we get

M (x0) 𝛿y + C (x

0) 𝛿y + K (x

0,𝜆0) 𝛿y = 0, (6)

where M(x0) = [

M(x0) 00 0

], C(x0) = [

C(x0) 00 0

], K(x0,𝜆0) =

[K(x0 ,𝜆0) G(x0)G𝑇(x0) 0

], M(x0) = (𝜕F/𝜕x)(x

0), C(x

0) = (𝜕F/𝜕x)(x

0),

K(x0,𝜆0) = (𝜕(F + G𝜆)/𝜕x)(x

0,𝜆0), and 𝛿y = [𝛿x 𝛿𝜆]

𝑇.

The Scientific World Journal 3

Writing the solution as 𝛿y = 𝑒𝑟𝑡V, where 𝑟 is one of

the eigenvalues, V is the corresponding eigenvector and theeigenvalue problem becomes

(𝑟2M + 𝑟C + K)V = 0. (7)

This equation represents a standard eigenvalue problemthat could be solved using standard solver.The frequency shiftcould be used to insure that K is nonsingular.

Aviles et al. [21] presented a procedure for the solutionof the nonlinear static equilibrium problem in complexmultibody mechanical systems, including rigid and elasticelements. The error function was simple based on thepotential function and set of nonlinear constraints. Lagrangemultipliers along with various versions of the augmentedLagrange multipliers were used to form the system dynamicequations. A Newton-Raphson second-order method is usedfor seeking functionminima for equilibrium positions.Wanget al. [22] used the bond graph to improve the efficiencyfor the kintostatic analysis of complex multibody systems.They proposed an effective decoupling method to simplifythe resulting complexity of the equation of motion. Asper Giampiero and Manfred [1], the linearized equation ofmotion will lead to nonlinear equations that can be solvedusing Newton’s method.

Similarly, commercial multibody dynamic simulationsolvers like SIMPACK use Newton-Raphson based solver fornonlinear equations and find the system equilibrium posi-tions and/or calculate the preload in the springs and bushingelements. This solution approach has been used for trains,automotive, and airplane systems. Another approach todetermine the static equilibrium is the quasistatic approach.Inmultibody systems, this approachmay not be very efficientif there is significant difference in the inertia of the bodies inthe system or if there are dynamic oscillators in the system.

This paper describes a general purpose formulation andimplementation for modeling rigid and flexible body inmultibody system based on the joint coordinates formula-tion. A new approach for solving static equilibrium will bepresented in this paper. The presented static solver utilizesa recursive transient solver to evaluate the system equationsof motion. An energy dissipative damping is introducedto eliminate the system oscillations around the equilibriumposition. The system states are dampened using weighteddamping factor that is function of the state velocity and thestate error. The attributes associated with this method arethe ability to handle large heterogeneous systems and abilityto linearize the system in terms of arbitrary user-definedcoordinates in a straightforward implementation.The spatialalgebra operators are used to formulate the kinematic anddynamic equations of motion.

This paper is organized as follows. In the following sec-tion, the structure of the equation of motion of themultibodysystem using joint coordinate formulation is introduced forrigid body system based on the spatial algebra operator. InSection 3, the flexible body kinematic and dynamic equationsof motion are presented. In Section 4, the equations ofmotion of the constrained multibody systems with closedloops will be presented. Section 5 outlines the constraint

enforcement technique based on Baumgarte stabilizationapproach. Section 6 presents the proposed static equilibriumtechnique based on the energy drainage. Section 7 outlinesthe recursive algorithm implemented to solve the proposedmultibody system equations of motion. Section 8 presentssample results of the proposed static solution approach formultibody systems. Finally, this paper is summarized andsome conclusions are drawn in Section 9.

2. Multibody Formulation and SystemEquation of Motion

Figure 1 shows an illustration of the typical structure ofarticulated earth-moving equipment, a wheel loader. Themajor dynamic components of the machine, as shown inthe figure, include the rear frame which contains the engineand drive train, the right and left rear wheels which areconnected to the rear frame, the front framewhich articulateswith respect to the rear frame to steer the machine, theboom which is connected to the front frame and is drivenby hydraulic cylinders, the bucket which is connected tothe boom and is controlled by other hydraulic cylinders,and the left and right front wheels which are connected tothe front frames. This machine model will be used to relatethe theoretical multibody equations to physical model forclarification.

The proposed formulation in this investigation utilizes ajoint coordinates based multibody approach. The kinematictree or connectivity graph is developed to describe the systemtopology based on the connectivity between the differentbodies in the system. The ground body (the inertial body) isconsidered the root (base) of the kinematic tree. Any dynamicbody in the system is connected or referenced to one parentbody through an arc joint that allows one ormoreDoF.Whileeach body can have only one parent (ancestor body), it couldhave one or more child bodies (descendant bodies). In thekinematic tree the root body is numbered as 0 while thedescendant bodies are numbered consecutively from 1 to 𝑛

𝑏.

The body and the joint connecting it to its parent are giventhe same number [10, 23]. It should be mentioned that thekinematic tree is not unique. Using the parent-child relation,a parent-child list could be developed and stored to be usedlater in the recursive calculations [13, 24, 25]. Any arbitrarybody in the system could be modeled as rigid or flexiblebody. Figure 2 shows the kinematic tree of the wheel loaderunder investigation. The dynamic bodies are represented bythe circles and the joints are represented by solid arrows.The rear frame is considered the base of our kinematic treeand is referenced to the ground with free joint (joint with6-DoF). The rear wheels and the front frame are considereddescendants of the rear frame and connected to their parentby revolute joints.The remaining bodies in the physicalmodelare shown in the kinematic tree. It should be mentionedthat, although the bucket and the boom are driven by twohydraulic cylinders each, only one cylinder is shown in thekinematic tree for simplicity,

parent = list (𝑘) = {0, 1, 1, 1, 2, 2, 2, 2, 2, 7, 8, 9} . (8)


Rear frame

Front frame

BoomBucket

Front wheels

Rear wheels

Bucket cylinder and rod

Boom cylinder and rod

Figure 1: Typical structure of a wheel loader.

0Ground (inertial reference frame)

1

frame2

Front 4Left

3

Rightfrontwheel

9

Boom7

Bucket rod

6-DOF joint

R

RR

R

P

6

Rightrear wheel rear wheel

Rear frame

wheel5

Leftfront

R

12

R10

8

11

Bucket cylinderP

R

Boom

cylinder

Boom rod

Bucket

R

R

Figure 2: Kinematic tree of the wheel loader model.

In the proposed formulation the child body is joinedto its parent through two markers; as shown in Figure 3,one marker is attached to the parent side called connectorwhile the other marker is on the child side called the outputmarker. The output marker is the reference frame of thechild body. The body kinematic quantities and dynamicmatrices are expressed with respect to the body reference

frame. The position and orientation of the connectors aremeasured relative to the parent reference marker throughthe spatial transformation matrix, as shown in Figure 3. Thejoint degrees of freedom, representing the joint variables,are defined as the allowed relative motion between the twoconnecting markers. Relative motion between the markerscould be translation or rotation along one of the connectormarker axes. Massless markers could be inserted betweenthe two bodies to represent joints with more than one DoF.The joint variables displacement, velocities, and accelerationsare used as the body states. The Cartesian displacements,velocities, acceleration vectors, and the joint reaction forcesare considered as augmented algebraic variables.

The position vector of body 𝑖 is defined in the globalcoordinate system by position vector of the origin of thebody reference marker 𝑟0

0𝑖,𝑖, as shown in Figure 3, while the

orientation of the body could be described by the 3 × 3rotation matrix 𝑅0𝑖 which is a function of set of spatialrotational angles. The spatial velocity vector of body 𝑖, k0

0𝑖,𝑖=

[𝜔0

0𝑖𝑟0

0𝑖,𝑖]𝑇

, could be obtained by differentiating the positionvector and the body orientation parameters. As the recursiveformulation is based on relative motion, the relative spatialvelocity vector of the reference frame of a child body 𝑗 withrespect to its parent rigid body 𝑖 observed at the origin of thelocal coordinate system of body 𝑗 can be written as

k𝑗𝑖𝑗,𝑗= [𝜔𝑗

𝑖𝑗𝑟𝑗

𝑖𝑗,𝑗]𝑇

, (9)


oi xi

ojoij xij

zi

yi

𝛼

yij

zij z j

yl

yjl

xjl

xlol

olk

ylk

zkxlk

zl

xj

yj

o

Joint relative displacement of body j

revolute joint

Body i

y

x

z Body i reference frame

Body l reference frame

Body j

Inertial reference frame

Joint influence coefficient matrices

Joint relative displacement of body l

relative to body j alongx-axis prismatic joint

Body l

hjij,j = [0 0 1 0 0 0]T

hljl,l = [0 0 0 1 0 0]T

riij,i

rj

jl,j

rooi,o

ojl

z jl

relative to body i around z-axis

Figure 3: Body kinematics and joint connections.

where 𝜔𝑗𝑖𝑗is the time derivative of the orientation parameters

of body 𝑗with respect to body 𝑖 and 𝑟𝑗

𝑖𝑗,𝑗is the derivative of the

position vector of body 𝑗 with respect to body 𝑖. The relativevelocity across the joint between body 𝑖 and body 𝑗 can bewritten as follows:

k𝑗𝑖𝑗,𝑗= h𝑗q

𝑗, (10)

where h𝑗 is a 6 by 𝑛𝑑joint influence coefficient matrix or

partial velocity matrix corresponding to particular columnsof the identity matrix depending on which 𝑛

𝑑primitive

degrees of freedom are represented by q𝑗, as shown in

Figure 3, and q𝑗and q

𝑗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 asfollows [9, 10, 24]:

k𝑖 = 𝑋𝑖𝑗k𝑗, (11)

where 𝑋𝑖𝑗 is the spatial transformation matrix and 𝑖 and 𝑗indicate the coordinate systems. The general transformationmatrix𝑋𝑖𝑗 is the result of spatial rotation followed by a spatialtranslation as follows:

𝑋𝑖𝑗= 𝐿𝑖𝑗𝑅𝑖𝑗= [

𝐼 0

𝑟𝑖

𝑖𝑗𝐼] [𝑅𝑖𝑗0

0 𝑅𝑖𝑗] = [

𝑅𝑖𝑗

0

𝑅𝑖𝑗𝑟𝑖

𝑖𝑗𝑅𝑖𝑗] , (12)

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 rotationmatrix, and𝑅𝑖𝑗 is the 3 × 3 transformationmatrix relating the coordinate frames 𝑖 and 𝑗.

The spatial velocity vector of body 𝑗 defined in the globalcoordinate system could be computed as follows:

k00𝑗,0= 𝑋0𝑗k𝑗0𝑗,𝑗,

where k𝑗0𝑗,𝑗= 𝑋𝑗𝑖k𝑖0𝑖,𝑖+ k𝑗𝑖𝑗,𝑗= k𝑗0𝑖,𝑗+ h𝑗𝑗,𝑗q𝑗.

(13)

The spatial velocity vector of a descendant body could becalculated recursively using its parent spatial velocity asfollows:

k00𝑗,0= k00𝑖,0+ k0𝑖𝑗,0= k00𝑖,0+ 𝑋0𝑗h𝑗q𝑗. (14)

Assuming the system shown in Figure 3 is modeled usingrigid bodies, the velocity of body 𝑙 could be written in asimilar way as follows:

k𝑙0𝑙,𝑙= 𝑋𝑙𝑗k𝑗0𝑗,𝑗+ h𝑙q𝑙. (15)

Rearranging the terms in the velocity equations, the recursiveform of the system velocity could be expressed as follows [13]:

[

[

𝐼 0 0

−𝑋𝑗𝑖

𝐼 0

0 −𝑋𝑙𝑗𝐼

]

]

[[

[

k𝑖𝑜𝑖,𝑖

k𝑗𝑜𝑗,𝑗

k𝑙𝑜𝑙,𝑙

]]

]

= [

[

h𝑖 0 0

0 h𝑗 00 0 h𝑙

]

]

[

[

q𝑖

q𝑗

q𝑙

]

]

. (16)

The velocity recursive equations in (16) could be written in acompact form as follows:

𝑇ℓℓ

𝑎kℓ𝑎= Hℓ𝑎q𝑎= kℓ𝑟, (17)

where 𝑇ℓℓ𝑎

represents the assembled system topology orconnectivity matrix which could be constructed from theconnectivity graph, the superscript ℓ refers to local matricesand the superscript 𝑎 refers to the system assembly matrix,andHℓ

𝑎is the assembly influence coefficientmatrices grouped

in blocks corresponding to the joints’ DoF.


It should be mentioned that 𝑇ℓℓ𝑎

is a lower triangularmatrix and thus has simple inverse that maintains thetopological structure of the original matrix and becomes anupper triangular matrix as follows [26]:

(𝑇ℓℓ

𝑎)−1

=[[

[

𝐼 −(𝑋𝑗𝑖)𝑇

0

0 𝐼 −(𝑋𝑙𝑗)𝑇

0 0 𝐼

]]

]

. (18)

Similarly the spatial acceleration vector of body 𝑗 couldbe obtained by differentiating the velocity vector defined in(14) as follows:

a00𝑗,0= a00𝑖,0+ a0𝑖𝑗,0,

where, a0𝑖𝑗,0= h0𝑗,0q𝑗+ k00𝑗,0

h0𝑗,0q𝑗.

(19)

Assuming that the quadratic velocity term could be expressedas 𝛾00𝑗,0= k00𝑗,0

h0𝑗,0q𝑗= k00𝑗,0

k00𝑗,0

, the global acceleration vectorcould written as follows:

a00𝑗,0= a00𝑖,0+ a0𝑖𝑗,0= a00𝑖,0+ h0𝑗,0q𝑗+ 𝛾0

0𝑗,0. (20)

The recursive form to calculate the system local accelerationscould be written as follows:

𝑇ℓℓ

𝑎aℓ𝑎= Hℓ𝑎q𝑎+ Hℓ𝑎q𝑎= Hℓ𝑎q𝑎+ kℓ𝑟kℓ𝑟= Hℓ𝑎q𝑎+ 𝛾ℓ

𝑎. (21)

Equations (17) and (21) could be used to recursivelycalculate the velocity and acceleration of the system bodiesmarching outward from the root to the descendant bodies.Similarly, the system topological matrix could be used toproject the joint forces in the joint subspace as follows:

𝑄𝑎= (𝐻𝑙

𝑎)𝑇

F𝑙𝑎. (22)

The kinematic equations derived earlier could be used toexpress the rigid body equations of motion from themomen-tum equations. The spatial momentum vector of the rigidbody 𝑗 can be written as follows:

P𝑗𝑗,𝐺= 𝑀𝑗𝑗

𝑗,𝐺𝐺k𝑗0𝑗,𝐺, (23)

where P𝑗𝑗,𝐺

is the spatial momentum of body 𝑗,𝑀𝑗𝑗𝑗,𝐺𝐺

is thespatial massmatrix defined at the body center ofmassmarker𝐺, and k𝑗

0𝑗,𝐺is the global velocity of the marker 𝐺 located at

the body center of mass.The centroidal spatial mass matrix isdefined as follows:

𝑀𝑗𝑗

𝑗,𝐺𝐺= [𝐽𝑗𝑗

𝑗,𝐺𝐺0

0 𝑚𝑗𝐼] , (24)

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 amarker located at the body center ofmass,𝐺.This rigid bodymomentum can be transformed intothe global coordinate system and differentiated with respectto time to obtain the body equation of motion as follows:

𝑀00

𝑗,00a00𝑗,0− k0𝑇0𝑗,0𝑀00

𝑗,00k00𝑗,0= G0𝑗,0, (25)

where the symbol 0 refers to global coordinate system,G0𝑗,0= [𝜏0

𝑗,0𝑔0

𝑗]𝑇

is the spatial force vector including thegravitational forces, 𝑔0

𝑗is the sum of all external forces acting

on the body 𝑗, and 𝜏0𝑗,0

contains the sum of all torques and themoments of all forces about the origin of frame 0.

The equation ofmotion of any two connected bodies 𝑖 and𝑗 could be driven from the free body diagram as follows. Forbody 𝑗, the equation of motion in its local coordinate systemcould be written as

𝑀𝑗𝑗

𝑗,𝑗𝑗a𝑗0𝑗,𝑗− F𝑗𝑗,𝑗= G𝑗𝑗,𝑗+ k𝑗𝑇0𝑗,𝑗

P𝑗𝑗,𝑗, (26)

where F𝑗𝑗,𝑗

is the vector of reaction forces of the jointconnecting body 𝑗 to its parent.While for body 𝑖, the equationof motion in its local coordinate system could be written as

𝑀𝑖𝑖

𝑖,𝑖𝑖a𝑖0𝑖,𝑖− F𝑖𝑖,𝑖+ (𝑋𝑗𝑖)𝑇

F𝑗𝑗,𝑗= G𝑖𝑖,𝑖+ (k𝑖0𝑖,𝑖)𝑇

P𝑖𝑖,𝑖. (27)

The equations could be rearranged in matrix form asfollows:

[𝑀𝑖𝑖

𝑖,𝑖𝑖0

0 𝑀𝑗𝑗

𝑗,𝑗𝑗

][a𝑖0𝑖,𝑖

a𝑗0𝑗,𝑗

] + [𝐼 −(𝑋

𝑗𝑖)𝑇

0 𝐼] [−F𝑖𝑖,𝑖

−F𝑗𝑗,𝑗

]

= [

[

G𝑖𝑖,𝑖+ (k𝑖0𝑖,𝑖)𝑇

P𝑖𝑖,𝑖

G𝑗𝑗,𝑗+ (k𝑗0𝑗,𝑗)𝑇

P𝑗𝑗,𝑗

]

]

.

(28)

Similar to the velocity and acceleration equations (14),(15), (16), and (17), it could be shown that the connectivitymatrix could be used to simplify the expression of the systemequations of motion. The system equation of motion couldbe written for the whole system assembly in a more compactform as follows:

𝑀ℓℓaℓ𝑎− (𝑇ℓℓ

𝑎)𝑇

Fℓ𝑎= Gℓ𝑎+ (kℓ𝑎)𝑇

P. (29)

The system of equations in (29) contains the unknownCartesian acceleration of the bodies as well as the jointreaction forces. In order to be able to solve the equationof motion, the kinematic relations from (21), the constraintequations from (22), and the system dynamic equation (29)are rearranged in matrix form as follows:

[[[

[

𝑀ℓℓ

0 (𝑇ℓℓ

𝑎)𝑇

0 0 −Hℓ𝑎

𝑇ℓℓ

𝑎−(Hℓ𝑎)𝑇

0

]]]

]

[

[

aℓ𝑎

q𝑎

−Fℓ𝑎

]

]

=[[

[

Gℓ𝑎+ (kℓ𝑎)𝑇

Pℓ𝑎

𝑄𝑎

𝛾ℓ

𝑎

]]

]

,

(30)

where𝑀ℓℓ is a block diagonal inertia matrix expressed in theCartesian 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


forces. Since the inertia matrix𝑀ℓℓ is invertible, (30) couldbe solved to get the unknown joint accelerations q

𝑎which can

be used to calculate the joint Cartesian accelerations, aℓ𝑎, and

the joint reaction forces Fℓ𝑎as follows:

((Hℓ𝑎)𝑇

(𝑇ℓℓ

𝑎)−𝑇

Mℓℓ(𝑇ℓℓ𝑎)−1

Hℓ𝑎) q𝑎

= 𝑄𝑎+ (Hℓ𝑎)𝑇

(𝑇ℓℓ

𝑎)−𝑇

(Gℓ𝑎+ (kℓ𝑎)𝑇

Pℓ𝑎) .

(31)

Equation (31) could be written in a compact form as follows:

M𝑞𝑞q𝑎= Q, (32)

where M𝑞𝑞

is the generalized mass matrix projected in thejoint subspace and can be calculated as follows:

M𝑞𝑞= (Hℓ𝑎)𝑇

(𝑇ℓℓ

𝑎)−𝑇

Mℓℓ(𝑇ℓℓ𝑎)−1

Hℓ𝑎

(33)

and Q is the generalized force vector projected in the jointsubspace and can be calculated as follows:

Q = 𝑄𝑎+ (Hℓ𝑎)𝑇

(𝑇ℓℓ

𝑎)−𝑇

(Gℓ𝑎+ (kℓ𝑎)𝑇

Pℓ𝑎) . (34)

This equation of motion (32) represents minimum set ofdifferential equations that are function of the system general-ized coordinates. However, this form is limited to open-loopunconstrained mechanisms and therefore is suitable only forrobotic type mechanisms.

3. Equation of Motion of Rigid and FlexibleBody System

The flexible body dynamics can be modeled in the multibodysystem as a reduced form of the finite element model. Thefinite element can be reduced using nodal approach or modalapproach. Modal formulation uses a set of kinematicallyadmissible modes to represent the deformation of the flexiblebody [27, 28]. Componentmode synthesis approach is widelyused in the multibody dynamics codes to accurately simulatethe flexible body dynamics in the multibody system. Themain advantage of the modal formulation is that fewer elasticmodes can be used to accurately capture the flexible bodydynamics at reasonable computational cost [29, 30]. Craig-Bampton approachwas introduced to account for the effect ofboundary conditions and the attachment joints of the flexiblebody [31, 32]. The structure of the reduced flexible bodyequation-of-motion can be written as follows [23]:

[𝑀𝑟𝑟

𝑟𝑏𝑀𝑟𝑓

𝑀𝑓𝑟𝑀𝑓𝑓] [𝑎𝑟

𝑞𝑓] = [

𝐺𝑟

𝑄𝑓] , (35)

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 matrixassociated with the elastic coordinates, 𝑎𝑟 is the reference

frame acceleration in the Cartesian space, 𝑞𝑓 representsthe 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 ofelastic forces projected into the modal space. It should bementioned that the matrices𝑀𝑟𝑟

𝑟𝑏,𝑀𝑟𝑓, and𝑀𝑓𝑟 depend on

the elastic coordinates and should be updated every time step.To improve the computation efficiency of the flexible bodysimulation, a set of inertia shape invariants (ISI) are identifiedand calculated before the simulation starts. The ISI are usedduring the dynamic simulation to update the inertia terms.The ISI depend on the kinematic expression of the systemequations.

A general form of the equation of motion of multibodysystem with flexible bodies could be written in a compactform as follows [23]:

[[

[

M𝑟𝑟 T𝑇 M𝑟𝑓T 0 −H

(M𝑟𝑓)𝑇

−H𝑇 M𝑓𝑓]]

]

[

[

a−Fq]

]

= [

[

G𝛾

Q]

]

, (36)

where the block diagonal matrix M𝑟𝑟 is composed of sixby six inertia matrices that represent the Cartesian inertiamatrices associated with the reference frame for the rigidand flexible bodies in the system,M𝑟𝑓 is the inertia couplingterms between the modal elastic coordinates and Cartesianreference accelerations, M𝑓𝑓 is a block diagonal matrixcontaining the flexible body inertias, T is a lower triangulartopology matrix, H is a block matrix of the joint influencecoefficient matrices, a is the vector of Cartesian accelerations,F is the vector of joint reaction forces, q is the vector of jointaccelerations with the appended elastic coordinates, G is thevector of external forces, 𝛾 is a column matrix of 6 × 1 spatialquadratic acceleration vectors, andQ is the vector of joint andelastic forces.

The system of equations given by (36) can be easily solvedfor the joint accelerations and the modal coordinates accel-erations. Then the Cartesian accelerations and the reactionforces could be easily computed. Since the inverse of systemtopology matrix T is just its transpose, manipulating theterms in (36), we can get the joint and modal accelerationsas follows:

(M𝑓𝑓 +H𝑇 (T𝑇M𝑟𝑟T)H +H𝑇T𝑇M𝑟𝑓 +M𝑓𝑟TH) q

= Q +H𝑇T𝑇G −H𝑇 (T𝑇M𝑟𝑟T) 𝛾 −M𝑓𝑟T𝛾.(37)

This equation could be written in a compact form as follows:

M𝑞𝑞q𝑎= Q. (38)

The Cartesian accelerations could be calculated as follows:

a = THq + T𝛾, (39)

and the Cartesian joint reaction forces could be obtainedfrom

F = T𝑇M𝑟𝑓 + (T𝑇M𝑎𝑎T)Hq − T𝑇G + (T𝑇M𝑎𝑎T) 𝛾. (40)


The sequence of evaluating the terms in (37) to (40) could beoptimized in order tominimize the computational efforts andto avoid repeated calculations. Both (32) and (38) representtheminimum set of differential equations that can completelydescribe the system dynamics.

4. Equation of Motion of Constrained System

The abovementioned development could be efficiently usedto model open-loop systems like robots and human bodysystems. In the open-loop systems, each body is connectedwith one joint only. This open-loop systems approach willnot be adequate to model the wheel loader system shownin Figure 1. In the kinematic tree, shown in Figure 2, thesolid arrows represent a joint where the dotted lines representa joint that will form a closed kinematic loop. Accordingto this description, the kinematic tree of the wheel loadercontains two closed loops formed of the following list ofbodies: Γ1 = {2, 8, 11, 9} and Γ2 = {2, 9, 12, 10, 7}. Each ofthose two loops could be represented by a set of constraintequations that are function of the joint variable of all thebodies forming the loop as follows:Φ1(q

2, q8, q11, q9) = 0 and

Φ2(q2, q9, q12, q10, q7) = 0. The first and second derivatives

of the constraint equations could be easily evaluated fromthe kinematic equations. The multibody system’s equationsof motion with appended constraints could be formed asfollows:

[M𝑞𝑞

J𝑇J 0

] [q𝑎

𝜆𝑞

] = [Q𝛾𝑞

] , (41)

whereM𝑞𝑞is the generalized mass matrix from (32) and (38),

J is the Jacobian matrix, and the other column matrices aresimilar to those described earlier.

The coefficient matrix in the left hand side of (41) isoften singular, and general-purpose sparse matrix solveralgorithms are routinely used to analyze, factor, and solvethe equations. An alternative approach based on generalizedcoordinate partitioning [14] may be employed at this stage.The factored forms of (33) and (34) used to obtain (32) playa significant role in the solver efficiency. The generalizedcoordinate partitioning method [14] first analyzes the modeltopology and decomposes it into a number of kinematicallyuncoupled superelements, each with a small Jacobian matrix[2].Theuncoupled acceleration constraint equations could bearranged as follows:

[[[

[

Φ1

𝑞⋅ ⋅ ⋅ 0

......

...0 ⋅ ⋅ ⋅ Φ

𝑛

𝑞

]]]

]

[[

[

𝑞1

...𝑞𝑛

]]

]

=[[

[

𝛾1

...𝛾𝑛

]]

]

. (42)

The Jacobianmatrices in (42) are factored into LU form usingcomplete pivoting to partition the variables into dependentand independent sets. The results are then combined into arevised set of constraint equations [33]

𝑞𝑑= 𝐵 𝑞𝑖+ 𝛾𝑑, (43)

where 𝑞𝑑 is the vector of dependent variables and 𝑞

𝑖 is thevector of independent variables. Appending (43) with a set

of Lagrange multipliers to the partitioned and permutedequations of motion gives

[[

[

𝑀𝑑𝑑

𝑞𝐼 𝑀

𝑑𝑖

𝑞

𝐼 0 −𝐵

𝑀𝑖𝑑

𝑞−𝐵𝑇𝑀𝑖𝑖

𝑞

]]

]

[[

[

𝑞𝑑

𝜆𝑑

𝑞𝑖

]]

]

= [

[

𝑄𝑑

𝛾𝑑

𝑄𝑖

]

]

. (44)

The coefficient matrix in the upper-left part of (44) isstructurally nonsingular, so it can be reduced to

𝑀𝑖𝑖

𝑞𝑞𝑖= 𝑄𝑖

, (45)

where

𝑀𝑖𝑖

𝑞= 𝑀𝑖𝑖

𝑞+𝑀𝑖𝑑

𝑞𝐵 + 𝐵𝑇𝑀𝑑𝑖

𝑞+ 𝐵𝑇𝑀𝑑𝑑

𝑞𝐵,

𝑄𝑖

= (𝑄𝑖−𝑀𝑖𝑑

𝑞𝛾𝑑) + 𝐵𝑇(𝑄𝑑−𝑀𝑑𝑑

𝑞𝛾𝑑) .

(46)

Equation (45) could be solved by Cholesky factorization.

5. Constraint Enforcement

The equations of motion in (41) and (44) are index-2differential algebraic equations (DAE) [1]. This set of DAEare traditionally linearized and solved by implicit numericalintegrators [34–36]. The generalized coordinate partitioningapproach [14] is used to avoid the complexity and costof implicit solvers, but this approach does not guaranteecontinued satisfaction of the system constraints. Baumgarte[37] introduced a constraint stabilization method based onthe control theory. In this approach, the original accelerationlevel constraints are replaced with

Φ + 𝛼Φ + 𝛽Φ = 0, (47)

where 𝛼 and 𝛽 are often chosen to appropriately achievecritical damping.TheBaumgarte stabilization is implementedin the following form:

Φ + 2𝜔𝑛Φ + 𝜔

2

𝑛Φ = 0, (48)

where 𝜔𝑛is a user-specified gain (natural frequency). Unfor-

tunately, the value of the parameter𝜔𝑛is problem dependent,

and no general procedure exists for its determination. IfΦ =Φ = Φ = 0 represents the constraint equations associatedwith (41), then substituting (48) into (41) leads to

[M𝑞J𝑇

J 0] [

q𝜆𝑞

] = [Q

𝛾𝑞− 2𝜔𝑛Φ − 𝜔

2

𝑛Φ] . (49)

A similar modification would be made to the right handside 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 motionwill tend to be stiff and the integrator will be slow. If 𝜔

𝑛

is too small, the stabilization algorithm will not efficientlyreduce the constraint violations. From experience, the max-imum value of best 𝜔

𝑛is probably near the highest system

frequencies in the system. To this end, the developed set ofdynamic equations of motion is sufficient to perform thedynamic simulation of open- and closed-loop systems. It isalso sufficient for modeling rigid and flexible bodies in themultibody system.


6. Static Equilibrium Analysis

The mechanical system can be considered at static equilib-rium if the kinetic energy is zero or constant target value andthe potential energy is minimum. Many methods have beenproposed to solve static equilibrium in nonlinear multibodysystem models. Most of those proposed techniques requireminimizing potential functions and/or linearizing the equa-tions of motion [36] as mentioned earlier. Writing potentialfunctions for arbitrary nonlinear forcemodules or linearizingthem is a daunting task.Theproblembecomesmore challeng-ing when models contain friction and other history-basedforcemodels because unique iterative static solutionsmay notexist.Theproposed approach uses the dynamicmodel itself tosolve the static equilibrium position.This approachmay havesome drawbacks because convergence can be slow in lightlydamped systems. In order to improve the solver efficiency, anenergy-drainage mechanism is used.

In the energy-drainage approach, a technique similar tothe Baumgarte equation was implemented into the solver.After solving (45) to obtain the accelerations q, the solverimposes the additional requirement on the second derivatives

𝑞𝑑= −2𝜔

𝑑𝑞𝑖− 𝜔2

𝑑(𝑞𝑖− 𝑞𝑖−1) , (50)

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 oscillatetoo long. And if 𝜔

𝑑is too large, damping will be strong and

the model will drift slowly toward equilibrium. With thisdampingmethod, equilibrium is approached as ‖𝑞

𝑖−𝑞𝑖−1‖ gets

acceptably small, not necessarily when the kinetic energy getssmall.

Another feature of energy-drainage damping is its abilityto inhibit the effects of bad initial conditions that oftencause models to fly apart and the numerical integratorto fail. Extremely high forces from the offending contactmodels can cause unrealistic accelerations, which are thenintegrated to give unrealistic velocities and displacements.When energy-drainage damping is active, the resulting largesystem velocities are immediately fed back through theabove damping equation, and this generates large opposingacceleration terms, canceling those from the model. Withenergy-drainage damping, the contacting bodies will tend todrift slowly apart until the forces and accelerations becomereasonable.

7. Recursive Algorithm

At the beginning of the dynamic simulation, the structure ofthe equation of motion is determined based on a preliminaryanalysis of the system topology. The dependent and inde-pendent variable sets are determined using the generalizedcoordinate portioning approach. The solver integrates bothdependent and independent variable sets and the kinematicconstraints are enforced using Baumgarte stabilization, asexplained in the previous section. The input states to theintegrator represent the first and second time derivative of

the joint variables and the output states are the joint dis-placements and the joint velocities. The recursive algorithmof the multibody dynamic simulation can be summarized asfollows.

(1) Using the joint variables which returned from theintegrator, the solver calculates the Cartesian dis-placements, velocities, and accelerations. Forwardevaluation scheme is utilized (starting from the rootbody 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 anddependent set of variables.

(4) Calculate the constraint violations and apply theBaumgarte stabilization.

(5) Calculate the internal and external forces and applythem to the different bodies (interaction forcesbetween bodies, driver forces, soil/terrain forces, etc.).

(6) Transform the inertia matrices into global coordinatesystem.

(7) Calculate the inertia forces and centrifugal and Cori-olis forces in Cartesian space.

(8) Propagate the external and inertia forces from thedescendant bodies into their parents.

(9) Project the Cartesian forces into the body joint space.(10) Factor themassmatrix based on the current indepen-

dent and dependent variables selection.(11) Calculate second derivatives of the joint variables.(12) Apply the energy drainage operator according

to (50).(13) Send the states to the integrator.

The solver utilizes a predictor-corrector integrator withvariable-order interpolation polynomials and variable timestep. This explicit integrator insures the stability of thesolution and the ability to capture the high-speed impactsbetween the different machine parts.

8. Examples

This section presents some examples of the simulation resultsmultibody system ranging from simple pendulum to a fullwheeled machine. The first example represents a uniformbeam connected to the ground with a revolute joint to form asingle pendulum, as shown in Figure 4. The link has mass of1.7595Kg. The beam center of mass is located in the middle.The pendulumwas originally horizontal and is allowed to fallunder the effect of gravity. The static equilibrium position ofthis pendulum is known to be vertical.

The proposed multibody formulation and solver wereused to model the pendulum and compute the static equi-librium position. The transient simulation is run for a fewiterations to allow the system to pick up some kinetic energy.Once the pendulum starts to move the energy drainingmechanism is applied. A stopping criterion is set to be a target


0.751.5

Figure 4: Single pendulum beam.

10

0

−10

−20

−30

−40

−50

−60

−70

−80

−90

−100

Pend

ulum

angl

e

Time0 1 2 3 4 5 6 7

Figure 5: The pendulum angle as it falls under the effect of gravity.

Kine

tic en

ergy

(J),

velo

city

(rad

/s)

6

5

4

3

2

1

−1

−2

−3

Velocity (rad/s)Kinetic energy (J)

Time0

0

1 2 3 4 5 6 7

Figure 6: The velocity and kinetic energy of the pendulum.

value of the kinetic energy. When the pendulum reaches thistarget value, it would have reached the static equilibrium.

Figure 5 shows the value of the pendulum angle as it fallsunder the effect of gravity till the system reaches equilibriumposition. The kinetic energy is drained out from the systemas shown in Figure 6.

The change in the pendulum potential energy from thereference is shown in Figure 7. As the pendulum startsmoving, the potential energy of the pendulum decreases

2

0

−2

−4

−6

−8

−10

−12

−14

Pote

ntia

l ene

rgy

Time0 1 2 3 4 5 6 7

Figure 7: The potential energy of the pendulum.

1000

100

10

1

0.1

0.01

1E − 3

1E − 4

1E − 50 20 40 60 80 100 120

Kine

tic en

ergy

(J)

Time

Figure 8: Kinetic energy of the 50-link chain.

and the minimum value is reached at the static equilibriumposition. The final value of the potential energy is −12.9455Jwhich matches the closed form solution for the pendulum.

It should be mentioned that time in the plots is only arepresentation of the number of iteration to achieve solutionconvergence. Also, it should be mentioned that this examplerepresents an extreme case where the pendulum initialposition was extremely far from the equilibrium position.This example shows that the solver is stable and can achieverealistic solutions even in extreme cases.

The second example represents a system that might haveminor assembly errors or constraint violations. The systemrepresents a chain of 50 links connected with revolute joints.The chain links had same properties of the link in firstexample. In this example, the first link is attached to theground and it has initial angle of 5∘ from the vertical position.Every following link has an initial deviation of 1∘ relative to itsparent. The static equilibrium analysis was performed on thesystem.The kinetic energy of the system is shown in Figure 8.The potential energy of the system is shown in Figure 9 whilethe angles of all the links are shown in Figure 10.


−29500

−30000

−30500

−31000

−31500

−32000

−32500

0 20 40 60 80 100 120

Pote

ntia

l ene

rgy

(J)

Time

Figure 9: Potential energy of the 50-link chain.

0 20 40 60 80 100 120

0.0

0.2

0.4

0.6

0.8

1.0

1.2

Ang

le

Time

−0.2

0.9

0.6

0.3

0 2 4 6

Figure 10: Angles of the links in the 50-link chain.

To demonstrate the completeness of the proposedapproach a full machine with closed kinematic loops willbe presented in the following example. Forestry equipmentis typically required to work on different terrains ranging

from swampy, compactable, and hard soil. It may be requiredto work on slopes dragging wood logs and tree. Duringoperation and articulations, the location of the center of massof such machine may change and causes the machine to slip,flip over, or fall. Design of such machines requires accurateprediction of themachine stability under the different loadingconditions and at different articulation angles. Also, powerrequirements analysis of such machine requires the simu-lation to start from a stable static equilibrium position. Agrapple skidder machine, shown in Figure 11, will be used asa demonstration example.

The kinematic tree of the machine could be developed,as shown in Figure 12, in order to establish the parent childlist. It could be seen that the kinematic tree of grappleskidder is very similar to that of the wheel loader shownin Figure 2. The solver algorithm analyzes the kinematictree to identify the closed loops of the mechanism anddefine the constraint equations associated with the closed-loop joints. The constraint equations of the two kinematicclosed loops could be written as Φ1(q

2, q8, q11, q9) = 0 and

Φ2(q2, q9, q12, q10, q7) = 0.

The next step for the solver is to analyze the closed loopsand merge any two loops that share any joint variable intoone larger superelement.This operation could be iterative andmay lead to very large superelements. In our example, sincethe two closed loops are sharing the variables of body 9, theyhave to be merged into a large superelement that containsall the bodies of the two loops. The Jacobian matrix of thissuperelement could be calculated as follows:

J =[[[

[

𝜕Φ1

𝜕q2

𝜕Φ1

𝜕q8

𝜕Φ1

𝜕q11

𝜕Φ1

𝜕q9

0 0 0

𝜕Φ2

𝜕q2

0 0𝜕Φ2

𝜕q9

𝜕Φ2

𝜕q12

𝜕Φ2

𝜕q10

𝜕Φ2

𝜕q7

]]]

]

. (51)

The first raw block of the Jacobian matrix representsthe partial derivatives of the first kinematic loop constraintequations with respect to the joint variables of the bodiesin the first loop while the second row block represents thepartial derivatives of the second loop constraint equations.The system topology matrix will be generated by the solverand should be as follows:

𝑇ℓℓ

𝑎=

[[[[[[[[[[[[[[[[[[[[[[[[

[

𝐼

−𝑋2,1

𝐼

−𝑋3,1

0 𝐼

−𝑋4,1

0 0 𝐼

−𝑋2,1−𝑋5,20 0 𝐼

−𝑋2,1−𝑋6,20 0 0 𝐼

−𝑋2,1−𝑋7,20 0 0 0 𝐼

−𝑋2,1−𝑋8,20 0 0 0 0 𝐼

−𝑋2,1−𝑋9,20 0 0 0 0 0 𝐼

−𝑋2,1−𝑋7,20 0 0 0 −𝑋

10,70 0 𝐼

−𝑋2,1−𝑋8,20 0 0 0 0 −𝑋

11,80 0 𝐼

−𝑋2,1−𝑋9,20 0 0 0 0 0 −𝑋

12,90 0 𝐼

−𝑋2,1−𝑋9,20 0 0 0 0 0 −𝑋

12,90 0 −𝑋

13,12𝐼

]]]]]]]]]]]]]]]]]]]]]]]]

]

. (52)


Rear frameFront frame

Stick

Grapple

Front wheels Rear wheels

Boom

Stick cylinder and rodBoom cylinder and rod

Figure 11: Grapple skidder forestry machine.

0

1Front frame

2

Rearframe 4 Front

left wheel

3

Rearright wheel

9

Boom7

RR

R

R

P

6

Front right wheel

5Rearleft

wheel

R

12

R10

8

11

P

R

Boom Cylinder

Boom Rod

Stick

R

R

13

S

Grapple

C

C

Ground (inertial reference frame)

Bucket rod

6-DOF joint

Bucket cylinder

R: revolute joint

P: prismatic joint

S: spherical joint

C: closed-loop joint

Dynamic body k

Joint (parent to child )k

Figure 12: Kinematic tree of log dragging forestry equipment.


Figure 13: Static equilibrium of wheeled machine (grapple skidder) on a washboard surface.

The system assembly influence coefficient matrix could bewritten as follows:

Hℓ𝑎=

[[[[[[[[[[[[[[[[[[[[[[

[

h1 0 0 0 0 0 0 0 0 0 0 0 0

0 h2 0 0 0 0 0 0 0 0 0 0 0

0 0 h3 0 0 0 0 0 0 0 0 0 0

0 0 0 h4 0 0 0 0 0 0 0 0 0

0 0 0 0 h5 0 0 0 0 0 0 0 0

0 0 0 0 0 h6 0 0 0 0 0 0 0

0 0 0 0 0 0 h7 0 0 0 0 0 0

0 0 0 0 0 0 0 h8 0 0 0 0 0

0 0 0 0 0 0 0 0 h9 0 0 0 0

0 0 0 0 0 0 0 0 0 h10 0 0 0

0 0 0 0 0 0 0 0 0 0 h11 0 0

0 0 0 0 0 0 0 0 0 0 0 h12 0

0 0 0 0 0 0 0 0 0 0 0 0 h13

]]]]]]]]]]]]]]]]]]]]]]

]

.

(53)

When the solver starts, the Cartesian velocities and accel-erations are calculated using forward substitution from theroot body to the descendant bodies. The Jacobian matricesare updated based on the new values of the spatial dis-placements and velocities. The external forces acting on thesystem, the contact forces between the different bodies, andthe interaction forces between the system and terrain arecalculated and applied to each body’s force vector.The systemtopology matrix is used to project the Cartesian quantitiesinto the joint subspace and define the minimum set of thesystem variables as shown in (32) to (34).The system Jacobianmatrix is then appended to the inertia matrix to form theaugmented constrained multibody equation of motion. Thesolver then utilizes the generalized coordinate portioningapproach to identify the dependent and independent vari-ables by factoring the Jacobian matrix into LU form. Theindependent variable is identified, as shown in (45). Theconstraint violation penalty is applied to the system equationof motion using Baumgarte equation, as shown in (48). Theentries to the mass matrix are permuted based on the orderin the LU factorization order.

To perform the static analysis, at this point, the systemaccelerations aremodified using the energy drainage functionas shown in (50), and both the dependent and independentvariables are sent to the integrator. The integrated states

are used to calculate the Cartesian position, velocities, andaccelerations of all bodies. The Cartesian velocities are usedto calculate the kinetic energy of the system and the globalposition of the bodies is used to calculate the potentialenergies. If the value of the kinetic energy of the systemreached the target, the solver stops and reports the currentstate as the body equilibrium position.

Figure 13 shows a successful simulation to achieve thestatic equilibrium of wheeled machine. The machine isdropping from a small height over a washboard surface.As the machine drops, the rear tires fall on the inclinedplane of the surface causing the machine to roll down andbackward.As themachine contacts the opposite surface of thewashboard ditch, the machine comes to static equilibrium.

9. Conclusions

This paper presented an approach for evaluating the staticequilibrium position of multibody systems. The multibodysystem can include rigid and flexible bodies. The proposedapproach is implemented in a joint-based multibody dynam-ics formulation that uses the spatial algebra to derive theequation of motion of the multibody system. The kinematicequation of the closed-loop systems was optimized to employBaumgarte constraint stabilization approach to eliminatethe constraint violation while at the same time avoidingusing iterative constraint enforcement schemes. In order todetermine the system static equilibrium position, an energydrainage mechanism was introduced to modify the systemstates before integrating it. The static equilibrium is achievedby running a transient simulation of the dynamics systemwhile the integrated states were dampened with the energydrainage operator.

Conflict of Interests

The author declares that there is no conflict of interestsregarding the publication of this paper.

References

[1] M. Giampiero and P. Manfred, Simulation Algorithms in VehicleSystem Dynamics, CRC Press, 2004.


[2] M. A. Omar, “An applied approach for large-scale multibodydynamics simulation and machine-terrain interaction,” SAEInternational Journal of Passenger Cars—Electronic and Electri-cal Systems, vol. 1, no. 1, pp. 820–828, 2009.

[3] J. Y. Wong, “Dynamics of tracked vehicles,” Vehicle SystemDynamics, vol. 28, no. 2-3, pp. 197–219, 1997.

[4] H. C. Lee, J. H. Choi, and A. A. Shabana, “Spatial dynamicsof multibody tracked vehicles part II: contact forces andsimulation results,” Vehicle System Dynamics, vol. 29, no. 2, pp.113–137, 1998.

[5] Z.-D. Ma and N. C. Perkins, “A track-wheel-terrain interactionmodel for dynamic simulation of tracked vehicles,” VehicleSystem Dynamics, vol. 37, no. 6, pp. 401–421, 2002.

[6] Z.-D. Ma and N. C. Perkins, “An efficient multibody dynamicsmodel for internal combustion engine systems,” MultibodySystem Dynamics, vol. 10, no. 4, pp. 363–391, 2003.

[7] P. Roland, D. Daniel, S. Emilio, and A. N. Miguel, “Validationof a multibody model for an X-by-wire vehicle prototypethrough field testing,” in Proceedings of the ECCOMASThematicConference on Multibody Dynamics, J. C. Samin and P. Fisette,Eds., Brussels, Belgium, July 2011.

[8] A. A. Shabana, Dynamics of Multibody Systems, CambridgeUniversity Press, Cambridge, UK, 3rd edition, 2005.

[9] P. E. Nikravesh, An Overview of Several Formulations forMultibody Dynamics, Product Engineering, 2005.

[10] R. Featherstone, Rigid Body Dynamics Algorithms, Springer,New York, NY, USA, 2008.

[11] R. Featherstone, “The acceleration vector of a rigid body,”International Journal of Robotics Research, vol. 20, no. 11, pp.841–846, 2001.

[12] R. Featherstone and D. Orin, “Robot dynamics: equations andalgorithms,” in Proceedings of the IEEE International Conferenceon Robotics and Automation (ICRA '00), pp. 826–834, April2000.

[13] R. Featherstone, “Efficient factorization of the joint-space iner-tia matrix for branched kinematic trees,” International Journalof Robotics Research, vol. 24, no. 6, pp. 487–500, 2005.

[14] R. A.Wehage and E. J. Haug, “Generalized coordinate partition-ing for dimension reduction in analysis of constrained dynamicsystems,” Journal of Mechanical Design, vol. 134, pp. 247–255,1982.

[15] R. A. Wehage and M. J. Belczynski, “High resolution vehiclesimulations using precomputer coefficients,” in Proceedings ofthe Winter Annual Meeting of the American Society of Mechani-cal Engineers, vol. 44, pp. 311–325, November 1992.

[16] G. Rodriguez, A. Jain, and K. Kreutz-Delgado, “Spatial operatoralgebra for multibody system dynamics,” Journal of the Astro-nautical Sciences, vol. 40, no. 1, pp. 27–50, 1992.

[17] G. Rodriguez, A. Jain, and K. Kreutz-Delgado, “Spatial operatoralgebra for manipulator modeling and control,” InternationalJournal of Robotics Research, vol. 10, no. 4, pp. 371–381, 1991.

[18] E. Eich-Soellner and C. Fuhrer, Numerical Methods in Multi-body Dynamics, Teubner, Stuttgart, Germany, 1998.

[19] C. Yang, D. Cao, Z. Zhao, Z. Zhang, and G. Ren, “A directeigenanalysis of multibody system in equilibrium,” Journal ofApplied Mathematics, vol. 2012, Article ID 638546, 12 pages,2012.

[20] J. Garcıa de Jalon and E. Bayo, Kinematic and Dynamic Simu-lation of Multibody Systems, the Real-Time Challenge, Springer,New-York, NY, USA, 1994.

[21] R. Aviles, G. Ajuria, V. Gomez-Garay, and S. Navalpotro,“Comparison among nonlinear optimization methods for thestatic equilibrium analysis of multibody systems with rigid andelastic elements,”Mechanism andMachineTheory, vol. 35, no. 8,pp. 1151–1168, 2000.

[22] Z. S. Wang, Y. Y. Tao, and Q. Y. Wen, “A vector bond graphmethod of kineto-static analysis for spatial multibody systems,”Applied Mechanics and Materials, vol. 321–324, pp. 1725–1729,2013.

[23] M. A. Omar, “Modeling flexible bodies in multibody systems injoint-coordinates formulation using spatial algebra,” Advancesin Mechanical Engineering, vol. 2014, Article ID 468986, 18pages, 2014.

[24] R. W. Wehage, “Automated procedures for robust and efficientsolution of over-constrained multibody dynamics,” in Proceed-ing of ASME International Mechanical Engineering Congressand Exposition (IMECE '12), vol. 85259, Huston, Tex, USA,November 2012.

[25] P. E. Nikravesh, “Construction of the equations of motionfor multibody dynamics using point and joint coordinates,”in Computer-Aided Analysis of Rigid and Flexible MechanicalSystems, vol. 268 of NATO ASI Series E: Applied Sciences, pp.31–60, Kluwer Academic Publishers, 1994.

[26] A. Jain, “Multibody graph transformations and analysis—partI: tree topology systems,”Nonlinear Dynamics, vol. 67, no. 4, pp.2779–2797, 2012.

[27] M. A. Omar, Finite Element Modeling of Leaf Springs in Multi-body Systems, VDMVerlag Dr. Muller Aktiengesellschaft & Co.KG, Saarbrucken, Germany, 2010.

[28] M. A. Omar, A. A. Shabana, A. Mikkola, W.-Y. I. Loh, and R.Basch, “Multibody system modeling of leaf springs,” Journal ofVibration and Control, vol. 10, no. 11, pp. 1601–1638, 2004.

[29] K. J. Bathe,Finite Element Procedures, Prentice-Hall, EnglewoodCliffs, NJ, USA, 1996.

[30] T. J. R. Hughes, The Finite Element Method: Linear Static andDynamic Finite Element Analysis, Dover, New York, NY, USA,2000.

[31] R. R. Craig and M. C. Bampton, “Coupling of substructures fordynamic analyses,” AIAA Journal, vol. 6, no. 7, pp. 1313–1319,1968.

[32] R. R. Craig and C. J. Chang, “On the use of attachment modesin substructure coupling for dynamic analysis,” in Proceedingsof the 18th Conference on Structures, Structural Dynamics andMaterials, San Diego, Calif, USA, 1977.

[33] A. A. Shabana, Computational Dynamics, John Wiley & Sons,3rd edition, 2010.

[34] U. M. Ascher and L. R. Petzold, Computer Methods for Ordi-naryDifferential Equations andDifferential-Algebraic Equations,Society for Industrial and Applied Mathematics, Philadelphia,Pa, USA, 1998.

[35] K. E. Brenan, S. L. Campbell, and L. R. Petzold, Numeri-cal Solutions of Initial-Value Problems in Differential-AlgebraicEquations, Society for Industrial and Applied Mathematics,New York, NY, USA, 1996.

[36] J. Garcıa de Jalon and E. Bayo, Kinematic and Dynamic Simu-lation of Multibody Systems, the Real-Time Challenge, Springer,NewYork, NY, USA, 1994.

[37] J. Baumgarte, “Stabilization of constraints and integrals ofmotion in dynamical systems,” Computer Methods in AppliedMechanics and Engineering, vol. 1, no. 1, pp. 1–16, 1972.

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

RoboticsJournal of

Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation http://www.hindawi.com

Journal ofEngineeringVolume 2014

Submit your manuscripts athttp://www.hindawi.com

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014

SensorsJournal of


Modelling & Simulation in EngineeringHindawi Publishing Corporation http://www.hindawi.com Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks


research article static analysis of large-scale multibody...

Documents