# A new factorization of the mass matrix for optimal serial and parallel calculation of multibody dynamics

## Abstract

This paper describes a new factorization of the inverse of the joint-space inertia matrix * M*. In this factorization,

**M**^{−1}is directly obtained as the product of a set of sparse matrices wherein, for a serial chain, only the inversion of a block-tridiagonal matrix is needed. In other words, this factorization reduces the inversion of a dense matrix to that of a block-tridiagonal one. As a result, this factorization leads to both an optimal serial and an optimal parallel algorithm, that is, a serial algorithm with a complexity of

*O*(

*N*) and a parallel algorithm with a time complexity of

*O*(log

*N*) on a computer with

*O*(

*N*) processors. The novel feature of this algorithm is that it first calculates the interbody forces. Once these forces are known, the accelerations are easily calculated. We discuss the extension of the algorithm to the task of calculating the forward dynamics of a kinematic tree consisting of a single main chain plus any number of short side branches. We also show that this new factorization of

**M**^{−1}leads to a new factorization of the operational-space inverse inertia,

**Λ**^{−1}, in the form of a product involving sparse matrices. We show that this factorization can be exploited for optimal serial and parallel computation of

**Λ**^{−1}, that is, a serial algorithm with a complexity of

*O*(

*N*) and a parallel algorithm with a time complexity of

*O*(log

*N*) on a computer with

*O*(

*N*) processors.

## Keywords

Multibody dynamics, Robot dynamics, Mass matrix factorization, Parallel computation## Nomenclature

*n*Total number of Degrees of Freedom (DOF) of the system

*n*_{i}DOF of joint

*i**N*Number of bodies in the system

*m*,*a*Cost of multiplication and addition

*O*_{i}Reference point fixed in body

*i***P**_{i,j}Matrix to shift reference point from

*O*_{ j }to*O*_{ i }**P**_{i}Shorthand for

**P**_{ i+1,i }- \(\boldsymbol {\mathcal {P}}\in\Re^{6N\times6N}\)
Global mapping from joint forces to body forces, or from body accelerations to joint accelerations (\(\boldsymbol {\mathcal {P}}^{\mathrm {T}}\))

**Q**_{i}3×3 rotational coordinate transform matrix from the frame located at

*O*_{ i+1}to the frame located at*O*_{ i }- Open image in new window
6×6 rotational coordinate transform constructed from

**Q**_{ i }*m*_{i}Mass of body

*i***s**_{i}Location of center of mass of body

*i*relative to*O*_{ i }**J**_{i}Second moment of mass of body

*i*about its center of mass**I**_{i}∈ℜ^{6×6}Spatial inertia of body

*i*referred to point*O*_{ i }- Open image in new window
Global matrix of body inertias (\(\boldsymbol {\mathcal {I}}\in\Re^{6N\times6N}\))

∈ℜ**M**^{n×n}Joint-space inertia matrix

- \(\ddot {\boldsymbol {q}}_{i} \in \Re^{n_{i}}\)
Acceleration variables of joint

*i*- \(\ddot {\boldsymbol {q}}= \mathrm {col}\{\ddot {\boldsymbol {q}}_{i}\} \in \Re^{n}\)
Global vector of joint acceleration variables

- \(\boldsymbol {\tau }_{i} \in \Re^{n_{i}}\)
Force variables of joint

*i*=col{**τ****τ**_{i}}∈ℜ^{n}Global vector of joint force variables

- \(\boldsymbol {\omega }_{i}, \dot{\boldsymbol {\omega }}_{i} \in \Re^{3}\)
Angular velocity and acceleration of body

*i*- \(\boldsymbol {v}_{i}, \dot{\boldsymbol {v}}_{i} \in \Re^{3}\)
Linear velocity and acceleration of body

*i*(point*O*_{ i })- Open image in new window
Spatial acceleration of body

*i*(assuming zero velocity)- \(\dot {\boldsymbol {\mathcal {V}}}= \mathrm {col}\{ \dot {\boldsymbol {V}}_{\!\!i}\}\in \Re^{6N}\)
Global vector of body accelerations

**f**_{i}∈ℜ^{3}Force applied from body

*i*−1 to body*i*at*O*_{ i }**n**_{i}∈ℜ^{3}Moment applied from body

*i*−1 to body*i*about*O*_{ i }- Open image in new window
Spatial force transmitted from body

*i*−1 to body*i*- \(\boldsymbol {\mathcal {F}}= \mathrm {col}\{ \boldsymbol {F}_{\!i}\} \in \Re^{6N}\)
Global vector of interbody forces

- Open image in new window
Matrix describing the motion freedom of joint

*i*(maps \(\ddot {\boldsymbol {q}}_{i}\) to spatial acceleration of joint*i*, assuming zero velocity)- Open image in new window
Global matrix of joint motion freedoms (\(\boldsymbol {\mathcal {H}}\in \Re^{6N \times n}\))

- \(\boldsymbol {W}_{\!\!i} \in \Re^{6 \times (6-n_{i})}\)
Matrix describing the constraint force space of joint

*i*- Open image in new window
Matrix that maps

**τ**_{ i }to a spatial force at joint*i*- \(\boldsymbol {\mathcal {W}}= \mathrm {diag}\{ \boldsymbol {W}_{\!\!i}\}\)
Global constraint force matrix (\(\boldsymbol {\mathcal {W}}\in \Re^{6N \times (6N-n)}\))

- Open image in new window
Global mapping from

to joint spatial forces (\(\boldsymbol {\mathcal {W}}_{\!\rm a}\in \Re^{6N \times n}\))**τ**

## Notes

### Acknowledgements

The first author would like to dedicate this paper to the memory of his dear friend, Ms. Bahareh Butterfly, for her courage, passion, and love.

## References

- 1.Anderson, K.S., Duan, S.: Highly parallelizable low order dynamics algorithm for complex multi-rigid-body systems. AIAA J. Guidance Control Dyn.
**23**(2), 355–364 (2000) CrossRefGoogle Scholar - 2.Featherstone, R.: Robot Dynamics Algorithms. Kluwer, Boston (1987) Google Scholar
- 3.Featherstone, R.: A divide-and-conquer articulated-body algorithm for parallel
*O*(log(*n*)) calculation of rigid-body dynamics. Part 1: Basic algorithm. Int. J. Robot. Res.**18**(9), 867–875 (1999) CrossRefGoogle Scholar - 4.Featherstone, R.: A divide-and-conquer articulated-body algorithm for parallel
*O*(log(*n*)) calculation of rigid-body dynamics. Part 2: Trees, loops and accuracy. Int. J. Robot. Res.**18**(9), 876–892 (1999) CrossRefGoogle Scholar - 5.Featherstone, R.: Rigid Body Dynamics Algorithms. Springer, New York (2008) MATHCrossRefGoogle Scholar
- 6.Featherstone, R., Fijany, A.: A technique for analyzing constrained rigid-body systems, and its application to the constraint force algorithm. IEEE Trans. Robot. Autom.
**15**(6), 1140–1144 (1999) CrossRefGoogle Scholar - 7.Featherstone, R., Orin, D.E.: Robot dynamics: equations and algorithms. In: Proc. IEEE Int. Conf. Robotics & Automation, San Francisco, CA, April 2000, pp. 826–834 (2000) Google Scholar
- 8.Featherstone, R., Orin, D.E.: Dynamics. In: Siciliano, B., Khatib, O. (eds.) Springer Handbook of Robotics, pp. 35–65. Springer, Berlin (2008) CrossRefGoogle Scholar
- 9.Fijany, A.: Schur complement factorizations and parallel
*O*(log*n*) algorithms for computation of operational space mass matrix and its inverse. In: Proc. IEEE Int. Conf. on Robotics & Automation, San Diego, May 1994, pp. 2369–2376 (1994) Google Scholar - 10.Fijany, A., Sharf, I., D’Eleuterio, G.M.T.: Parallel
*O*(log*N*) algorithms for computation of manipulator forward dynamics. IEEE Trans. Robot. Autom.**11**(3), 389–400 (1995) CrossRefGoogle Scholar - 11.Golub, G.H., Van Loan, C.F.: Matrix Computations, 2nd edn. The Johns Hopkins Univ. Press, Baltimore (1989) MATHGoogle Scholar
- 12.Hockney, R.W., Jesshope, C.R.: Parallel Computers. Adam Hilger, London (1981) MATHGoogle Scholar
- 13.Khatib, O.: A unified approach to motion and force control of robot manipulators: the operational space formulation. IEEE J. Robot. Autom.
**3**(1), 43–53 (1987) CrossRefGoogle Scholar - 14.Khatib, O.: Inertial properties in robotic manipulation: an object-level framework. Int. J. Robot. Res.
**14**(1), 19–36 (1995) CrossRefGoogle Scholar - 15.Lathrop, R.H.: Parallelism in manipulator dynamics. Int. J. Robot. Res.
**4**(2), 80–102 (1985) CrossRefGoogle Scholar - 16.Lee, C.S.G., Chang, P.R.: Efficient parallel algorithms for robot inverse dynamics computation. IEEE Trans. Syst. Man Cybern.
**16**(4), 532–542 (1986) CrossRefGoogle Scholar - 17.Mukherjee, R.M., Anderson, K.S.: A logarithmic complexity divide-and-conquer algorithm for multi-flexible articulated body systems. J. Comput. Nonlinear Dyn.
**2**(1), 10–21 (2007) MathSciNetCrossRefGoogle Scholar - 18.Redon, S., Galoppo, N., Lin, M.C.: Adaptive dynamics of articulated bodies. ACM Trans. Graph.
**24**(3), 936–945 (2005) (SIGGRAPH 2005) CrossRefGoogle Scholar - 19.Rodriguez, G., Jain, A., Kreutz-Delgado, K.: A spatial operator algebra for manipulator modelling and control. Int. J. Robot. Res.
**10**(4), 371–381 (1991) CrossRefGoogle Scholar - 20.Rodriguez, G., Kreutz-Delgado, K.: Spatial operator factorization and inversion of the manipulator mass matrix. IEEE Trans. Robot. Autom.
**8**(1), 65–76 (1992) MathSciNetCrossRefGoogle Scholar - 21.Walker, M.W., Orin, D.E.: Efficient dynamic computer simulation of robotic mechanisms. J. Dyn. Syst. Meas. Control
**104**, 205–211 (1982) MATHCrossRefGoogle Scholar - 22.Wehage, R.A.: Solution of multibody dynamics using natural factors and iterative refinement—Part I: Open kinematic loops. In: Proc. Advances in Design Automation, Montreal, Quebec, Canada, Sept. 1989. ASME, New York, ASME Publication DE 19-3, pp. 125–132 (1989) Google Scholar
- 23.Yamane, K., Nakamura, Y.: Comparative study on serial and parallel forward dynamics algorithms for kinematic chains. Int. J. Robot. Res.
**28**(5), 622–629 (2009) CrossRefGoogle Scholar