Contact Dynamics Formulation Using Minimal Coordinates

Abstract

In recent years, complementarity techniques have been developed for solving non-smooth multibody dynamics involving contact and collision events. The linear complementarity approach sets up a linear complementarity problem (LCP) using non-minimal coordinates for the unilateral contact constraints and inter-link bilateral constraints on the system. In this chapter, we develop a complementarity formulation that uses minimal coordinates. This results in a much smaller LCP whose size is independent of the number of bodies and the number of degrees of freedom in the system. Furthermore, we exploit operational space low-order algorithms to overcome key computational bottlenecks to obtain over an order of magnitude speed up in the solution procedure.

Appendix

The operational space for the multi-link system is defined by the configuration of the set of constraint nodes on the system. The key implementation and computational challenge for setting up the OS formulation LCP in Eq. 5.53 is the need for evaluating the ${{\underline{{{ \Lambda }}}}}$ matrix. As seen in Eq. 5.51, ${{\underline{{{ \Lambda }}}}}$ involves the configuration dependent matrix products of the Jacobian matrix and the mass matrix inverse. A direct evaluation of this expression requires $O({{{\mathcal{N}}}}^3)$ computations. However references [12, 16, 23] have used spatial operators to develop simpler and recursive computational algorithms for ${{\underline{{{ \Lambda }}}}}$ that are of only $O({{{\mathcal{N}}}})$ complexity. We briefly describe the underlying analysis and structure of this algorithm, and refer the reader to [12, 16, 23] for notation and derivation details.

5.1.1 Spatial Operator Factorization of ${{\mathcal{M}^{-1}}}$

We begin with the following key spatial operator based analytical results that provide explicit, closed-form expressions for the factorization and inversion of a tree mass matrix [12, 24]:

$$\begin{aligned} {{\mathcal{M}}}&= {{H}}{{\upphi }} {{{\mathcal {M}}}}{{{\upphi }^{*}}}{{H^{*}}}\nonumber \\ {{\mathcal{M}}}&= \left[ {{I}}+ {{H}}{{\upphi }} {{ \mathcal{K}}}\right] {{\mathcal{D}}} \left[ {{I}}+ {{H}}{{\upphi }} {{ \mathcal{K}}}\right] ^* \nonumber \\ \left[ {{I}}+ {{H}}{{\upphi }} {{ \mathcal{K}}}\right] ^{-1}&= \left[ {{I}}- {{H}}{{\uppsi }} {{ \mathcal{K}}}\right] \\ {{\mathcal{M}}}^{-1}&= \left[ {{I}}- {{H}}{{\uppsi }} {{ \mathcal{K}}}\right] ^* {{\mathcal{D}^{-1}}}\left[ {{I}}- {{H}}{{\uppsi }} {{ \mathcal{K}}}\right] \nonumber \end{aligned}$$

(5.59)

The first expression defines the Newton-Euler operator factorization of the mass matrix ${{\mathcal{M}}}$ in terms of the ${{H}}$ hinge articulation, the ${{\upphi }}$ rigid body propagation and the ${{{\mathcal {M}}}}$ link spatial inertia operators. While this factorization has non-square factors, the second expression describes an alternative factorization involving only square factors with block diagonal ${{\mathcal{D}}}$ and block lower-triangular $[{{I}}+ {{H}}{{\upphi }} {{ \mathcal{K}}}]$ matrices. This factorization involves new spatial operators that are associated with the articulated body (AB) forward dynamics algorithm [11, 23] for the system. The next expression describes an analytical expression for the inverse of the $[{{I}}+ {{H}}{{\upphi }} {{ \mathcal{K}}}]$ operator. Using this leads to the final analytical expression for the inverse of the mass matrix. These operator expressions hold generally for tree-topology systems irrespective of the number of bodies, the types of hinges, the specific topological structure, and even for non-rigid links [12].

5.1.2 The ${{ {\Omega }}}$ Extended Operational Space Compliance Matrix

With ${{\mathcal{V}}}\in {{\mathcal R}}^{6{{n}}}$ denoting the stacked vector of link spatial velocities, its spatial operator expression is [12]

$$\begin{aligned} {{\mathcal{V}}}= {{{{{\upphi }^{*}}}{{H^{*}}}}}{{{\dot{{{\uptheta }}}}}} \end{aligned}$$

(5.60)

Bundling together the rigid body transformations for all nodes we define the ${{\mathcal{B}}}\in {{\mathcal R}}^{{6{{n}}}\times {6{{n_{{c}}}}}}$ pick-off matrix such that the stacked vector of node spatial velocities ${{{{\mathcal{V}}}_{{c}}}}$ can be expressed as

$$\begin{aligned} {{{{\mathcal{V}}}_{{c}}}}= {{\mathcal{B}}}^*{{\mathcal{V}}}\mathop {\ \ =\ \ }\limits ^{{5.60}} {{\mathcal{B}}}^*{{{{{\upphi }^{*}}}{{H^{*}}}}}{{{\dot{{{\uptheta }}}}}}\quad \Rightarrow \quad {{\mathcal{J}}}\mathop {\ \ =\ \ }\limits ^{{5.49}} {{\mathcal{B}}}^*{{{{{\upphi }^{*}}}{{H^{*}}}}} \end{aligned}$$

(5.61)

This is the spatial operator expression for the ${{\mathcal{J}}}$ Jacobian matrix. Using this expression and Eq. 5.59 for the mass matrix inverse within Eq. 5.51 leads to the following expression for ${{\underline{{{ \Lambda }}}}}$:

$$\begin{aligned} {{\underline{{{ \Lambda }}}}}\mathop {\ \ =\ \ }\limits ^{{5.51}} {{\mathcal{J}}}{{\mathcal{M}^{-1}}}{{\mathcal{J}^*}}\mathop {\ \ =\ \ }\limits ^{{5.59}} {{\mathcal{B}}}^* {{{\upphi }^{*}}}{{H^{*}}}({{I}}- {{H}}{{\uppsi }} {{ \mathcal{K}}})^* {{\mathcal{D}^{-1}}}({{I}}- {{H}}{{\uppsi }} {{ \mathcal{K}}}) {{H}}{{\upphi }}{{\mathcal{B}}} \end{aligned}$$

(5.62)

Using the spatial operator identity [12, 24]

$$\begin{aligned} ({{I}}- {{H}}{{\uppsi }} {{ \mathcal{K}}}) {{H}}{{\upphi }}= {{H}}{{\uppsi }} \end{aligned}$$

(5.63)

in Eq. 5.62 leads to the following simpler expression for ${{\underline{{{ \Lambda }}}}}$: Extended operational space compliance matrix

$$\begin{aligned} {{\underline{{{ \Lambda }}}}}= {{\mathcal{B}}}^*{{ {\Omega }}}{{\mathcal{B}}}\ \text {with}\ {{ {\Omega }}} \, \, \overset{\triangle }{ = } \, \, {{{{\uppsi }}^*}}{{H^{*}}}{{\mathcal{D}^{-1}}}{{H}}{{\uppsi }}\ \in {{\mathcal R}}^{{6{{n_{{c}}}}}\times {6{{n_{{c}}}}}} \end{aligned}$$

(5.64)

We have arrived at an expression for ${{\underline{{{ \Lambda }}}}}$, that unlike Eq. 5.51, involves neither the mass matrix inverse nor the node’s Jacobian matrix! We refer to ${{ {\Omega }}}$ as the Extended Operational Space Compliance Matrix. This terminology is based on Eq. 5.64 which shows that the OSCM, ${{\underline{{{ \Lambda }}}}}$ can be obtained by a reducing transformation of the full, all body ${{ {\Omega }}}$ matrix by the ${{\mathcal{B}}}$ pick-off operator involving just the matrix sub-blocks associated with the parent links of the nodes. From its definition, it is clear that ${{ {\Omega }}}$ is a symmetric and positive semi-definite since ${{\mathcal{D}^{-1}}}$ is a symmetric positive-definite matrix.

While the explicit computation of ${{\mathcal{M}^{-1}}}$ or ${{\mathcal{J}}}$ is not needed to obtain ${{\underline{{{ \Lambda }}}}}$, the direct evaluation of Eq. 5.64 still remains of $O({{{\mathcal{N}}}}^3)$ complexity due to the need for carrying out the multiple matrix/matrix products. The next section shows that these matrix/matrix products can be avoided by exploiting a decomposition of the ${{ {\Omega }}}$ matrix.

5.1.3 Decomposition of ${{ {\Omega }}}$

The following lemma describes a decomposition of ${{ {\Omega }}}$ into simpler component terms and an expression for its block elements. The ${{{\mathcal{E}}_{{\uppsi }}^*}}$ and ${{\uppsi }}()$ terms used below are defined in references [12, 23]. Furthermore, ${{\wp }}(k)$ denotes the parent link for the $k{\text {th}}$ link, and $i \prec j$ notation implies that the $j^{th}$ link is an ancestor of the $i{\text {th}}$ link in the tree.

Lemma 1

(Decomposition of ${{ {\Omega }}}$) ${{ {\Omega }}}$ can be decomposed into the following disjoint sum:

Operator decomposition!of ${{ {\Omega }}}$ Extended operational space compliance matrix!operator decomposition Backward Lyapunov equation

$$\begin{aligned} {{ {\Omega }}}= {{\Upsilon }}+ {{\tilde{{\uppsi }}}}^* {{\Upsilon }}+ {{\Upsilon }}{{\tilde{{\uppsi }}}}+ R \quad \text {where}\quad R \, \, \overset{\triangle }{ = } \, \, \sum _{\begin{array}{c} \forall i,j:\ i \nprec \nsucc j \\ \scriptstyle k = {{\wp }}( i, j) \end{array} } {{\mathrm {e}_{i}}} {{{{\uppsi }}^*}}(k, i)Y(k) {{\uppsi }}(k, j){{\mathrm {e}^*_{j}}} \end{aligned}$$

(5.65)

${{\Upsilon }}\in {{\mathcal R}}^{{6{{n_{{c}}}}}\times {6{{n_{{c}}}}}}$ is a block-diagonal operator, referred to as the operational space compliance kernel, satisfying the following backward Lyapunov equation: Operational space compliance kernel

$$\begin{aligned} {{H^{*}}}{{\mathcal{D}^{-1}}}{{H}}= {{\Upsilon }}- \mathrm{diagOf }\Big \{ {{{\mathcal{E}}_{{\uppsi }}^*}}{{\Upsilon }}{{{\mathcal{E}}_{{\uppsi }}}} \Big \} \end{aligned}$$

(5.66)

$\mathrm{diagOf }\Big \{ {{{\mathcal{E}}_{{\uppsi }}^*}}{{\Upsilon }}{{{\mathcal{E}}_{{\uppsi }}}} \Big \}$ represents just the block-diagonal part of the (generally non block-diagonal) ${{{\mathcal{E}}_{{\uppsi }}^*}}{{\Upsilon }}{{{\mathcal{E}}_{{\uppsi }}}}$ matrix. The $6\times 6$ dimensional, symmetric, positive semi-definite ${{\Upsilon }}(k)$ diagonal matrices satisfy the following parent/child recursive relationship:

$$\begin{aligned} {{\Upsilon }}{{(k)}}= {{{{\uppsi }}^*}}({{\wp }}(k), k) {{\Upsilon }}({{\wp }}(k)){{\uppsi }}({{\wp }}(k),k) + {{H^{*}}}{{(k)}}{{\mathcal{D}^{-1}}}{{(k)}}{{H}}{{(k)}} \end{aligned}$$

(5.67)

This relationship forms the basis for the following $O({{{\mathcal{N}}}})$ base-to-tips scatter recursion for computing the ${{\Upsilon }}(k)$ diagonal elements:

(5.68)

While ${{\Upsilon }}$ defines the block-diagonal elements of ${{ {\Omega }}}$, the following recursive expressions describe its off-diagonal terms:

$$\begin{aligned} {{ {\Omega }}}(i, j) = \left\{ \begin{array}{llccc} {{\Upsilon }}(i) &{} \mathrm{f or\ }\; i = j \\ {{ {\Omega }}}(i, k) {{\uppsi }}(k, j)\ &{} \mathrm{f or\ }\; i \succeq k \succ j, \quad k = {{\wp }}(j) \\ {{ {\Omega }}}^*(j,i) &{} \mathrm{f or\ }\; i \prec j \\ {{ {\Omega }}}(i, k) {{\uppsi }}(k, j) &{} \mathrm{f or\ }\; i \nsucc j, \ j \nsucc i, \ k ={{\wp }}(i, j) \end{array} \right. \end{aligned}$$

(5.69)

Proof

See [12, 23].

Equation 5.65 shows that ${{ {\Omega }}}$ can be decomposed into the sum of simpler terms consisting of the block diagonal ${{\Upsilon }}$, the upper-triangular ${{\tilde{{\uppsi }}}}^* {{\Upsilon }}$, the lower triangular ${{\Upsilon }}{{\tilde{{\uppsi }}}}$, and the sparse $R$ matrices. Furthermore, Eq. 5.69 reveals that all of the block-elements of ${{ {\Omega }}}(i,j)$ can be obtained from the ${{\Upsilon }}(i)$ elements of the ${{\Upsilon }}$ block-diagonal operational space compliance kernel.

From the ${{\underline{{{ \Lambda }}}}}= {{\mathcal{B}}}^*{{ {\Omega }}}{{\mathcal{B}}}$ expression, and the sparse structure of ${{\mathcal{B}}}$, it is clear that only a subset of the elements of ${{ {\Omega }}}$ are needed to compute ${{\underline{{{ \Lambda }}}}}$. The ${{\mathcal{B}}}$ pick-off operator has one column for each of the nodes, with each such column having only a single non-zero $6\times 6$ matrix entry at the $k{\text {th}}$ parent link slot. Only as many elements of ${{ {\Omega }}}$ as there are elements in ${{\underline{{{ \Lambda }}}}}$ are needed. Thus, just ${{n_{{c}}}}\times {{n_{{c}}}}$ number of $6\times 6$ sub-block matrices of ${{ {\Omega }}}$ are required. In view of the symmetry of the matrices, we actually need just ${{n_{{c}}}}({{n_{{c}}}}+ 1)/2$ such sub-block matrices. The overall complexity of this algorithm is linearly proportional to the number of degrees of freedom, and a quadratic function of the number of nodes. This is much lower than the $O({{{\mathcal{N}}}}^3)$ complexity implied by Eq. 5.51.Algorithm for computing!operational space!inertia Operational space inertia!algorithm for computing