Multibody Graph Transformations and Analysis Part II: Closed-chain constraint embedding

Abstract

This is the second part of a two-part paper that develops graph theoretic techniques for the topological transformation and analysis of multibody system dynamics. The first part focused on tree systems, and developed systematic and rigorous techniques for the partitioning, aggregation and sub-structuring of multibody dynamics models. This second part, uses the aggregation techniques as the foundation to develop the constraint-embedding technique that enables the transformation of the non-tree system graphs into tree graphs. This enables the application of a large family of analytical and computational techniques for trees to closed-chain systems. This is illustrated through an extension of the low-order articulated-body forward dynamics algorithm for tree systems to closed-chain systems.

1 Introduction

The notion of SKO models have been shown to provide a general way of deriving a broad range of analytical and algorithmic techniques for tree multibody systems, while requiring few assumptions on the underlying low-level properties of the system [7, 8, 10, 11]. This remarkable consistency persists despite significant differences in the component structure of the corresponding spatial operators across the systems. The development of SKO models is based on connections between graph theory ideas and the structural properties of tree multibody system dynamics models. The first of this two-part paper [9], further extends the SKO model ideas to develop topological transformation techniques for the partitioning, aggregation and sub-structuring of SKO models.

This paper builds upon the sub-graph aggregation ideas to extend the notion of SKO models to non-tree, closed-chain systems. The benefit of this extension is that the large family of SKO model based analysis and computational techniques for tree-systems become readily applicable to closed-chain systems. Further it allows the closed-chain dynamics formulation to be based on minimal coordinates and ordinary differential equations and avoids the need for over-parametrized differential algebraic equation (DAE) formulations [4, 15, 16]. The proposed technique provides the most computational benefit for closed-chain systems with topologies containing isolated, small to moderate sized loops. While the theory also applies to topologies with large loops and mesh like coupled loop structure, practical computational considerations limit the benefits for these cases. Our graph techniques based aggregation approach generalizes the forward dynamics constraint embedding technique [6] which in turn was inspired by the Recursive Coordinate Reduction (RCR) algorithm from [2].

The organization of this paper is as follows. We begin by reviewing the aggregation results in Section 2. Section 3 looks in detail at the body level recursive relationships in the aggregated system model. These set the stage for the development of the constraint embedding techniques in Section 4. Examples of the application of constraint embedding are described in Section 5. Section 6 provides an overview of the mass matrix factorization and inversion techniques and the resulting AB forward dynamics model for tree SKO models. The application of these analytical results and computational algorithms to the closed-chain SKO model is discussed in Section 7. Section 8 discusses details of computing time derivatives of quantities needed in the constraint embedding process, while Section 9 discusses generalizations of the constraint embedding technique.

2 Multibody system aggregation overview

We begin with a brief overview of the discussion from the first part of this paper [9] which studied the partitioning of tree multibody systems and derived expressions for the resulting partitioned structure of their SKO models.

An SKO model for an n-links tree-topology multibody system consists of the following:

1. A tree digraph reflecting the bodies and their connectivity in the system.

2. An ℰ SKO operator and associated SPO operator, $\mathbb{A}\triangleq {(\mathbf{I}-{ℰ}_{\mathbb{A}})}^{-1}$.

3. A full-rank block-diagonal, joint map matrix operator, H.

4. A block-diagonal and positive-definite spatial inertia operator, M.

5. Stacked vectors: θ̇ denoting independent generalized velocities, J the generalized forces, the body velocities, α the body accelerations, the inter-body forces, the body Coriolis accelerations, the body gyroscopic forces the system, the number of degrees of freedom, and the equations of motion defined as:

   $$\begin{array}{l}\mathcal{V}={\mathbb{A}}^{\ast }{\mathrm{H}}^{\ast }\dot{\mathrm{\theta }}\\ \alpha ={\mathbb{A}}^{\ast }\left({\mathrm{H}}^{\ast }\ddot{\mathrm{\theta }}+\mathfrak{a}\right)\\ \mathfrak{f}=\mathbb{A}(\mathbf{M}\alpha +\mathfrak{b})\\ \mathrm{J}=\mathrm{H}\mathfrak{f}\end{array}$$ (1)
   Thus,

   $$\mathrm{J}=\mathrm{ℳ}\left(\mathrm{\theta }\right)\ddot{\mathrm{\theta }}+\mathrm{C}\left(\mathrm{\theta },\dot{\mathrm{\theta }}\right)$$ (2)
   Where

   $$\mathrm{ℳ}\left(\mathrm{\theta }\right)\triangleq \mathrm{H}\mathbb{A}\mathbf{M}{\mathbb{A}}^{\ast }{\mathrm{H}}^{\ast }\in {ℛ}^{\mathcal{N}\times \mathcal{N}}\text{and}\mathrm{C}\left(\mathrm{\theta },\dot{\mathrm{\theta }}\right)\triangleq \mathrm{H}\mathbb{A}(\mathbf{M}{\mathbb{A}}^{\ast }\mathfrak{a}+\mathfrak{b})\in {ℛ}^{\mathcal{N}}$$ (3)

   ℳ is the symmetric and positive-definite mass matrix for the tree system and C is the vector of nonlinear Coriolis and gyroscopic velocity dependent terms. Eq. 3 is the Newton-Euler operator factorization of the mass matrix.

SKO models are also referred to by the (H, , M) triplet of operators that define them.

Reference [9] showed that a path-induced sub-graph¹ partitions the nodes in a tree digraph into a parent sub-graph, P, and child sub-graph, C, that are themselves path-induced. For tree multibody systems, the SKO model for the full system was shown to be expressible using the SKO models of the partitioned subsystems. The partitioning technique was used to develop techniques for sub-structuring a multi-body SKO model by aggregating sub-graphs into single variable-geometry bodies. This section summarizes the aggregation results which will be used to develop constraint embedding techniques in subsequent sections.

The key difference between the aggregated and the original tree is that the former treats the set of bodies in as a single body. The aggregation process provides a way of transforming and sub-structuring SKO models for tree-topology multibody systems into coarser SKO models. The aggregation process induces the following partitioning of the θ̇, , and J stacked vectors across the original, the parent and the child sub-graphs:

$$\dot{\mathrm{\theta }}=\left[\begin{array}{c}{\dot{\mathrm{\theta }}}_{\mathrm{C}}\\ {\dot{\mathrm{\theta }}}_{\mathfrak{S}}\\ {\dot{\mathrm{\theta }}}_{\mathrm{P}}\end{array}\right],\mathcal{V}=\left[\begin{array}{c}{\mathcal{V}}_{\mathrm{C}}\\ {\mathcal{V}}_{\mathfrak{S}}\\ {\mathcal{V}}_{\mathrm{P}}\end{array}\right],\mathfrak{f}=\left[\begin{array}{c}{\mathfrak{f}}_{\mathrm{C}}\\ {\mathfrak{f}}_{\mathfrak{S}}\\ {\mathfrak{f}}_{\mathrm{P}}\end{array}\right],\mathrm{J}=\left[\begin{array}{c}{\mathrm{J}}_{\mathrm{C}}\\ {\mathrm{J}}_{\mathfrak{S}}\\ {\mathrm{J}}_{\mathrm{P}}\end{array}\right]$$ (4)

For the aggregated tree, the , θ̇, etc., sub-vectors of the and θ̇ stacked vectors correspond to the single aggregate link. This partitioning of the system-level θ̇ and stacked vectors extends to other stacked vectors such as the Coriolis spatial accelerations vector , the gyroscopic spatial forces vector , and to the other spatial operators.

The following defines the aggregation condition for sub-graphs.

Assumption 1 (Aggregation Condition)

A sub-graph , of a tree digraph , is said to satisfy the aggregation condition if:

1. is an induced sub-graph, i.e., ≡ _I.

2. ℘( ) contains exactly one node — one that is necessarily the ancestor of all the nodes in .

Lemma 1 Tree property after sub-graph aggregation

Let denote an induced sub-graph of a tree digraph . Let us assume that is aggregated to create a new digraph with aggregated node . Then, the aggregated digraph is a tree if and only if satisfies the aggregation condition.

Proof See [9].

The aggregation sub-graph, for a sub-graph of a tree, is defined as the minimal sub-graph containing that also satisfies the aggregation condition. The aggregation sub-graph of a sub-graph is denoted _A.

Lemma 2 SKO operator for tree

With satisfying the aggregation condition, define the ℰ_a matrix as follows:

$${ℰ}_{{\mathbb{A}}_a}\triangleq \left(\begin{array}{ccc}{ℰ}_{{\mathbb{A}}_{\mathrm{C}}}& \mathbf{0}& \mathbf{0}\\ {ℬ}_{\mathfrak{S}}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathrm{E}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}& {ℰ}_{{\mathbb{A}}_{\mathrm{P}}}\end{array}\right)$$ (5)

where ℰ_C and ℰ_P denote SKO operators for the C and P sub-graphs, is the SPO operator for the sub-graph, and ℬ, E are connector blocks. The following facts hold for ℰ_a:

1. ℰ_a is an SKO operator for the original tree, .

2. ℰ_a is an SKO operator for the aggregated tree, .

Proof See [9].

Lemma 3 The _a SPO operator for the tree

Using the same assumptions and notation from Lemma 2, the SPO operator, _a, for is given by the following expression:

$$\begin{array}{c}{\mathbb{A}}_a=\left(\begin{array}{ccc}{\mathbb{A}}_{\mathrm{C}}& \mathbf{0}& \mathbf{0}\\ {\mathrm{ℬ}}_{\mathfrak{S}}{\mathbb{A}}_{\mathrm{C}}& \mathbf{I}& \mathbf{0}\\ {\mathbb{A}}_{\mathrm{P}}\left({\mathrm{E}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}{\mathrm{ℬ}}_{\mathfrak{S}}\right){\mathbb{A}}_{\mathrm{C}}& {\mathbb{A}}_{\mathrm{P}}{\mathrm{E}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}& {\mathbb{A}}_{\mathrm{P}}\end{array}\right)\\ \mathit{\text{where}}{\mathfrak{J}}_{\mathbb{A}}\triangleq \left(\begin{array}{ccc}\mathbf{I}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathbb{A}}_{\mathfrak{S}}^{-1}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{I}\end{array}\right)\end{array}$$ (6)

where _C and _P denote SPO operators for the C and P sub-graphs.

Proof See [9].

Next, lemma 4 shows that the transformed system with the aggregated tree pos­sesses a well-defined SKO model, and defines the equations of motion for the model.

Lemma 4 SKO model for an aggregated tree

Let (H, , M) denote an SKO model with tree digraph . Let be a sub-graph of satisfying the aggregation condition. Then the (H_a, _a, M) spatial operators define an SKO model for the aggregated tree, where

$${\mathrm{H}}_a\triangleq \mathrm{H}{\mathfrak{J}}_a^{-1}\stackrel{6}{=}\left(\begin{array}{ccc}{\mathrm{H}}_{\mathrm{C}}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathrm{H}}_{\mathrm{P}}\end{array}\right)\mathit{\text{and}}{\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}\triangleq {\mathrm{H}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}$$ (7)

H_a defines the new joint map matrix for the aggregated tree, with H_C, H, and H_P denoting the component joint map matrices. The transformed version of the equations of motion from Eq. 1 are given by:

$$\begin{array}{l}\mathcal{V}={\mathbb{A}}_a^{\ast }{\mathrm{H}}_a^{\ast }\dot{\mathrm{\theta }}\\ \alpha ={\mathbb{A}}_a^{\ast }({\mathrm{H}}_a^{\ast }\ddot{\mathrm{\theta }}+\underset{\_}{\mathfrak{a}}),\mathit{\text{where}}\underset{\_}{\mathfrak{a}}\triangleq {\mathfrak{J}}_a^{-\ast }\mathfrak{a}=\left[\begin{array}{c}{\mathfrak{a}}_{\mathrm{C}}\\ {\underset{\_}{\mathfrak{a}}}_{\mathfrak{S}}\\ {\mathfrak{a}}_{\mathrm{P}}\end{array}\right],{\underset{\_}{\mathfrak{a}}}_{\mathfrak{S}}\triangleq {\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathfrak{a}}_{\mathfrak{S}}\\ \underset{\_}{\mathfrak{f}}={\mathbb{A}}_a(\mathbf{M}\alpha +\mathfrak{b}),\mathit{\text{where}}\underset{\_}{\mathfrak{f}}\triangleq {\mathfrak{J}}_a\mathfrak{f}=\left[\begin{array}{c}{\mathfrak{f}}_{\mathrm{C}}\\ {\mathfrak{f}}_{\mathfrak{S}}^{\prime }\\ {\mathfrak{f}}_{\mathrm{P}}\end{array}\right],{\mathfrak{f}}_{\mathfrak{S}}^{\prime }\triangleq {\mathbb{A}}_{\mathfrak{S}}^{-1}{\mathfrak{f}}_{\mathfrak{S}}\\ \mathrm{J}={\mathrm{H}}_{\mathfrak{a}}\underset{\_}{\mathfrak{f}}\end{array}$$ (8)

Thus,

$$\mathrm{J}=\mathrm{ℳ}\ddot{\mathrm{\theta }}+\mathrm{C}$$ (9)

with

$$\mathrm{ℳ}={\mathrm{H}}_a{\mathbb{A}}_a\mathbf{M}{\mathbb{A}}_a^{\ast }{\mathrm{H}}_a^{\ast }\in {ℛ}^{\mathcal{N}\times \mathcal{N}}\mathit{\text{and}}\mathrm{C}\triangleq {\mathrm{H}}_a{\mathbb{A}}_a(\mathbf{M}{\mathbb{A}}_a^{\ast }\underset{\_}{\mathfrak{a}}+\mathfrak{b})\in {ℛ}^{\mathcal{N}}$$ (10)

The mass matrix expression above represents the Newton-Euler operator factorization of the mass matrix for the SKO model of the aggregated system.

Proof

3 Aggregation relationships at the component level

W now examine the component level recursive relationships between the bodies in the aggregated SKO model. The parent link for the aggregate link is denoted p, while c denotes a child body of . Also, denotes a body in the sub-graph that is a child of p, i.e., a base-body in the sub-graph. Body in the sub-graph denotes the parent of body c. This notation is illustrated in Figure 1. We focus only on the component-level relationships involving the aggregated body and its adjacent bodies since these relationships are the only ones effected by the aggregation process. The relationships involving non-aggregated links alone remain unchanged.

Fig. 1.

Body indexing scheme for the aggregation sub-graph

3.1 Velocity relationships

The following link-to-link spatial velocity recursive relationship holds between adjacent bodies in the original tree:

$$\mathcal{V}\left(\mathrm{k}\right)={\mathbb{A}}^{\ast }(\wp \left(\mathrm{k}\right),\mathrm{k})\mathcal{V}(\wp \left(\mathrm{k}\right))+{\mathrm{H}}^{\ast }\left(\mathrm{k}\right)\dot{\mathrm{\theta }}\left(\mathrm{k}\right)$$ (11)

We will use it to derive velocity relationships for the aggregated body and its adjacent bodies in the tree. First, let us develop a recursive relationship for . Assembling Eq. 11 for all the links in , we obtain the following relationship for

$${\mathcal{V}}_{\mathfrak{S}}={ℰ}_{{\mathbb{A}}_{\mathfrak{S}}}^{\ast }{\mathcal{V}}_{\mathfrak{S}}+{\mathrm{E}}_{\mathfrak{S}}^{\ast }\mathcal{V}\left(\mathrm{p}\right)+{\mathrm{H}}_{\mathfrak{S}}^{\ast }{\dot{\mathrm{\theta }}}_{\mathfrak{S}}\text{where}{\mathrm{E}}_{\mathfrak{S}}\triangleq \left[\mathbf{0},\mathbf{0},\cdots \mathbb{A}\left(\mathrm{p},\mathfrak{i}\right)\right]$$ (12)

This implicit relationship for involves quantities associated with nodes in the aggregated tree rather than the nodes in the spanning tree. E is the connector block matrix for the coupling between the aggregated body and its p parent body. While the Eq. 12 expression for E shows only a single (p, ) non-zero entry, more generally, it will have a non-zero entry for each base-body in at locations corresponding to the indices for the base-bodies. Since ℰ is an SKO operator, we can use the ≜ (I − ℰ)⁻¹ expression to convert the implicit form of Eq. 12 into the following explicit expression:

$${\mathcal{V}}_{\mathfrak{S}}\stackrel{12}{=}{\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathrm{E}}_{\mathfrak{S}}^{\ast }\mathcal{V}(\mathrm{p})+{\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathrm{H}}_{\mathfrak{S}}^{\ast }{\dot{\mathrm{\theta }}}_{\mathfrak{S}}\stackrel{7}{=}{\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathrm{E}}_{\mathfrak{S}}^{\ast }\mathcal{V}(\mathrm{p})+{\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}^{\ast }{\dot{\mathrm{\theta }}}_{\mathfrak{S}}$$ (13)

The form of Eq. 13 is similar to Eq. 11. It defines how the spatial velocity of the parent body p couples into the aggregated body . From this expression, we infer that E is the (p, ) SKO weight matrix associated with the and p adjacent body pair in , i.e.,

$$\mathbb{A}\left(\mathrm{p},\mathfrak{S}\right)\equiv {\mathrm{E}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}$$ (14)

Observe that the E weight matrix also appears in the partitioned expression for the ℰ aggregated SKO operator in Eq. 5.

The (c) spatial velocity of the child link c depends on the spatial velocity of its parent link in the sub-graph. The Eq. 11 relationship for link c can be restated as:

$$\mathcal{V}\left(\mathrm{c}\right)={ℬ}_{\mathrm{c}}^{\ast }{\mathcal{V}}_{\mathfrak{S}}+{\mathrm{H}}^{\ast }\left(\mathrm{c}\right)\dot{\mathrm{\theta }}\left(\mathrm{c}\right)\text{where}{ℬ}_{\mathrm{c}}\triangleq \left[\begin{array}{c}\mathbb{A}(\mathfrak{j},\mathrm{c})\\ \mathbf{0}\\ ⋮\\ \mathbf{0}\end{array}\right]$$ (15)

The form of this new relationship is similar to Eq. 11 and defines how the velocities of the parent body couple into the child body c. Eq. 15 involves quantities associated with nodes in the aggregated tree rather than quantities associated with nodes in the spanning tree. ℬ_c is the connector block matrix for the coupling between the c body and its parent body . From this expression, we infer that ℬ_c is the ( , c) SKO weight matrix associated with the c and adjacent pair of bodies, i.e.,

$$\mathbb{A}\left(\mathfrak{S},\mathrm{c}\right)\equiv {ℬ}_{\mathrm{c}}$$ (16)

There is one ℬ_c matrix for each child body c in ∁( ). The ℬ_c matrices for all the children bodies make up the columns of the ℬ matrix in the partitioned expression for ℰ in Eq. 5.

Gathering together the expressions in Eq. 13 and Eq. 15, the spatial velocity relationship between the aggregate link and its adjacent links p and c are as follows:

$$\begin{array}{c}{\mathcal{V}}_{\mathfrak{S}}\stackrel{13,14}{=}{\mathbb{A}}^{\ast }(\mathrm{p},\mathfrak{S})\mathcal{V}(\mathrm{p})+{\mathrm{H}}_{\mathfrak{S}}^{\ast }{\dot{\mathrm{\theta }}}_{\mathfrak{S}}\\ \mathcal{V}(\mathrm{c})\stackrel{15,16}{=}{\mathbb{A}}^{\ast }(\mathfrak{S},\mathrm{c}){\mathcal{V}}_{\mathfrak{S}}+{\mathrm{H}}^{\ast }(\mathrm{c})\dot{\mathrm{\theta }}(\mathrm{c})\end{array}$$ (17)

3.2 Acceleration relationships

The following link-to-link spatial acceleration relationship holds for bodies in the original tree:

$$\alpha \left(\mathrm{k}\right)={\mathbb{A}}^{\ast }(\wp \left(\mathrm{k}\right),\mathrm{k})\alpha (\wp \left(\mathrm{k}\right))+{\mathrm{H}}^{\ast }\left(\mathrm{k}\right)\ddot{\mathrm{\theta }}\left(\mathrm{k}\right)+\mathfrak{a}\left(\mathrm{k}\right)$$ (18)

Assembling Eq. 18 for all the links in , we obtain the following implicit relationship for α:

$${\alpha }_{\mathfrak{S}}={ℰ}_{{\mathbb{A}}_{\mathfrak{S}}}^{\ast }{\alpha }_{\mathfrak{S}}+{\mathrm{E}}_{\mathfrak{S}}^{\ast }\alpha \left(\mathrm{p}\right)+{\mathrm{H}}_{\mathfrak{S}}^{\ast }{\ddot{\mathrm{\theta }}}_{\mathfrak{S}}+{\mathfrak{a}}_{\mathfrak{S}}$$ (19)

Once again using the ≜ (I − ℰ)⁻¹ expression, we can convert the implicit form above into the following explicit expression:

$$\begin{array}{l}{\alpha }_{\mathfrak{S}}\stackrel{19}{=}{\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathrm{E}}_{\mathfrak{S}}^{\ast }\alpha (\mathrm{p})+{\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathrm{H}}_{\mathfrak{S}}^{\ast }{\ddot{\mathrm{\theta }}}_{\mathfrak{S}}+{\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathfrak{a}}_{\mathfrak{S}}\\ \stackrel{8,7,14}{=}{\mathbb{A}}^{\ast }(\mathrm{p},\mathfrak{S})\alpha (\mathrm{p})+{\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}^{\ast }{\ddot{\mathrm{\theta }}}_{\mathfrak{S}}+{\underset{\_}{\mathfrak{a}}}_{\mathfrak{S}}\end{array}$$ (20)

Similarly, the spatial acceleration relationship between body and its child body c takes the form:

$$\begin{array}{l}\alpha \left(\mathrm{c}\right)={ℬ}_{\mathrm{c}}^{\ast }\alpha \left(\mathfrak{S}\right)+{\mathrm{H}}^{\ast }\left(\mathrm{c}\right)\ddot{\mathrm{\theta }}\left(\mathrm{c}\right)+\mathfrak{a}\left(\mathrm{c}\right)\\ \stackrel{16}{=}{\mathbb{A}}^{\ast }\left(\mathfrak{S},\mathrm{c}\right)\alpha \left(\mathfrak{S}\right)+{\mathrm{H}}^{\ast }\left(\mathrm{c}\right)\ddot{\mathrm{\theta }}\left(\mathrm{c}\right)+\mathfrak{a}\left(\mathrm{c}\right)\end{array}$$ (21)

Both Eq. 20 and Eq. 21 have the familiar recursive form of Eq. 18, except that they are for the aggregated tree instead of the original tree.

3.3 Force relationships

We now examine the transformed spatial force expressions for the aggregated body. For this, we need the spatial inertia term, M, for the aggregate link. M is simply the partitioned sub-block of the block-diagonal M corresponding to the aggregated body in . The link-to-link spatial force relationships for bodies in the original tree are as follows:

$$\begin{array}{l}\mathfrak{f}(\mathrm{k})=\sum _{\forall \mathfrak{j}\in \complement (\mathrm{k})}\mathbb{A}\left(\mathrm{k},\mathfrak{j}\right)\mathfrak{f}\left(\mathfrak{j}\right)+\mathrm{M}\left(\mathrm{k}\right)\alpha \left(\mathrm{k}\right)+\mathfrak{b}\left(\mathrm{k}\right)\\ \mathrm{J}(\mathrm{k})=\mathrm{H}(\mathrm{k})\mathfrak{f}(\mathrm{k})\end{array}$$ (22)

Assembling these expressions for all the links in , we obtain the following relationships for :

$$\begin{array}{l}{\mathfrak{f}}_{\mathfrak{S}}\stackrel{22}{=}{ℰ}_{{\mathbb{A}}_{\mathfrak{S}}}{\mathfrak{f}}_{\mathfrak{S}}+\sum _{\forall \mathrm{c}\in \complement (\mathfrak{S})}{ℬ}_{\mathrm{c}}\mathfrak{f}(\mathrm{c})+{\mathrm{M}}_{\mathfrak{S}}{\alpha }_{\mathfrak{S}}+{\mathfrak{b}}_{\mathfrak{S}}\\ \stackrel{16}{=}{ℰ}_{{\mathbb{A}}_{\mathfrak{S}}}{\mathfrak{f}}_{\mathfrak{S}}+\sum _{\forall \mathrm{c}\in \complement (\mathfrak{S})}\mathbb{A}(\mathfrak{S},\mathrm{c})\mathfrak{f}(\mathrm{c})+{\mathrm{M}}_{\mathfrak{S}}{\alpha }_{\mathfrak{S}}+{\mathfrak{b}}_{\mathfrak{S}}\\ {\mathrm{J}}_{\mathfrak{S}}={\mathrm{H}}_{\mathfrak{S}}{\mathfrak{f}}_{\mathfrak{S}}\end{array}$$

Using ≜ (I − ℰ)⁻¹ leads to the following explicit form:

$$\begin{array}{l}(\mathbf{I}-{ℰ}_{{\mathbb{A}}_{\mathfrak{S}}}){\mathfrak{f}}_{\mathfrak{S}}=\sum _{\forall \mathrm{c}\in \complement (\mathfrak{S})}\mathbb{A}(\mathfrak{S},\mathrm{c})\mathfrak{f}(\mathrm{c})+{\mathrm{M}}_{\mathfrak{S}}{\alpha }_{\mathfrak{S}}+{\mathfrak{b}}_{\mathfrak{S}}\\ \Rightarrow {\mathfrak{f}}_{\mathfrak{S}}^{\prime }\stackrel{8}{=}\sum _{\forall \mathrm{c}\in \complement (\mathfrak{S})}\mathbb{A}(\mathfrak{S},\mathrm{c})\mathfrak{f}(\mathrm{c})+{\mathrm{M}}_{\mathfrak{S}}{\alpha }_{\mathfrak{S}}+{\mathfrak{b}}_{\mathfrak{S}}\\ {\mathrm{J}}_{\mathfrak{S}}\stackrel{8}{=}{\mathrm{H}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}{\mathfrak{f}}_{\mathfrak{S}}^{\prime }\stackrel{7}{=}{\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}{\mathfrak{f}}_{\mathfrak{S}}^{\prime }\end{array}$$ (23)

Similarly, the spatial force relationship between the and p bodies can be expressed as:

$$\begin{array}{l}\mathfrak{f}(\mathrm{p})\stackrel{22}{=}{\mathrm{E}}_{\mathfrak{S}}{\mathfrak{f}}_{\mathfrak{S}}+\mathrm{M}(\mathrm{p})\alpha (\mathrm{p})+\mathfrak{b}(\mathrm{p})\\ \stackrel{23,8}{=}{\mathrm{E}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}{\mathfrak{f}}_{\mathfrak{S}}^{\prime }+\mathrm{M}(\mathrm{p})\alpha (\mathrm{p})+\mathfrak{b}(\mathrm{p})\\ \stackrel{14}{=}\mathbb{A}(\mathrm{p},\mathfrak{S}){\mathfrak{f}}_{\mathfrak{S}}^{\prime }+\mathrm{M}(\mathrm{p})\alpha (\mathrm{p})+\mathfrak{b}(\mathrm{p})\\ \mathrm{J}(\mathrm{p})=\mathrm{H}(\mathrm{p})\mathfrak{f}(\mathrm{p})\end{array}$$ (24)

Taken together, Eq. 23 and Eq. 24 define the spatial force relationships involving the aggregated body and its adjacent links. These expressions have the recursive structure of Eq. 22. They are component-level versions of the equivalent operator expressions for the SKO model in Eq. 8. They define the altered steps for aggregated bodies in the computational algorithms derived from the SKO model formulation.

4 Constraint embedding

The notion of SKO models has so far been restricted to multibody systems with tree digraphs. Since the standard digraph² for closed-chain systems is not a tree, SKO models cannot be directly defined for them. In the following sections, we use the aggregation technique for tree systems to develop a constraint embedding strategy that allows us to develop SKO models for closed-chain systems.

4.1 Closed-chain system dynamics

There is a wide gap between the rich set of analysis techniques and algorithms available for the dynamics of multibody systems with tree-topologies and the more limited set available for closed-graph topology systems. While absolute coordinate formulations utilizing non-minimal generalized coordinates lead to differential algebraic equation (DAE) forms of equations of motion for tree-topology systems, the alternative SKO model minimal coordinates formulation for the same system can be described using nonlinear ordinary differential equations (ODE). A host of analytical and algorithmic techniques for tree systems are a direct consequence of the underlying SKO model structure.

While formulation choice for tree-topology systems can replace a DAE formulation with on ODE one, in contrast, closed-chain formulations typically use non-minimal coordinates DAE formulations [1, 4, 12, 15–17]. Projected dynamics [13] are an alternative minimal coordinate formulation for closed-chain systems where the minimal coordinates are selected using numerical techniques (eg. singular value decomposition) and without accompanying SKO models. The lack of an SKO model for such systems implies that the host of algorithms and analysis techniques for tree systems are not directly available for closed-chain systems. In this section we discuss a constraint embedding approach based on aggregation techniques that allows us to develop SKO models for closed-chain systems, and bridges the gap between tree and closed-chain dynamics formulations.

4.2 Constraint embedding strategy

The constraint embedding strategy is to transform the non-tree closed-chain system into an SKO model. The approach is to isolate non-tree sub-graphs and excise them using aggregation to transform the system digraph into a tree. Once the transformation is complete, an SKO model is developed for the transformed tree. The constraint embedding strategy involves the following steps:

1. Decompose the non-tree digraph for the system into a spanning tree, , and a collection of cut-edges for the constraints. The set of cut-edges is usually not unique. Develop an SKO model for the system associated with the spanning tree, .

2. For each cut-edge, identify the aggregation sub-graph, , for the sub-graph consisting of the node pair for the cut-edge. Figure 2 illustrates minimal and non-minimal choices for an aggregation sub-graph for a system with a loop constraint.

   A procedure for creating this aggregation sub-graph is is as follows [9]:

   1. Identify the smallest sub-tree that contains the nodes in the cut-edge.

   2. Remove the root node from this sub-tree to obtain the aggregation sub-graph .

3. Aggregate , and use Lemma 4 to derive an SKO model for the aggregated tree.

4. By construction, the nodes for each cut-edge belong to the internal sub-graph of the aggregated body. Moreover, by Jain [9], any directed cycles and multiply-connected paths associated with the cut-edge belong to , since the sub-graph satisfies the aggregation condition and is therefore path-induced. Consequently, the cut-edge constraints can be defined purely in terms of the state of the nodes within the sub-graph, independent of the rest of the nodes in the system! Once all the cut-edges have been processed, we are left with an aggregated tree containing a set of aggregated nodes, where the sub-graph of each aggregated node has one or more cut-edge constraints internal to it. Figure 3 illustrates the transformation of the original system digraph into a tree with aggregate nodes having internal cut-edge constraints. The next step is to eliminate the constraint from the sub-graph of each aggregated node. This process is described in detail in Section 4.3.

Fig. 2.

Minimal, and non-minimal, choices for a sub-graph that is a an aggregation sub-graph for the cut-edge nodes sub-graph.

Fig. 3.

Transformation of the original digraph of the system into a tree using sub-graph aggregation.

At the conclusion of the constrained embedding process, all of the constraints are absorbed into the aggregated links and subsequently eliminated from the multibody digraph. An SKO model, with minimal coordinates (satisfying the constraints), can now be developed as shown later in Lemma 6. Being an SKO model, all SKO formulation techniques and algorithms can be applied.

Figure 4 illustrates the following additional examples of aggregation sub-graphs for non-tree digraphs:

Fig. 4.

Examples of valid aggregation sub-graphs. For the same digraph in (1), the split sub-graphs are preferable to the combined single sub-graph since the sub-graphs are smaller. Example (2), has a cut-edge across independent trees. In Example (3), a pair of cut-edges share a node and, hence, they must be in the same aggregation sub-graph.

1. This example contains a pair of cut-edges. Two possible choices of aggregation sub-graphs are shown. One option contains a single aggregation sub-graph with both cut-edges, while the second shows a pair of sub-graphs, each with a single cut-edge. Even though the latter option results in an aggregated tree with an extra node, it is the slightly more preferable option since the aggregation sub-graphs are smaller.

2. In this example, the cut-edge is between nodes on disconnected systems. In this case, the aggregation sub-graph contains all the ancestor nodes of the cut-edge nodes. The aggregated node is a root node for the SKO-forest of the aggregated tree.

3. This example shows a digraph in which a pair of cut-edges share a node. In this case, the aggregated sub-graph includes the nodes from both cut-edges, and, unlike example (1), there is no split option possibility for the aggregation sub-graph.

4.3 Embedding constraint sub-graphs

In this section, we describe the process for eliminating the constraints from the aggregation sub-graphs required in step 4 of the constraint embedding strategy in the previous section. We focus our attention on a single constraint cut-edge in the system. The process can be applied repeatedly when there are multiple cut-edge constraints. Let us assume that steps 1-3 in Section 4 have been completed to obtain an aggregated body, with sub-graph satisfying the aggregation condition. The aggregation sub-graph has the cut-edge constraints. One consequence of the cut-edge constraints is that the effective degrees of freedom in is less than the total hinge degrees of freedom in .

We adopt the body indexing scheme from Section 3 for sub-graph aggregation. The parent link for the aggregated link is denoted p, while c denotes a child body of . Also, denotes a body in the sub-graph that is a child of p, i.e., a base-body in the sub-graph. Body in sub-graph denotes the parent of body c. This indexing scheme for is illustrated in Figure 5.

Fig. 5.

Body indexing scheme for the aggregation sub-graph with its cut-edge

Due to the cut-edge internal constraints on the , the elements of θ̇ are not independent, but are instead coupled by the constraint. At the coordinate level, the constraint often takes the form of a nonlinear algebraic relationship. However, at the velocity level, the generalized velocities in the system are subject to a linear, configuration dependent constraint. It is possible to partition the θ̇ generalized velocities of the sub-graph into disjoint sets of independent and dependent generalized velocities. The independent set is denoted θ̇_R . They can be used to obtain the dependent generalized velocities satisfying the cut-edge constraints on the sub-graph. In other words, there exists a (configuration dependent) mapping X such that

$${\dot{\mathrm{\theta }}}_{\mathfrak{S}}={\mathrm{X}}_{\mathfrak{S}}{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}$$ (25)

In deriving Eq. 25 we have assumed for simplicity that the constraint is catastatic so that the particular solution of the constraint equation is zero. When the constraint sub-graphs are isolated and of moderate size, the constraint elimination process is especially straightforward and computationally inexpensive. Examples of such local loops include constraints associated with geared motors, 4-bar linkages, wishbone suspensions, and differentials, etc.

Using the partitioned structure induced by the sub-graph, as discussed in Section 3, define

$${\mathrm{H}}_{\mathrm{R}a}\triangleq \left(\begin{array}{ccc}\mathbf{I}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\mathrm{X}}_{\mathfrak{S}}^{\ast }& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{I}\end{array}\right){\mathrm{H}}_a\stackrel{7}{=}\left(\begin{array}{ccc}{\mathrm{H}}_{\mathrm{C}}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& {\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& {\mathrm{H}}_{\mathrm{P}}\end{array}\right)$$ (26)

with

$${\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}\triangleq {\mathrm{X}}_{\mathfrak{S}}^{\ast }{\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}\stackrel{7}{=}{\mathrm{X}}_{\mathfrak{S}}^{\ast }{\mathrm{H}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}\text{and}{\dot{\mathrm{\theta }}}_{\mathrm{R}}\triangleq \left[\begin{array}{c}{\dot{\mathrm{\theta }}}_{\mathrm{C}}\\ {\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}\\ {\dot{\mathrm{\theta }}}_{\mathrm{P}}\end{array}\right]$$ (27)

θ̇_R denotes the minimal size, system-level, generalized velocity stacked vector containing the independent generalized velocities for the constrained system. Observe from Eq. 25 and Eq. 26 that

$${\mathrm{H}}_{\mathrm{R}a}^{\ast }{\dot{\mathrm{\theta }}}_{\mathrm{R}}={\mathrm{H}}_a^{\ast }\dot{\mathrm{\theta }}$$ (28)

The following lemma establishes a relationship between the sequence of spatial operators we have introduced during the constraint embedding process.

Lemma 5 Velocity relationships with constraint embedding

We have the following identities for :

$$\mathcal{V}={\mathbb{A}}^{\ast }{\mathrm{H}}^{\ast }\dot{\mathrm{\theta }}={\mathbb{A}}_a^{\ast }{\mathrm{H}}_a^{\ast }\dot{\mathrm{\theta }}={\mathbb{A}}_a^{\ast }{\mathrm{H}}_{\mathrm{R}a}^{\ast }{\dot{\mathrm{\theta }}}_{\mathrm{R}}$$ (29)

Proof The definitions of _a and H_a in Eq. 6 and Eq. 7 lead to the ${\mathbb{A}}^{\ast }{\mathrm{H}}^{\ast }={\mathbb{A}}_a^{\ast }{\mathrm{H}}_a^{\ast }$ identity and the first equality in Eq. 29. The latter part follows from the use of Eq. 28.

The last equality in Eq. 29 depends on only the independent generalized velocities θ̇_R for the system. It effectively eliminates the cut-edge constraints on the sub-graph, since independent generalized velocities are minimal and always satisfy the constraint.

The following lemma develops an SKO model for the closed-chain system based on the constraint embedding approach.

Lemma 6 Constraint embedding SKO model

With θ̇_R serving as the minimal size generalized velocities, and the aggregated tree as the system digraph, the spatial operators (H_Ra, _a, M) define an SKO model for the closed-chain system. The equations of motion are as follows:

$$\begin{array}{l}\mathcal{V}={\mathbb{A}}_a^{\ast }{\mathrm{H}}_{\mathrm{R}a}^{\ast }{\dot{\mathrm{\theta }}}_{\mathrm{R}}\\ \alpha ={\mathbb{A}}_a^{\ast }({\mathrm{H}}_{\mathrm{R}a}^{\ast }{\ddot{\mathrm{\theta }}}_{\mathrm{R}}+{\mathfrak{a}}^{\prime }),\mathit{\text{where}}{\mathfrak{a}}^{\prime }\triangleq \left[\begin{array}{c}{\mathfrak{a}}_{\mathrm{C}}\\ {\mathfrak{a}}_{\mathfrak{S}}^{\prime }\\ {\mathfrak{a}}_{\mathrm{P}}\end{array}\right],{\mathfrak{a}}_{\mathfrak{S}}^{\prime }\triangleq {\underset{\_}{\mathfrak{a}}}_{\mathfrak{S}}+{\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}^{\ast }{\dot{\mathrm{X}}}_{\mathfrak{S}}{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}\\ \mathfrak{f}={\mathbb{A}}_a(\mathbf{M}\alpha +\mathfrak{b})\\ {\mathrm{J}}_{\mathrm{R}}={\mathrm{H}}_{\mathrm{R}a}\underset{\_}{\mathfrak{f}}\end{array}$$ (30)

Thus,

$${\mathrm{J}}_{\mathrm{R}}={\mathrm{ℳ}}_{\mathrm{R}}{\dot{\mathrm{\theta }}}_{\mathrm{R}}+{\mathrm{C}}_{\mathrm{r}}$$ (31)

where

$${\mathrm{ℳ}}_{\mathrm{r}}={\mathrm{H}}_{\mathrm{R}a}{\mathbb{A}}_a\mathbf{M}{\mathbb{A}}_a^{\ast }{\mathrm{H}}_{\mathrm{R}a}^{\ast }\mathit{\text{and}}{\mathrm{C}}_{\mathrm{r}}\triangleq {\mathrm{H}}_{\mathrm{R}a}{\mathbb{A}}_a(\mathbf{M}{\mathbb{A}}_a^{\ast }{\mathfrak{a}}^{\prime }+\mathfrak{b})$$ (32)

The mass matrix expression above represents the Newton-Euler operator expression for the SKO model mass matrix for the constrained system.

Proof By construction, the sub-graph satisfies the aggregation condition. Lemma 4 defines an SKO model for the aggregated tree with equations of motion defined as follows in Eq. 8:

$$\begin{array}{ll}\mathcal{V}={\mathbb{A}}_a^{\ast }{\mathrm{H}}_a^{\ast }\dot{\mathrm{\theta }}& \alpha ={\mathbb{A}}_a^{\ast }\left({\mathrm{H}}_a^{\ast }\ddot{\mathrm{\theta }}+\underset{\_}{\mathfrak{a}}\right)\\ \underset{\_}{\mathfrak{f}}={\mathbb{A}}_a\left(\mathbf{M}\alpha +\mathfrak{b}\right)& \mathrm{J}={\mathrm{H}}_a\underset{\_}{\mathfrak{f}}\end{array}$$ (33)

In addition, the θ̇_R generalized coordinates are independent and eliminate the cut-edge constraint within . Since the (H_a, _a, M) spatial operators satisfy the requirements for an SKO model for the tree digraph, replacing H_a by H_Ra leaves the required block-diagonal property intact, implying that (H_Ra, _a, M) also satisfy the SKO model structural requirements.

The first equation in Eq. 30 follows directly from the use of Eq. 29 in the first equation in Eq. 33.

Differentiating Eq. 25 we have

$${\ddot{\mathrm{\theta }}}_{\mathfrak{S}}={\mathrm{X}}_{\mathfrak{S}}{\ddot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}+{\dot{\mathrm{X}}}_{\mathfrak{S}}{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}$$ (34)

Using this in Eq. 20 leads to

$$\begin{array}{l}{\alpha }_{\mathfrak{S}}\stackrel{20}{=}{\mathbb{A}}^{\ast }\left(\mathrm{p},\mathfrak{S}\right)\alpha \left(\mathrm{p}\right)+{\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}^{\ast }{\ddot{\mathrm{\theta }}}_{\mathfrak{S}}+{\underset{\_}{\mathfrak{a}}}_{\mathfrak{S}}\\ \stackrel{34}{=}{\mathbb{A}}^{\ast }\left(\mathrm{p},\mathfrak{S}\right)\alpha \left(\mathrm{p}\right)+{\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}^{\ast }{\ddot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}+{\underset{\_}{\mathfrak{a}}}_{\mathfrak{S}}+{\underset{\_}{\mathrm{H}}}_{\mathfrak{S}}^{\ast }{\dot{\mathrm{X}}}_{\mathfrak{S}}{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}\\ \stackrel{30}{=}{\mathbb{A}}^{\ast }\left(\mathrm{p},\mathfrak{S}\right)\alpha \left(\mathrm{p}\right)+{\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}^{\ast }{\ddot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}+{\mathfrak{a}}_{\mathfrak{S}}^{\prime }\end{array}$$ (35)

Using this the expression for α in Eq. 33 leads to the corresponding expression in Eq. 30.

The generalized forces vector for the aggregated link with the reduced generalized velocity coordinates, J_R , is defined as

$${\mathrm{J}}_{\mathrm{R}\mathfrak{S}}\triangleq {\mathrm{X}}_{\mathfrak{S}}^{\ast }{\mathrm{J}}_{\mathfrak{S}}\stackrel{27}{=}{\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}{\mathfrak{f}}_{\mathfrak{S}}^{\prime }$$ (36)

Together these establish all the expressions in Eq. 30. Eq. 32 is a simple assembly of these component relationships.

J_R denotes the minimal size, system-level generalized force stacked vector with structure similar to that of θ̇_R. Similarly, the ′ stacked vector is a modified version of the Coriolis acceleration stacked vector with the ${\mathfrak{a}}_{\mathfrak{S}}^{\prime }$ term for body .

This lemma completes the conversion of the original constrained graph multibody model into an unconstrained tree-topology model through constraint embedding. Not only have the constraints been eliminated, but we also have an SKO model for the system and all the SKO techniques and algorithms can be applied.

5 Examples of constraint embedding

There are two important classes of constraints to consider:

1. Direct joint-level constraints: In this case, the constraints are expressed as a direct (often constant) algebraic relationship between joint angles (e.g., gear-box, differential suspensions).

2. Loop constraints: In this case, configuration dependent, loop constraints involve nodes on different bodies in the system (e.g., four-bar linkages).

The following sections describe examples for these constraint types. Section 5.1 describes the direct joint-level constraint for a geared link. Section 5.2 and 5.3 examine loop constraints for a planar four-bar linkage with different cut-edge choices. We assume that the bodies in these systems are rigid links, and therefore we switch to the ℰ_ϕ and ϕ notation in place of the abstract ℰ and notation.

5.1 Geared links

The motor is mounted on an inboard link driving the outer link for a geared link as shown in Figure 6. This creates a closure constraint between the three bodies. The aggregation sub-graph consists of the (k − 1)^th motor and its driven (k − 1)^th link body, with the k^th driven link serving as the parent body for the sub-graph. The multibody digraph and its associated spanning tree are illustrated in Figure 7. With θ_m(k) and θ_l(k) denoting the generalized coordinates for the k^th motor and link pair, and μ_G(k) the gear ratio, the constraint embedding terms for the geared joints are given by:

Fig. 6.

An internal loop created by a link mounted motor and gear mechanism

Fig. 7.

The figure on the left illustrates the original non-tree digraph associated with geared links systems. The figure on right is the spanning tree digraph for the system.

$$\begin{array}{c}{\dot{\mathrm{\theta }}}_{\mathfrak{S}}=\left[\begin{array}{c}{\mathrm{\theta }}_{\mathrm{m}}(\mathrm{k}-1)\\ {\mathrm{\theta }}_{\mathrm{l}}(\mathrm{k}-1)\end{array}\right]\in {ℛ}^2,{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}={\mathrm{\theta }}_{\mathrm{l}}(\mathrm{k}-1)\in {ℛ}^1\\ {\mathrm{X}}_{\mathfrak{S}}=\left[\begin{array}{c}{\mathrm{\mu }}_{\mathfrak{S}}(\mathrm{k}-1)\\ 1\end{array}\right]\in {ℛ}^{2\times 1}\\ {\dot{\mathrm{X}}}_{\mathfrak{S}}=\mathbf{0},{ℰ}_{\mathrm{\varphi }\mathfrak{S}}=\mathbf{0}\in {ℛ}^{12\times 12},{\mathrm{\varphi }}_{\mathfrak{S}}=\mathbf{I}\in {ℛ}^{12\times 12}\end{array}$$ (37)

Observe that X is constant and configuration independent.

5.2 Planar 4-bar linkage system (terminal cut)

Now consider a four-bar linkage within a system, as illustrated in Figure 8. The aggregation sub-graph consists of links , k, and l, with the root link being link p. We make a terminal cut at the hinge joining links p and l, to convert the sub-graph into a tree-topology system. The child link c is connected via a hinge to link . The constraint embedding terms for the four-bar linkage with the terminal cut are as follows:

Fig. 8.

A four-bar mechanism with a terminal cut

$$\begin{array}{c}{\mathrm{E}}_{\mathfrak{S}}=\left[\mathbf{0},\mathbf{0},\mathrm{\varphi }(\mathrm{p},\mathfrak{j})\right]\in {ℛ}^{6\times 18},\\ {\mathrm{\varphi }}_{\mathfrak{S}}=\left(\begin{array}{ccc}\mathbf{I}& \mathbf{0}& \mathbf{0}\\ \mathrm{\varphi }(\mathrm{k}-\mathrm{l})& \mathbf{I}& \mathbf{0}\\ \mathrm{\varphi }(\mathfrak{j},\mathrm{l})& \mathrm{\varphi }(\mathfrak{j},\mathrm{k})& \mathbf{I}\end{array}\right)\in {ℛ}^{18\times 18},{\mathrm{X}}_{\mathfrak{S}}=\left[\begin{array}{c}{r}_{\mathrm{l}}\\ r_{\mathrm{k}}\\ 1\end{array}\right]\in {ℛ}^{3\times 1}\\ {ℰ}_{{\mathrm{\varphi }}_{\mathfrak{S}}}==\left(\begin{array}{ccc}\mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathrm{\varphi }(\mathrm{k}-\mathrm{l})& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathrm{\varphi }(\mathfrak{j},\mathrm{k})& \mathbf{0}\end{array}\right)\in {ℛ}^{18\times 18},{ℬ}_{\mathfrak{S}}=\left[\begin{array}{c}\mathbf{0}\\ \mathrm{\varphi }(\mathfrak{j},\mathrm{c})\\ \mathbf{0}\end{array}\right]\in {ℛ}^{18\times 6}\end{array}$$ (38)

In the above, r_l and r_k are configuration dependent parameters that characterize the ratio of the hinge velocities of the , k, and l hinges within the planar four-bar linkage. Thus, X is configuration dependent and not constant.

5.3 Planar 4-bar linkage system (internal cut)

We once again consider the same four-bar linkage. This time an internal cut is made at the hinge joining links k and l. This converts the sub-graph into a tree-topology system, as shown in Figure 9. Hinges and l connect links and l to the p parent link. Observe that is now a forest of two sub-trees, unlike the terminal cut-edge choice in the previous section. One of the trees contains just the body l, while the other contains bodies and k. The constraint embedding terms for the four-bar linkage with internal cut are as follows:

Fig. 9.

A four-bar mechanism with an internal cut

$$\begin{array}{c}{ℰ}_{{\mathrm{\varphi }}_{\mathfrak{S}}}=\left(\begin{array}{ccc}\mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathrm{\varphi }(\mathfrak{j},\mathrm{k})& \mathbf{0}\end{array}\right)\in {ℛ}^{18\times 18}\\ {\mathrm{\varphi }}_{\mathfrak{S}}=\left(\begin{array}{ccc}\mathbf{I}& \mathbf{0}& \mathbf{0}\\ \mathbf{0}& \mathbf{I}& \mathbf{0}\\ \mathbf{0}& \mathrm{\varphi }(\mathfrak{j},\mathrm{k})& \mathbf{I}\end{array}\right)\in {ℛ}^{18\times 18},{\mathrm{X}}_{\mathfrak{S}}=\left[\begin{array}{c}{r}_{\mathrm{l}}\\ r_{\mathrm{k}}\\ 1\end{array}\right]\in {ℛ}^{3\times 1}\\ {\mathrm{E}}_{\mathfrak{S}}=\left[\mathrm{\varphi }(\mathrm{p},\mathrm{l})\right],\mathbf{0},\mathrm{\varphi }(\mathrm{p},\mathfrak{j})\in {ℛ}^{6\times 18},{ℬ}_{\mathfrak{S}}=\left[\begin{array}{c}\mathbf{0}\\ \mathrm{\varphi }(\mathfrak{j},\mathrm{c})\\ \mathbf{0}\end{array}\right]\in {ℛ}^{18\times 6}\end{array}$$ (39)

In comparison to the earlier terminal cut example, the digraph and the ℰ_ϕ and ϕ matrices are simpler here. On the other hand, the E matrix is more complex. E_S contains two non-zero elements, one for each of the base-bodies in the sub-graph.

6 Recursive AB forward dynamics

Having developed an SKO model for closed-chain systems in Lemma 6 using constraint embedding techniques, all of the SKO model algorithms and analysis techniques are now applicable to the system. We choose the important example of the articulated body AB forward dynamics algorithm for SKO models as an example to illustrate the application of these techniques to closed-chain systems via constraint embedding. The recursive AB algorithm is the fastest available forward dynamics algorithm for tree-topology systems [3, 14]. It represents an O( ) solution to the traditional O( ³) mass matrix based techniques for the same problem. The AB algorithm has been shown to be a natural consequence of an SKO model for the system [7, 8]. The lack of a corresponding AB algorithm for closed-chain systems can be traced to the lack of an SKO model to date for such non-tree systems.

The following sections provide an overview of the mass matrix factorization and AB forward dynamics algorithms for SKO models, followed by its extension to the constraint embedding SKO model.

6.1 Mass matrix factorization and AB algorithm for SKO models

The following Lemma describes the solution to a Riccati equation that is at the heart of the analytic mass matrix inversion technique for SKO models.

Lemma 7 The Riccati equation for SKO models

Let (H, , M) denote spatial operators for an SKO model. The following Riccati equation has a block-diagonal, symmetric and positive-definite operator solution, P:

$$\mathbf{M}=\mathrm{P}-{ℰ}_{\mathbb{A}}[\mathrm{P}-\mathrm{P}{\mathrm{H}}^{\ast }{(\mathrm{H}\mathrm{P}{\mathrm{H}}^{\ast })}^{-1}\mathrm{H}\mathrm{P}]{ℰ}_{\mathbb{A}}^{\ast }$$ (40)

The expression in Eq. 40 can be broken down into simpler sub-expressions as follows:

$$\mathbf{M}=\mathrm{P}-{ℰ}_{\mathbb{A}}[\underset{\mathrm{P}+}{\underbrace{\mathrm{P}-\underset{\tau }{\underbrace{\underset{\mathcal{G}}{\underbrace{\mathrm{P}{\mathrm{H}}^{\ast }{(\underset{\mathcal{D}}{\underbrace{\mathrm{H}\mathrm{P}{\mathrm{H}}^{\ast }}})}^{-1}}}\mathrm{H}}}\mathrm{P}}}]{ℰ}_{\mathbb{A}}^{\ast }$$

The above sub-expressions define the block-diagonal spatial operators:

$$\begin{array}{l}\mathcal{D}\triangleq \mathrm{H}\mathrm{P}{\mathrm{H}}^{\ast },\mathcal{G}\triangleq \mathrm{P}{\mathrm{H}}^{\ast }{\mathcal{D}}^{-1},\tau \triangleq \mathcal{G}\mathrm{H}\\ \overline{\tau }\triangleq \mathbf{I}-\tau ,{\mathrm{P}}^{+}\triangleq \overline{\tau }\mathrm{P}=\overline{\tau }\mathrm{P}{\overline{\tau }}^{\ast }\end{array}$$ (41)

The P(k) and other diagonal elements can be computed by the following O( ) tips-to-base gather recursion:

$$\{\begin{array}{l}\mathbf{\text{for}}\mathit{\text{all nodes}}\mathrm{k}\mathit{(}\mathit{\text{tips‐to‐base gather}}\mathit{)}\\ \mathrm{P}(\mathrm{k})=\sum _{\forall \mathfrak{j}\in \complement (\mathrm{k})}\mathbb{A}(\mathrm{k},\mathfrak{j}){\mathrm{P}}^{+}(\mathfrak{j}){\mathbb{A}}^{\ast }(\mathrm{k},\mathfrak{j})+\mathbf{M}(\mathrm{k})\\ \mathcal{D}(\mathrm{k})=\mathrm{H}(\mathrm{k})\mathrm{P}(\mathrm{k}){\mathrm{H}}^{\ast }(\mathrm{k})\\ \mathcal{G}(\mathrm{k})=\mathrm{P}(\mathrm{k}){\mathrm{H}}^{\ast }(\mathrm{k}){\mathcal{D}}^{-1}(\mathrm{k})\\ \tau (\mathrm{k})=\mathcal{G}(\mathrm{k})\mathrm{H}(\mathrm{k})\\ \overline{\tau }(\mathrm{k})=\mathbf{I}-\tau (\mathrm{k})\\ {\mathrm{P}}^{+}(\mathrm{k})=\overline{\tau }(\mathrm{k})\mathrm{P}(\mathrm{k})\\ \mathbf{\text{end loop}}\end{array}$$ (42)

Proof See [5].

Lemma 8 The ℰ_ψ SKO operator

Let ℰ_A be an SKO operator, and H, M, be compatible operators satisfying the conditions in Lemma 7. Let P denote the block-diagonal solution to the Eq. 40 Riccati equation. Define

$${ℰ}_{\mathrm{\psi }}\triangleq {ℰ}_{\mathbb{A}}\overline{\tau }$$ (43)

Then ℰ_ψ is an SKO operator, with weights and SPO operator ψ, defined by

$$\mathrm{\psi }(\wp \left(\mathrm{k}\right),\mathrm{k})\triangleq \mathbb{A}(\wp \left(\mathrm{k}\right),\mathrm{k})\overline{\tau }\left(\mathrm{k}\right),\mathit{\text{and}}\mathrm{\psi }\triangleq {(\mathbf{I}-{ℰ}_{\mathrm{\psi }})}^{-1}$$ (44)

Moreover, the Eq. 40 Riccati equation can be re-expressed as:

$$\mathbf{M}=\mathrm{P}-{ℰ}_{\mathrm{\psi }}\mathrm{P}{ℰ}_{\mathrm{\psi }}^{\ast }=\mathrm{P}-{ℰ}_{\mathbb{A}}\mathrm{P}{ℰ}_{\mathrm{\psi }}^{\ast }$$ (45)

Proof See [7, 8].

The following lemma derives the mass matrix factorization and inversion properties for SKO models.

Lemma 9 SKO model mass matrix factorization and inversion

Let (H, , M) denote spatial operators for an SKO model. Recall that its mass-matrix is defined as ℳ = H M *H*.

1. The ℳ mass matrix has an alternative Innovations Operator factorization defined by

   $$\mathrm{ℳ}=\left[\mathbf{I}+\mathrm{H}\mathbb{A}\mathrm{K}\right]\mathcal{D}{\left[\mathbf{I}+\mathrm{H}\mathbb{A}\mathrm{K}\right]}^{\ast }$$ (46)

   with K ≜ ℰ . In this equation, and K are operators obtained from the solution of the discrete Riccati equation from Lemma 8 for the SKO model.

2. [I + H K] is invertible, with inverse given by:

   $${\left[\mathbf{I}+\mathrm{H}\mathbb{A}\mathrm{K}\right]}^{-1}=\left[\mathbf{I}-\mathrm{H}\mathrm{\psi }\mathrm{K}\right]$$ (47)
3. The mass matrix ℳ is invertible, and the expression for its inverse is

   $${\mathrm{ℳ}}^{-1}={\left[\mathbf{I}-\mathrm{H}\mathrm{\psi }\mathrm{K}\right]}^{\ast }{\mathcal{D}}^{-1}\left[\mathbf{I}-\mathrm{H}\mathrm{\psi }\mathrm{K}\right]$$ (48)

Proof See [5].

The forward dynamics problem consists of computing the θ̈ generalized accelerations, given the J generalized forces for the system. The following lemma describes an explicit operator expression for θ̈.

Lemma 10 Expression for θ̈ = ℳ⁻¹ (J − C)

The explicit expression for θ̈ is

$$\ddot{\mathrm{\theta }}={\mathrm{ℳ}}^{-1}(\mathrm{J}-\mathrm{C})={\left[\mathbf{I}-\mathrm{H}\mathrm{\psi }\mathrm{K}\right]}^{\ast }{\mathcal{D}}^{-1}\left[\mathrm{J}-\mathrm{H}\mathrm{\psi }(\mathrm{K}\mathrm{J}+\mathrm{P}\mathfrak{a}+\mathfrak{b})\right]-{\mathrm{K}}^{\ast }{\mathrm{\psi }}^{\ast }\mathfrak{a}$$ (49)

Proof See [5].

The expression in Eq. 49 implies the recursive O( ) implementation described in Al­gorithm 6.1 and is known as the AB forward dynamics algorithm for SKO models. The algorithm consists of a tips-to-base gather sweep to compute the residual terms, followed by a base-to-tips scatter sweep for the generalized accelerations.

Algorithm 6.1 (O( ) AB forward dynamics for SKO models)

1. Compute the articulated body inertia P, , etc., quantities using the gather algorithm in Eq. 42.

2. Use the following gather and scatter recursions to compute θ̈:

$$\begin{array}{l}\{\begin{array}{l}\mathbf{\text{for}}\mathit{\text{all nodes}}\mathrm{k}\mathit{(}\mathit{\text{tips‐to‐base gather}}\mathit{)}\\ \mathfrak{z}(\mathrm{k})=\sum _{\forall \mathfrak{j}\in \complement (\mathrm{k})}\mathbb{A}(\mathrm{k},\mathfrak{j}){\mathfrak{z}}^{+}(\mathfrak{j})+\mathrm{P}(\mathrm{k})\mathfrak{a}(\mathrm{k})+\mathfrak{b}(\mathrm{k})\\ \epsilon (\mathrm{k})=\mathrm{J}(\mathrm{k})-\mathrm{H}(\mathrm{k})\mathfrak{z}(\mathrm{k})\\ \nu (\mathrm{k})={\mathcal{D}}^{-1}(\mathrm{k})\epsilon (\mathrm{k})\\ {\mathfrak{z}}^{+}(\mathrm{k})=\mathfrak{z}(\mathrm{k})+\mathcal{G}(\mathrm{k})\epsilon (\mathrm{k})\\ \mathbf{\text{end loop}}\end{array}\\ \{\begin{array}{l}\mathbf{\text{for}}\mathit{\text{all nodes}}\mathrm{k}\mathit{(}\mathit{\text{base‐to‐tips scatter}}\mathit{)}\\ {\alpha }^{+}\left(\mathrm{k}\right)={\mathbb{A}}^{\ast }(\wp \left(\mathrm{k}\right),\mathrm{k})\alpha (\wp \left(\mathrm{k}\right))\\ \ddot{\mathrm{\theta }}(\mathrm{k})=\nu (\mathrm{k})-{\mathcal{G}}^{\ast }(\mathrm{k}){\alpha }^{+}(\mathrm{k})\\ \alpha (\mathrm{k})={\alpha }^{+}(\mathrm{k})+{\mathrm{H}}^{\ast }(\mathrm{k})\ddot{\mathrm{\theta }}(\mathrm{k})+\mathfrak{a}(\mathrm{k})\\ \mathbf{\text{end loop}}\end{array}\end{array}$$

7 Recursive AB forward dynamics with constraint embedding

In this section we apply the AB forward dynamics algorithm to the SKO model developed in Lemma 6.

7.1 Articulated body inertias with constraint embedding

First, we describe the articulated body inertia steps from the Riccati equation, Eq. 42, for the aggregated link and its adjacent links:

$$\begin{array}{l}{\mathrm{P}}^{+}\left(\mathrm{c}\right)=\overline{\tau }(\mathrm{c})\mathrm{P}\left(\mathrm{c}\right)\\ {\mathrm{P}}_{\mathfrak{S}}=\sum _{\forall \mathrm{c}\in \complement \left(\mathfrak{S}\right)}\mathbb{A}\left(\mathfrak{S},\mathrm{c}\right){\mathrm{P}}^{+}\left(\mathrm{c}\right){\mathbb{A}}^{\ast }\left(\mathfrak{S},\mathrm{c}\right)+{\mathrm{M}}_{\mathfrak{S}}\\ {\mathcal{D}}_{\mathfrak{S}}={\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}{\mathrm{P}}_{\mathfrak{S}}{\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}^{\ast }\\ {\mathcal{G}}_{\mathfrak{S}}={\mathrm{P}}_{\mathfrak{S}}{\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}^{\ast }{\mathcal{D}}_{\mathfrak{S}}^{-1}\\ {\tau }_{\mathfrak{S}}={\mathcal{G}}_{\mathfrak{S}}{\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}\\ {\mathrm{P}}_{\mathfrak{S}}^{+}={\mathrm{P}}_{\mathfrak{S}}-{\tau }_{\mathfrak{S}}{\mathrm{P}}_{\mathfrak{S}}\\ \mathrm{P}\left(\mathrm{p}\right)=\mathbb{A}\left(\mathrm{p},\mathfrak{S}\right){\mathrm{p}}_{\mathfrak{S}}^{+}{\mathbb{A}}^{\ast }\left(\mathrm{p},\mathfrak{S}\right)+\mathrm{M}\left(\mathrm{p}\right)\end{array}$$ (50)

Recall that Eq. 14 and Eq. 16 define the following expressions for (p, ) and ( , c).

$$\mathbb{A}(\mathrm{p},\mathfrak{S})={\mathrm{E}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}\text{and}\mathbb{A}(\mathfrak{S},\mathrm{c})={ℬ}_{\mathrm{c}}$$

can be re-expressed as

$$\begin{array}{c}{\mathcal{D}}_{\mathfrak{S}}\stackrel{27}{=}{\mathrm{X}}_{\mathfrak{S}}^{\ast }\left({\mathrm{H}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}{\mathrm{P}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathrm{H}}_{\mathfrak{S}}^{\ast }\right){\mathrm{X}}_{\mathfrak{S}}={\mathrm{X}}_{\mathfrak{S}}^{\ast }{\mathrm{ℳ}}_{\mathfrak{S}}{\mathrm{X}}_{\mathfrak{S}}\\ \text{where}{\mathrm{ℳ}}_{\mathfrak{S}}\triangleq {\mathrm{H}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}{\mathrm{P}}_{\mathfrak{S}}{\mathbb{A}}_{\mathfrak{S}}^{\ast }{\mathrm{H}}_{\mathfrak{S}}^{\ast }\end{array}$$ (51)

ℳ has the structure of a mass matrix for the sub-graph. The primary difference from the true mass matrix of is that the central inertia term is P, instead of the usual ℳ spatial inertia term. P includes the articulated body contribution from the descendants of the body. is thus the projection of this mass matrix onto its independent degrees of freedom.

7.2 Mass matrix factorization and inversion with constraint embedding

The Riccati equation solution, and the associated articulated body inertia quantities in Eq. 50, together with Lemma 9, lead to the following Innovations factorization and inversion expressions for the mass matrix of the transformed system:

$$\begin{array}{l}{\mathrm{ℳ}}_{\mathrm{r}}\stackrel{32}{=}{\mathrm{H}}_{\mathrm{R}a}{\mathbb{A}}_a{\text{MA}}_a^{\ast }{\mathrm{H}}_{\mathrm{R}a}^{\ast }\\ =\left[\mathbf{I}+{\mathrm{H}}_{\mathrm{R}a}{\mathbb{A}}_a\mathrm{K}\right]\mathcal{D}{\left[\mathbf{I}+{\mathrm{H}}_{\mathrm{R}a}{\mathbb{A}}_a\mathrm{K}\right]}^{\ast }\\ {\left[\mathbf{I}+{\mathrm{H}}_{\mathrm{R}a}{\mathbb{A}}_a\mathrm{K}\right]}^{-1}=\mathbf{I}-{\mathrm{H}}_{\mathrm{R}a}{\mathrm{\psi }}_a\mathrm{K}\\ {\mathrm{ℳ}}_{\mathrm{r}}^{-1}={\left[\mathbf{I}-{\mathrm{H}}_{\mathrm{R}a}{\mathrm{\psi }}_a\mathrm{K}\right]}^{\ast }{\mathcal{D}}^{-1}\left[\mathbf{I}-{\mathrm{H}}_{\mathrm{R}a}{\mathrm{\psi }}_a\mathrm{K}\right]\end{array}$$ (52)

The ψ_a, , and K operators are those associated with the Riccati equation solution in Eq. 50. Continuing on, the explicit expression for the reduced generalized expressions from Lemma 10 takes the form:

$${\ddot{\mathrm{\theta }}}_{\mathrm{R}}={\left[\mathbf{I}-{\mathrm{H}}_{\mathrm{R}a}{\mathrm{\psi }}_a\mathrm{K}\right]}^{\ast }{\mathcal{D}}^{-1}\left[{\mathrm{J}}_{\mathrm{R}}-{\mathrm{H}}_{\mathrm{R}a}{\mathrm{\psi }}_a\left(\mathrm{K}{\mathrm{J}}_{\mathrm{R}}+\mathrm{P}{\mathfrak{a}}^{\prime }+\mathfrak{b}\right)\right]-{\mathrm{K}}^{\ast }{\mathrm{\psi }}_a^{\ast }{\mathfrak{a}}^{\prime }$$ (53)

Thus, the SKO model for the system has led to an explicit expression for the θ̈_R minimal generalized acceleration vector for the closed-chain system.

7.3 AB forward dynamics algorithm with constraint embedding

Eq. 53 can be converted into the AB recursive forward dynamics algorithm as described in Algorithm 6.1. The overall structure of the AB forward dynamics algorithm remains unchanged, with differences only at the steps involving the aggregated body and its neighbors.

The tip-to-base recursion steps from body c to the aggregated link have the following form:

$$\begin{array}{l}{\mathfrak{z}}^{+}\left(\mathrm{c}\right)=\mathfrak{z}\left(\mathrm{c}\right)+\mathcal{G}\left(\mathrm{c}\right)\epsilon \left(\mathrm{c}\right)\\ {\mathfrak{z}}_{\mathfrak{S}}=\sum _{\forall \mathrm{c}\in \complement \left(\mathfrak{S}\right)}\mathbb{A}\left(\mathfrak{S},\mathrm{c}\right){\mathfrak{z}}^{+}\left(\mathrm{c}\right)+{\mathfrak{b}}_{\mathfrak{S}}+{\mathrm{P}}_{\mathfrak{S}}{\mathfrak{a}}_{\mathfrak{S}}^{\prime }\\ {\epsilon }_{\mathfrak{S}}={\mathrm{J}}_{\mathrm{R}\mathfrak{S}}-{\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}{\mathfrak{z}}_{\mathfrak{S}}\\ {\nu }_{\mathfrak{S}}={\mathcal{D}}_{\mathfrak{S}}^{-1}{\epsilon }_{\mathfrak{S}}\end{array}$$ (54)

The tip-to-base recursion steps from body to body p are as follows:

$$\begin{array}{l}{\mathfrak{z}}_{\mathfrak{S}}^{+}={\mathfrak{z}}_{\mathfrak{S}}+{\mathcal{G}}_{\mathfrak{S}}{\epsilon }_{\mathfrak{S}}\\ \mathfrak{z}\left(\mathrm{p}\right)=\mathbb{A}\left(\mathrm{p},\mathfrak{S}\right){\mathfrak{z}}_{\mathfrak{S}}^{+}+\mathfrak{b}\left(\mathrm{p}\right)+\mathrm{P}\left(\mathrm{p}\right)\mathfrak{a}\left(\mathrm{p}\right)\\ \epsilon \left(\mathrm{p}\right)=\mathrm{J}\left(\mathrm{p}\right)-\mathrm{H}\left(\mathrm{p}\right)\mathfrak{z}\left(\mathrm{p}\right)\\ \nu \left(\mathrm{p}\right)={\mathcal{D}}^{-1}\left(\mathrm{p}\right)\epsilon \left(\mathrm{p}\right)\end{array}$$ (55)

The base-to-tip accelerations sweep steps are also altered for the aggregated link. The steps from body p to the aggregated body are as follows:

$$\begin{array}{l}{\alpha }_{\mathfrak{S}}^{+}={\mathbb{A}}^{\ast }\left(\mathrm{p},\mathfrak{S}\right)\alpha \left(\mathrm{p}\right)\\ {\ddot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}={\nu }_{\mathfrak{S}}-{\mathcal{G}}_{\mathfrak{S}}^{\ast }{\alpha }_{\mathfrak{S}}^{+}\\ {\ddot{\mathrm{\theta }}}_{\mathfrak{S}}={\mathrm{X}}_{\mathfrak{S}}{\ddot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}+{\dot{\mathrm{X}}}_{\mathfrak{S}}{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}\\ {\alpha }_{\mathfrak{S}}={\alpha }_{\mathfrak{S}}^{+}+{\underset{\_}{\mathrm{H}}}_{\mathrm{R}\mathfrak{S}}^{\ast }{\ddot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}+{\mathfrak{a}}_{\mathfrak{S}}^{\text{'}}\end{array}$$ (56)

The steps from the aggregated body to body c are as follows:

$$\begin{array}{l}{\alpha }^{+}\left(\mathrm{c}\right)={\mathbb{A}}^{\ast }\left(\mathfrak{S},\mathrm{c}\right){\alpha }_{\mathfrak{S}}\\ \ddot{\mathrm{\theta }}\left(\mathrm{c}\right)=\nu \left(\mathrm{c}\right)-{\mathcal{G}}^{\ast }\left(\mathrm{c}\right){\alpha }^{+}\left(\mathrm{c}\right)\\ \alpha \left(\mathrm{c}\right)={\alpha }^{+}\left(\mathrm{c}\right)+{\mathrm{H}}^{\ast }\left(\mathrm{c}\right)\ddot{\mathrm{\theta }}\left(\mathrm{c}\right)+\mathfrak{a}\left(\mathrm{c}\right)\end{array}$$ (57)

The most computationally expensive part of these steps is the evaluation and inversion of the symmetric, positive definite matrix in Eq. 50. Its size is the number of independent degrees of freedom for the aggregated link. Thus, the computational cost of the AB algorithm is no longer linear in the number of independent degrees of freedom for the aggregated links, but, instead, is (in the worst case) quadratic in the total degrees of freedom in the aggregation sub-graph, and cubic in the number of independent degrees of freedom in the sub-graph. These additional costs, however, are modest when the loops are of moderate size. Indeed, if we impose an upper bound on the size of the sub-graphs that the constraint-embedding is applied to, then the cost is quadratic to cubic in this upper bound size, and the overall algorithm remains linear in the overall system size. This AB algorithm is the first known general purpose extension of the algorithm to non-tree systems.

These computational considerations imply that the constraint-embedding method is best suited for small to moderate size aggregation sub-graphs. As the size of the sub-graph increases, the cost of the constraint-embedding algorithm goes up at least quadratically. The aggregation sub-graph can become large in cases where the system topology has such large loops or when small loops are coupled in mesh-like structures. For these type of constraints, conventional techniques based on solving for constraint forces are advisable.

8 Computing X and Ẋ

X maps the independent generalized velocities to the full generalized velocities of . For the case of direct joint-level constraints, it is straightforward to select the independent coordinates subset, and X is then simply the gradient of the mapping from the independent to the full generalized coordinates. For direct joint-level constraints, X is often constant, and Ẋ is consequently zero. In the following sections, we look at the less trivial case of loop constraints, and derive expressions for the configuration dependent X and its time-derivative Ẋ needed in Eq. 25 and Eq. 30.

For loop constraints, we have an algebraic constraint on the relative velocities of a pair of physical closure nodes in the sub-graph. Denoting a representative pair of closure nodes as o and p, such a constraint can be expressed as

$$\begin{array}{l}\mathbf{0}=\mathrm{A}({\mathcal{V}}_{\mathrm{o}}-{\mathcal{V}}_{\mathrm{p}})=\mathrm{A}({\mathrm{J}}_{\mathrm{o}}-{\mathrm{J}}_{\mathrm{p}}){\dot{\mathrm{\theta }}}_{\mathfrak{S}}\\ =\mathrm{A}[{\mathrm{J}}_{\mathrm{o}1}-{\mathrm{J}}_{\mathrm{p}1},{\mathrm{J}}_{\mathrm{o}2}-{\mathrm{J}}_{\mathrm{p}2}]\left[\begin{array}{c}{\dot{\mathrm{\theta }}}_{\mathrm{u}\mathfrak{S}}\\ {\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}\end{array}\right]=[{\mathrm{Y}}_1,{\mathrm{Y}}_2]\left[\begin{array}{c}{\dot{\mathrm{\theta }}}_{\mathrm{u}\mathfrak{S}}\\ {\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}\end{array}\right]\end{array}$$ (58)

Here, A denotes the constraint on the relative spatial velocities between this pair of closure nodes, and J_o, J_p denote sub-graph Jacobians relating the generalized velocities of the sub-graph to the spatial velocities at the o and p closure nodes. θ̇_u is the complement of the θ̇_R sub-vector in θ̇ and represents the dependent generalized velocity coordinates. J_o1, etc., represent sub-blocks within the corresponding partitioned Jacobians. Y₁ and Y₂ are defined as

$${\mathrm{Y}}_1\triangleq \mathrm{A}({\mathrm{J}}_{\mathrm{o}1}-{\mathrm{J}}_{\mathrm{p}1})\text{and}{\mathrm{Y}}_2\triangleq \mathrm{A}({\mathrm{J}}_{\mathrm{o}2}-{\mathrm{J}}_{\mathrm{p}2})$$ (59)

In Eq. 58, the partitioning is chosen such that Y₁ is square and full rank, and thus invertible. It follows that:

$${\dot{\mathrm{\theta }}}_{\mathrm{u}\mathfrak{S}}=-{\mathrm{Y}}_1^{-1}{\mathrm{Y}}_2{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}\Rightarrow {\mathrm{X}}_{\mathfrak{S}}=\left[\begin{array}{c}-{\mathrm{Y}}_1^{-1}{\mathrm{Y}}_2\\ \mathbf{I}\end{array}\right]$$ (60)

The following corollary derives an expression for Ẋ for systems with loop constraints.

Corollary 8.1 (Expression for Ẋ)

We have

$${\dot{\mathrm{X}}}_{\mathfrak{S}}=\left[\begin{array}{c}-{\mathrm{Y}}_1^{-1}\dot{\mathrm{Y}}{\mathrm{X}}_{\mathfrak{S}}\\ \mathbf{0}\end{array}\right]\mathit{\text{and}}{\dot{\mathrm{X}}}_{\mathfrak{S}}{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}=\left[\begin{array}{c}-{\mathrm{Y}}_1^{-1}\dot{\mathrm{Y}}{\dot{\mathrm{\theta }}}_{\mathfrak{S}}\\ \mathbf{0}\end{array}\right]$$ (61)

Where Y ≜ [Y₁, Y₂].

Proof: With $\mathrm{Z}\triangleq {\mathrm{Y}}_1^{-1}{\mathrm{Y}}_2$,

$$\begin{array}{l}\frac{\mathrm{d}\mathrm{Z}}{\mathrm{d}\mathrm{t}}=\frac{\mathrm{d}{\mathrm{Y}}_1^{-1}}{\mathrm{d}\mathrm{t}}{\mathrm{Y}}_2+{\mathrm{Y}}_1^{-1}\frac{\mathrm{d}{\mathrm{Y}}_2}{\mathrm{d}\mathrm{t}}=-{\mathrm{Y}}_1^{-1}\frac{\mathrm{d}{\mathrm{Y}}_1}{\mathrm{d}\mathrm{t}}{\mathrm{Y}}_1^{-1}{\mathrm{Y}}_2+{\mathrm{Y}}_1^{-1}\frac{\mathrm{d}{\mathrm{Y}}_2}{\mathrm{d}\mathrm{t}}\\ ={\mathrm{Y}}_1^{-1}\left[\frac{\mathrm{d}{\mathrm{Y}}_2}{\mathrm{d}\mathrm{t}}-\frac{\mathrm{d}{\mathrm{Y}}_1}{\mathrm{d}\mathrm{t}}\mathrm{Z}\right]\end{array}$$

Thus,

$${\dot{\mathrm{X}}}_{\mathfrak{S}}=\left[\begin{array}{c}-\dot{\mathrm{Z}}\\ \mathbf{0}\end{array}\right]=\left[\begin{array}{c}{\mathrm{Y}}_1^{-1}\left[{\dot{\mathrm{Y}}}_1\mathrm{Z}-{\dot{\mathrm{Y}}}_2\right]\\ \mathbf{0}\end{array}\right]=\left[\begin{array}{c}-{\mathrm{Y}}_1^{-1}\dot{\mathrm{Y}}{\mathrm{X}}_{\mathfrak{S}}\\ \mathbf{0}\end{array}\right]$$

This establishes the first half of Eq. 61.

Using this, we have

$${\dot{\mathrm{X}}}_{\mathfrak{S}}{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}=\left[\begin{array}{c}-{\mathrm{Y}}_1^{-1}\dot{\mathrm{Y}}{\mathrm{X}}_{\mathfrak{S}}{\dot{\mathrm{\theta }}}_{\mathrm{R}\mathfrak{S}}\\ \mathbf{0}\end{array}\right]=\left[\begin{array}{c}-{\mathrm{Y}}_1^{-1}\dot{\mathrm{Y}}{\dot{\mathrm{\theta }}}_{\mathfrak{S}}\\ \mathbf{0}\end{array}\right]$$

This establishes the second half of Eq. 61.

Eq. 61 requires expressions for Ẏ1 and Ẏ2. From Eq. 59, these derivatives require Ȧ and the time derivatives of the J₀₁, etc., blocks. The expression for Ȧ is usually straightforward to obtain from the inter-node constraint expression, and hence, we focus on the time derivative of the Jacobian blocks. The k^th column of J₀₁ is of the form ϕ*( _k, o)H*(k), where _k denotes the frame at the k^th hinge in θ̇, and H*(k) is the corresponding joint map matrix for the hinge. This derivative is a simple expression involving the relative linear and angular velocity of the o closure node with respect to the _k closure node.

9 Generalization to multiple branches and cut-edges

We summarize extensions for applying the constraint embedding technique to the more general multibody system context.

Multiple cut-edges

Our development of the component-level constraint embedding expression focused on the case of a single aggregation sub-graph during the con­straint embedding process. More generally, systems will have multiple aggregation sub-graphs from multiple cut-edge constraints for the system. In this case, the sys­tem will contain multiple aggregated links, potentially with multiple constraints embedded within each aggregated link. The structural form of the equations of motion remains the same. The key change to the algorithms is that aggregated link steps need to be executed when an aggregated link is encountered, and regular steps are executed otherwise.

Multiple child bodies

Aggregated bodies can also have multiple child bodies. For this case, ℬ is a multi-column matrix, with one column for each child body. Each column has a single non-zero “ ( , c)” entry corresponding to the parent body in the sub-graph and the child body. The AB forward dynamics algorithm has the standard gather/scatter structure for tree-topology algorithms. Thus, the tip-to-base sweeps gather and accumulate the results from all the branches at the ag­gregated link. The base-to-tip sweeps scatter the results onto the children branches of the aggregated link.

Multiple base-bodies

While the Eq. 12 expression for E shows only a single (p, ) non-zero entry, more generally, when the sub-graph has multiple base-bodies, E will have a non-zero entry for each base-body at locations corresponding to the indices for the base-bodies. This is illustrated in the expressions in Eq. 39 for the four-bar linkage example.

Closure constraint with the inertial frame

Another situation that we have not explicitly addressed is that of a closure constraint between a body node and the inertial frame. In this case, node p in ℘( ) is the inertial frame. The aggregated link in this case is a base-body attached directly to the inertial frame.

Joint-level constraints involving multiple hinges

It is possible to have the situation where a direct joint-level velocity constraint involves more than two hinges. Instead of a single closure-constraint edge, multiple cut-edges that span all the hinges involved in the constraint must be included in the system digraph before determining the aggregation sub-graph.

Singular constraint maps

Especially in the case of loop constraints, it is possible for the X matrix to lose rank at specific singular configurations of the sub-graph. The number of independent generalized velocities decreases at such singular configurations. Since the invertibility of depends on X being full-rank, it is important to monitor the rank of X and to adjust its size near, and at, singular configurations.

10 Conclusions

This paper uses graph theory techniques to develop a general constraint embedding approach for the development of closed-chain system SKO models, that are otherwise only available for tree multibody systems. The new SKO models represent a minimal coordinate formulation of the dynamics for the closed-chain system. In essence, the new formulation is an instance of projected dynamics [13], except that it also preserves the tree-topology structure in the model. The development of this technique builds upon the sub-graph aggregation methods developed in the first part of this two-part paper. This extension enables the application of the large family of analytical and algorithmic techniques for tree systems to closed-chain systems. These include the key analytical mass matrix factorization and inversion techniques as well as the well known articulated body inertia forward dynamics algorithm. This is illustrated through an extension of the low-order AB forward dynamics method for trees to closed-chain systems. More generally, the structural impact of extending the recursive dynamics algorithms from tree to closed-chain systems is limited to the steps that traverse an aggregated body in the system.