# A divide and conquer algorithm for constrained multibody system dynamics based on augmented Lagrangian method with projections-based error correction

- First Online:

- Received:
- Accepted:

DOI: 10.1007/s11071-012-0503-2

- Cite this article as:
- Malczyk, P. & Frączek, J. Nonlinear Dyn (2012) 70: 871. doi:10.1007/s11071-012-0503-2

## Abstract

This paper presents a new parallel algorithm for dynamics simulation of general multibody systems. The developed formulations are iterative and possess divide and conquer structure. The constraints equations are imposed at the acceleration level. Augmented Lagrangian methods with mass-orthogonal projections are used to prevent from constraint violation errors. The proposed approaches treat tree topology mechanisms or multibody systems which contain kinematic closed loops in a uniform manner and can handle problems with rank deficient Jacobian matrices. Test case results indicate good accuracy performance dependent on the expense put in the iterative correction of constraint equations. Good numerical properties and robustness of the algorithms are observed when handling systems with single and coupled kinematic loops, redundant constraints, which may repeatedly enter singular configurations.

### Keywords

Divide and conquer algorithmAugmented Lagrangian methodMass-orthogonal projectionsParallel computing## 1 Introduction

Efficient dynamics simulations of complex multibody systems (MBS) are of crucial importance in many areas of computer aided engineering and design. There are many examples of such systems including vehicles, biomechanical models, robots and multidisciplinary applications. Computations can be carried out by means of different types of formulations. To meet requirements for high-fidelity performance and accurate dynamics simulations of complex systems, it has become a practice to apply efficient, low order algorithms designed both for sequential and parallel computations.

The examples of efficient *O*(*n*) (*n*—number of bodies) recursive sequential algorithms for analysis of tree-like topology rigid body dynamics can be found in [1, 4, 5, 7, 14, 22, 36, 37, 41, 42]. Some researchers developed recursive order “*O*(*n*)” formulations dynamics simulations of multibody systems with closed loops [2, 33, 38, 39]. These algorithms gave basis for further development of efficient low order formulations.

As parallel computing resources became more available, researchers began to adapt the existing formulations or design completely new algorithms, suitable for parallel computing. The strategies enabled to decrease the turnaround time associated with computer simulations and even achieve results in real-time. The first attempts to exploit parallel strategies can be found in [6, 12, 18, 19, 25, 26].

In the literature, there are more recent ideas regarding parallel algorithms for rigid multibody dynamics simulation. Featherstone [15, 16] developed the optimal-time, logarithmic order divide and conquer algorithm (DCA) for dynamics of general multibody systems. The idea behind the formulation lies in a recursive binary assembly and disassembly of articulated bodies. The extension to closed loop systems were obtained through the application of constraint stabilization methods and proper decomposition of constraint forces. Fisette and Peterkenne [20] and independently Anderson and Duan [3] adopted the idea of a decomposition of multibody system into subchains. In [13], the authors explored the ideas of recursive coordinate reduction and presented parallel multibody algorithm with optimal logarithmic time complexity for general multibody systems applicability. The formulation, however, was not free from problems arising from the fact that some matrices lost their ranks. Yamane and Nakamura [43] developed the new assembly-disassembly algorithm, which is based on the concept of divide and conquer scheme. In work [32], Mukherjee and Anderson presented exact and non-iterative divide and conquer algorithm for forward dynamics of MBS with single and coupled loops, which incorporated neither coordinate reductions nor Lagrange multipliers. The latest comparative study on efficient sequential and parallel multibody dynamics algorithms can be found in [44] and in recent books [17, 23].

Based on the divide and conquer scheme [15], the authors presented recently parallel algorithms for general constrained MBS dynamics using augmented Lagrangian methods and mass-orthogonal projections [27–31]. The improved, extended and verified versions of the methods are proposed in this paper. The developed parallel formulations for dynamics of general multibody systems encompass efficiency, accuracy and robustness in case of analyzing systems with redundant constraints containing single or multiple kinematic loops, and which may enter singular configurations.

## 2 Algorithm overview

### 2.1 Preliminaries

### 2.2 Assembly phase

#### 2.2.1 Equations of motion for two articulated bodies

*i*=1,…,

*n*is the number of bodies in the system. Vector

**r**

_{i}describes a position of the origin (located in body’s mass center) of the reference frame rigidly attached to body

*i*with respect to global reference frame and

**p**

_{i}is a vector of Euler parameters representing orientation of the local frame. Moreover, Lagrange multipliers

**λ**_{1},

**λ**_{2},

*are associated with constraint forces in joints (Fig. 2).*

**λ***A*and

*B*can be written in the form, which takes into account Euler parameter representation of rotations:

**M**

^{A}and

**M**

^{B}are 7×7 quantities, which are known to be singular [21, 34], vectors

**Q**

^{A},

**Q**

^{B}are applied, position and velocity dependent forces, Lagrange multipliers

**λ**_{1},

**λ**_{2},

*are associated with joint constraint equations*

**λ****Φ**

^{1},

**Φ**

^{2},

**Φ**, and \({\lambda}_{A}^{N}\), \({\lambda}_{B}^{N}\) are Lagrange multipliers related to Euler parameters normalization constraints

**Φ**

^{N}for body

*A*and

*B*. The joint between the bodies imposes the constraint equations that could be expressed at the acceleration level as follows:

^{AB}is that part of acceleration constraint equations which are dependent on positions and velocities. The Euler parameters normalization constraints can be also written as a second derivatives with respect to time:

*A*and

*B*(see [15, 29]). The matrices

**M**

^{A}and

**M**

^{B}are not invertible in Eqs. (1) and (2). To overcome this difficulty, augmented Lagrangian formulation is employed [10, 11, 24, 35], which leads to the following Lagrange multipliers iterative approximation: where

**λ**^{∗}, \({\lambda}^{N*}_{A}\) and \({\lambda}^{N*}_{B}\) are Lagrange multipliers obtained from the previous iteration and multipliers without star sign are related to the current iteration. We assume that

**λ**^{∗}=

**0**, \({\lambda}^{N*}_{A}=0\), \({\lambda}^{N*}_{B}=0\) for the initial iteration. The matrices \(\boldsymbol {\upalpha}=\alpha \mathbf {I}=\operatorname{diag}(\alpha)\), μ and

**Ω**are diagonal matrices that contain values of the large penalty numbers, natural frequencies and damping ratios of the system added to each constraint to fulfill constraint equations [9–11]. Let us substitute Eqs. (6), (7), and (8) to Eqs. (1) and (2) to obtain the following result:

Equations (9) and (10) form a basis for further derivations. They are starting point for the main pass phase. In contrast to Eqs. (1) and (2), relations (9) and (10) can be solved for \(\ddot{\mathbf {q}}^{A}\) and \(\ddot{\mathbf {q}}^{B}\), respectively. The encountered leading matrices are symmetric and positive definite, therefore, they are invertible, even in situations when constraint Jacobian matrices lose their rank.

#### 2.2.2 Equations of motion for the system of articulated bodies

*A*and

*B*interconnected by joints, as shown in Fig. 3.

*A*and

*B*serve as those, which are eliminated during the computations. The generalization of Eqs. (9) and (10) may be written as follows: and

*C*, in the form:

*A*and

*B*can be found from the iterative process:

**I**is the identity matrix of the proper dimension. The relations (18) can be solved in terms of Lagrange multipliers between the sets of bodies

*A*and

*B*:

Relations (27) to (32) are used for the first computational stage called assembly phase. The recursive formulas are exploited until the whole MBS is constructed according to the binary tree associated with mechanism decomposition.

### 2.3 Disassembly phase

*A*in Fig. 3) and \(\ddot{\mathbf {q}}_{2}^{C}\) is the acceleration of the last body (body 2 from the set

*B*in Fig. 3). Lagrange multipliers

**λ**_{1}and

**λ**_{2}correspond to constraints connecting first and last body with topological parent and child bodies, respectively, provided they exist. There are three different boundary cases associated with the root node of the binary tree:

- 1.
**λ**_{1}=**0**,**λ**_{2}=**0**.Taking into account Eqs. (33) and (34), the accelerations can be evaluated from the following system of linear equations:The matrix coefficients \(\mathbf {M}_{11}^{C}\) and \(\mathbf {M}_{22}^{C}\) are symmetric and positive definite, moreover \(\mathbf {M}_{12}^{C}=(\mathbf {M}_{12}^{C})^{T}\). The accelerations \(\ddot{\mathbf {q}}_{1}^{C}\) and \(\ddot{\mathbf {q}}_{2}^{C}\) can be easily evaluated from Eq. (35) to begin disassembly phase. The case can be used for the simulation of free-floating systems.$$ \left [\begin{array}{c@{\quad }c} \mathbf {M}_{11}^{C} & \mathbf {M}_{12}^{C} \\[3pt] \mathbf {M}_{21}^{C} & \mathbf {M}_{22}^{C} \end{array} \right ] \left [\begin{array}{c} \ddot{\mathbf {q}}_{1}^{C} \\[3pt] \ddot{\mathbf {q}}_{2}^{C}\end{array} \right ] = \left [\begin{array}{c} \mathbf {Q}_{1}^{C} \\[3pt] \mathbf {Q}_{2}^{C} \end{array} \right ]. $$(35) - 2.
**λ**_{1}≠**0**i**λ**_{2}=**0**or inversely.The Lagrange multipliers**λ**_{1}can be approximated from the process (6) or (17) as follows:$$ \boldsymbol {\lambda}_{1}=\boldsymbol {\lambda}_{1}^{*}+\boldsymbol {\upalpha}\bigl(\boldsymbol {\Phi}_{\mathbf {q}_{1}^{C}}^{1}\ddot{\mathbf {q}}_{1}^{C}+ \overline{\boldsymbol {\gamma}}^{AB}_{1}\bigr). $$(36) - 3.
**λ**_{1}≠**0**and**λ**_{2}≠**0**.The Lagrange multipliers**λ**_{1}and**λ**_{2}can be obtained from the approximation similar to that in Eq. (36): Inserting Eq. (38) into (33) and Eq. (39) into (34) yields: Again the matrix coefficients in linear system of Eqs. (40) are symmetric and positive definite, thus they are invertible. This case can be applied for analysis of general closed loop multibody systems.

The process of assembly starts with equations of motion for individual bodies in the system and is continued until the whole mechanism is composed. Subassemblies, which correspond to nodes in the graph, are constructed by traversing the binary tree, according to Eqs. (27)–(32). Finally, the procedure for solving the equations of motion of the root node using the boundary conditions described in this subsection is employed. Traversing the tree from root node to leaves, all accelerations are computed according to Eq. (21) and Eq. (23). Moreover, if it is needed, Lagrange multipliers can be evaluated using Eq. (17) or Eq. (19).

## 3 Mass-orthogonal projections

### 3.1 Introduction

Derived parallel divide and conquer scheme possesses good overall characteristics and robustness in case of general multibody dynamics simulations. However, the formulation does not enforce simultaneously the error control on position and velocity constraints. In order to overcome these drawbacks, mass-orthogonal projections in positions and velocities are employed for further improvements [10, 11]. The process of correction of the state variables is numerically expensive. The projections in positions and velocities are expressed in a form of separate algorithms, which maintain the divide and conquer structure [27]. The Appendix accompanying the paper may be helpful in understanding some of the following relations.

### 3.2 Projections in positions

**q**

_{∗}may not satisfy position constraint equations. In order to correct bodies’ positions, mass-orthogonal projections of the solution to the constraint manifold are performed [10]. Consider again the system of two rigid bodies connected by a joint, as depicted in Fig. 2. The projections in positions [10, 27] can be written in a form, which is similar to that expressed in Eqs. (1) and (2) (see Eq. (93) in the Appendix): where vectors

*(with and without subscripts or superscripts) are associated with the problem of constrained minimization of the functional*

**λ****Φ**(

**q**,

*t*)=

**0**[10, 27], whereas Δ

**q**

^{A}and Δ

**q**

^{B}are unknown position corrections. Constraint equations between body

*A*and

*B*can be written as

*A*and

*B*can be expressed as

*with the asterisk sign are quantities evaluated in previous iteration. We assume*

**λ**

**λ**^{∗}=

**0**, \({\lambda}^{N*}_{A}=0\), \({\lambda}^{N*}_{B}=0\) for initial iteration. Inserting Eqs. (45), (46), and (47) into Eqs. (41) and (42) yields:

*C*(Fig. 3) in the form:

*A*and

*B*can be evaluated using the following iterative scheme (see Eq. (100) in the Appendix):

Relations (50)–(56) take on the same form of Eqs. (11)–(17). Consistently, the assembly phase can be performed by using relations (27)–(32). The matrix coefficients (27)–(30) are computed once per integration step and are exploited in the velocity projections as well as in the process of acceleration evaluation. On the other hand vector quantities \(\mathbf {Q}_{i}^{A}\), \(\mathbf {Q}_{i}^{B}\) for *i*=1,2 in Eqs. (31)–(32) are computed in each iteration. The iterative scheme is continued until the stop criterion is met, which can be formulated as ∥Δ**q**∥<*ϵ*_{p}, where *ϵ*_{p} is the user specified tolerance.

### 3.3 Projections in velocities

*(with and without subscripts or superscripts) correspond to the process of constrained minimization of the functional \(V=\frac{1}{2}(\dot{\mathbf {q}}-\dot{\mathbf {q}}_{*})^{T}\mathbf {M}(\dot{\mathbf {q}}-\dot{\mathbf {q}}_{*})\) subjected to velocity constraint equations \(\dot{\boldsymbol {\Phi}}(\mathbf {q},\dot{\mathbf {q}},t)=\boldsymbol {0}\). The constraint equations between body*

**λ***A*and

*B*and Euler parameter normalization constraints at the velocity level can be written as follows:

*with the asterisk sign are evaluated in previous iteration. We assume*

**λ**

**λ**^{∗}=

**0**, \({\lambda}^{N*}_{A}=0\), \({\lambda}^{N*}_{B}=0\) for the initial iteration. Inserting Eqs. (61), (62), and (63) into (57), (58) yields

*C*(Fig. 3) in the form:

*A*and

*B*can be approximated as follows (see Eq. (110) in the Appendix):

The relations (66)–(72) are of the same structure as Eqs. (11)–(17). The matrix coefficients \(\mathbf {M}_{ij}^{A}\), \(\mathbf {M}_{ij}^{B}\) for *i*,*j*=1,2 are computed earlier in the process of projections in positions, whereas vector quantities \(\mathbf {Q}_{i}^{A}\), \(\mathbf {Q}_{i}^{B}\) for *i*=1,2 are evaluated in each iteration as a function of corrected velocities. The projections in velocities are continued until the specified convergence conditions are met, e.g., \(\|\dot{\mathbf {q}}^{(i+1)}-\dot{\mathbf {q}}^{(i)}\|<\epsilon_{v}\), where *ϵ*_{v} is the user specified tolerance.

The mass-orthogonal projections in positions and velocities can be performed at each time step to obtain a set of values, which satisfy constraints equations within the user specified tolerance. Afterward, the corrected state variables are used to evaluate accelerations according to the divide and conquer scheme described in Sect. 2. The matrix coefficients **M**_{ij} for *i*,*j*=1,2 from Eqs. (27)–(30) do not need to be reevaluated at this computational stage, because they are computed earlier in the process of position projections. The vector quantities **Q**_{i} for *i*=1,2 from Eqs. (31), (32) are computed as a function of corrected state variables. If the projections are performed at each time step, the choice of parameters **Ω** and μ is irrelevant. They may be set to zero, because the position and velocity constraint equations are satisfied after the projections to the constraint manifold.

## 4 Numerical examples

### 4.1 Introduction

This section presents some results of the numerical experiments, which are performed to prove the correctness of the developed formulations as well as indicate some of its features. All of the test cases are simulated with intent of gathering characteristics of the parallel divide and conquer algorithm without and with mass-orthogonal projections. The comparisons are made with respect to accuracy and robustness of the formulations. All of the test cases are systems with closed kinematic loops and they are modeled as spatial mechanisms with redundant constraints, which may undergo singular configurations. Apart from the numerical issues, the ways of modeling MBS with closed kinematic chains are demonstrated and generalized.

The test cases are implemented in Matlab in double precision arithmetic, with standard *ode45* integration procedure [40]. The absolute and relative tolerances are set to 10^{−6}. All bodies are modeled as rigid interconnected via revolute joints only. The characteristic length of each body in the system is *a*=0.2 m, with masses *m*_{i}=1 kg and inertia matrices equal to \(\mathbf{J}'_{i}=\operatorname{diag}(0.25)~\mathrm{kg\,m}^{2}\) with respect to the local reference frames (*x*_{i},*y*_{i},*z*_{i}) with the origin located at the centers of masses of bodies. It is assumed that the penalty factor is set to \(\boldsymbol {\upalpha}=\operatorname{diag}(10^{6})\), whereas other matrix coefficients, if needed, are \(\boldsymbol {\upmu}=\operatorname{diag}(1.0)\), \(\boldsymbol {\Omega}=\operatorname{diag}(1.0)\). The maximum number of iterations is limited to 10 at position, velocity and acceleration level, and simultaneously user specified tolerances are chosen to be *ϵ*_{p}=10^{−14}, *ϵ*_{v}=10^{−12} and *ϵ*_{a}=10^{−10}, respectively. For each test multibody systems 10 second simulations are performed, with the mechanisms released from the initial state shown in figures under gravity forces.

### 4.2 Single kinematic loop

*w*=6⋅4−5⋅5=−1, whereas in fact the system possess two degrees of freedom. Thus, three redundant constraint are imposed on the system. This situation implicates the permanent row rank deficiency of the Jacobian matrix. Moreover, the five-bar mechanism may undergo singular configurations as shown in Fig. 4, at that time the Jacobian matrix temporarily loses its current rank.

**λ**_{1}and

**λ**_{5}correspond to constraint forces in first and fifth joint. These quantities can be approximated as follows:

It should be emphasized that the matrix coefficients in Eq. (77) are symmetric and positive definite even in the case, when Jacobian matrices are rank deficient (permanently or temporarily). In addition, there is a relation \(\mathbf {M}_{12}^{1-4}=(\mathbf {M}_{21}^{1-4})^{T}\), which can be used for improving computational efficiency. After evaluating \(\ddot{\mathbf {q}}_{1}\), \(\ddot{\mathbf {q}}_{4}\) and Lagrange multipliers **λ**_{1}, **λ**_{5}, the disassembly phase is started to compute all of bodies’ accelerations and constraint forces in joints.

On the basis of obtained results, it is can be seen that the proposed algorithms behave well for dynamics simulation of closed loop multibody systems with redundant constraints, which may repeatedly pass through singular configurations. Violations in constraint equations are reduced considerably. The formulations demonstrate advantages over traditional methods for constraint imposition based on Lagrange multipliers approach that may fail completely in considered test cases.

### 4.3 Coupled kinematic loops

*w*=6⋅8−5⋅10=−2. In fact the system possesses four degrees of freedom, i.e., six redundant constraints are imposed on the system.

**λ**_{6},

**λ**_{10}with accelerations of body 3. Let us consider equations of motion for body 3:

It should be emphasized that Eq. (86) expresses dynamics of body 3 as well as motion of lower kinematic loop. The operation of reduction of lower kinematic loop enables to treat upper loop as single closed kinematic chain. Therefore, it is possible to apply the similar formulation as described in Sect. 4.2 to that case.

### 4.4 Discussion

The constraint equations for multibody systems considered in the paper are imposed at the acceleration level. The solution of mixed differential equations of motion together with such algebraic relations suffers from accumulation of constraint errors. Constraint imposition at the acceleration level may lead to substantial violation of the position and velocity constraint equations and, in consequence, unacceptable results for a given simulation. The problem may be solved by introducing constraint stabilization techniques. Special formulations have to be taken into account when dealing with redundant constraints in multibody systems and some peculiarities of the methodologies should be concerned in modeling of mechanisms passing through singular configurations.

The first variant of the formulation presented in the paper combines the divide and conquer algorithm with the augmented Lagrangian method. Lagrange multipliers are approximated by the weighted sum of constraint equations and its two derivatives. The algorithm is robust in the case of modeling systems with redundant constraints, and may handle singular configurations. The second variant maintains all the advantages of the former method and yields better constraints fulfillment by applying mass-orthogonal projections to the constraint manifold at position and velocity level. This algorithm appears to be much more accurate than the regular DCA with augmented Lagrangian method but at the expense of computational burden associated with error corrections.

In the context of DCA based methods, the singularities may be overcome by different approaches. Featherstone in his original work [16] includes the capability of dealing with closed loop systems. For the most general case, the kinematic loops are treated by using generalized inverses and the loop constraint equations are made stable with the add of well-known Baumgarte technique [8]. On the other hand, Mukherjee and Anderson [32] developed a noniterative divide and conquer algorithm for forward dynamics of MBS with single and coupled loops that incorporates orthogonal complements of the joint motion subspace. This method indicates well constraint satisfaction without necessity of applying additional constraint stabilization techniques. In the paper, the singular configurations are overcome by introducing the approximation of Lagrange multipliers based on the augmented Lagrangian method. Due to that fact, the key matrices in the algorithms are nonsingular even when Jacobian matrices lose permanently or exceptionally their rank. The proposed algorithms are inherently iterative. Therefore, some additional computational burden is expected for the first and second variant of the methodology as compared to mentioned counterparts.

The main computational load for the DCA algorithm combined with the augmented Lagrangian formulation is associated with the first iteration. The process of assembly requires the calculation of coefficients according to Eqs. (27)–(32). Taking into account the boundary conditions presented in Sect. 2.3, the disassembly phase is started by exploiting Eqs. (21), (23), and (17). The computational burden is significantly reduced each next iteration, because the matrices **M**_{ij} for *i*,*j*=1,2 are constant in the iterative process. Vectors **Q**_{i} for *i*=1,2 and accelerations are the only quantities that should be reevaluated each iteration. Looking carefully at the recursive formulae, one can find that the efficiency can be further improved by computing only these values, which are dependent on the Lagrange multipliers and accelerations from the previous iteration. This remark allows to avoid expensive matrix multiplications and inversions and, in consequence, diminish the overall numerical cost of the formulation to the extent possible.

The additional computational burden is expected for the second formulation proposed in this work. The added load is associated with the mass-orthogonal projections to the constraint manifold. Similar conclusions may be drawn for the error correction stage at position and velocity level compared to the regular algorithm. The matrix coefficients in the form of **M**_{ij} for *i*,*j*=1,2 are evaluated at the beginning of the projection at the position level. During the iterative process, they are constant, and then are once reevaluated before the start of the velocity projection scheme. It should be noted that these values remain also constant at the velocity and acceleration level. Vectors **Q**_{i} for *i*=1,2 and demanded unknowns (positions, velocities, or accelerations) are the only quantities that are altered each iteration. The experience of the authors demonstrated that the penalty factors ranging from 10^{5} to 10^{7} give a good convergence rate at each level of computations. The number of iterations is dependent on the user specified tolerances but usually at most 4 iterations are sufficient to obtain reasonable solutions for considered mechanisms.

In conclusion, the computational complexity of both variants considered in the work can be approximated as *O*(*kn*) sequentially and becomes \(O(k\operatorname{log}_{2}n)\) for parallel implementations on *O*(*n*) processors, where *k* is the number of iterations. A more detailed and quantitative discussion is needed to estimate the numerical cost associated with the proposed algorithms compared to other approaches available in the literature. These issues are areas of ongoing work of the authors and is planned to be addressed in a forthcoming publications.

## 5 Conclusions

In this paper, new formulations for application to general multibody system dynamics have been developed, verified, and compared. Obtained iterative algorithms are linear, when considered sequentially and achieve logarithmic complexity for parallel computing. Developed methods treat tree topology systems, and general systems which contain kinematic closed loops in a uniform manner and can handle situations, when constraint Jacobian matrices lose its rank. The methods based on augmented Lagrangian formulations with projections ensure appropriate numerical properties. Numerical results of the test cases indicate good accuracy performance dependent on the expense put in the iterative refinement of constraint equations. The formulations are robust in case of analyzing systems with redundant constraints, and which may enter singular configurations. The developed formulations can be extended for analyzing dynamics of complex systems with unilateral constraints, which are areas of forthcoming research for the authors.

## Acknowledgements

This work has been supported by the European Union in the framework of European Social Fund through the Warsaw University of Technology Development Programme, realized by Center for Advanced Studies.

### Open Access

This article is distributed under the terms of the Creative Commons Attribution License which permits any use, distribution, and reproduction in any medium, provided the original author(s) and the source are credited.