A projection continuation approach for minimal coordinate set constrained dynamics

Abstract

The formulation of constrained system dynamics using coordinate projection onto a subspace locally tangent to the constraint manifold is revisited using the QR factorization of the constraint Jacobian matrix to extract a suitable subspace and integrating the evolution of the QR factorization along with that of the constraint Jacobian matrix, as the solution evolves. A true continuation algorithm is thus proposed to track the evolution of the subspace of independent coordinates. It does not visibly affect the quality of the solution but avoids the artificial algorithmic irregularities or discontinuities in the generalized velocities that could otherwise result from arbitrary reparameterizations of the coordinate set. The characteristics of the proposed subspace evolution approach are exemplified by solving simple single- and multi-degree-of-freedom problems.

1 Introduction

In mechanical system dynamics, unconstrained dynamics problems are usually formulated as a set of second-order ordinary differential equations (ODE) that depend on a corresponding set of coordinates. A convenient approximation to describe the interaction between the parts of the system is often their idealization at a purely kinematic level, as algebraic relationships between the coordinates of those parts. The addition of these algebraic equations turns the problem into a system of differential-algebraic equations (DAE). The original coordinates are no longer independent; the actual number of independent coordinates reduces to that of the truly independent degrees of freedom. The enforcement of the constraints results in constraint reaction generalized forces, usually formulated as Lagrange multipliers. They represent the (unknown) internal forces whose value is whatever is required to guarantee the constraints’ enforcement. A review of the possible approaches can be found in [1].

The constrained dynamics problem can be solved either directly, as a system of DAEs, where the original coordinates, augmented by the Lagrange multipliers, represent the unknowns in the so-called redundant coordinate set (RCS) formulation or, through manipulations that will be detailed in a later section, it can be transformed into the corresponding underlying ODE problem, reducing the set of coordinates to the truly independent (Lagrangian) ones, following the so-called minimal coordinate set (MCS) formulation. For a review of the possible approaches, see for example [2]. This paper focuses on this latter approach.

It is worth noticing that a third approach is possible, i.e., to somehow embed the constraints in the unconstrained problem, formally preserving its original structure and unknowns. See for example the so-called augmented Lagrangian approach [3] or the force projection method [4], as discussed in [5]. In this case, the problem formally reduces as well to ODE, with all the related implications, opportunities, and limitations in terms of approaches for its numerical integration: explicit methods can be used, subjected to conditional stability limitations.

It is recognized that the reduction of the original coordinates into the MCS may be a challenging task and that their nature is local, i.e., there may not exist a generally valid choice, which works for all configurations of the system [6]. Such a choice being local, when the coordinates need to be redefined with respect to a new configuration, discontinuities are expected in the generalized coordinates (specifically, in their derivatives), although such discontinuities are not related to any physical discontinuity in the kinematics or dynamics of the system. Indeed, the motion with respect to the original coordinates is not expected to show any discontinuity, the latter being mere artifacts of a redefinition of the local coordinates across time steps.

As originally discussed in [7], this work presents a well-known and effective method for selecting a subspace of independent coordinates that is intrinsically tangent to the constraint manifold at a specific configuration, based on the QR factorization of the constraint Jacobian matrix, and discusses how to operate the redefinition of the coordinates’ subspace in a continuous manner, whenever possible, to eliminate those unnecessary, formal discontinuities.

What this work does not is advocate any superiority of the MCS over other approaches, nor any superiority of the proposed coordinate selection algorithm over others, especially in terms of performances. Being at its core a mere recombination of the coordinates, it does not substantially impact the effectiveness nor the efficiency of the solution, the required or allowed time step, nor the computational time, with the possible exception of the additional operations needed by the proposed coordinate selection, which likely add some minimal computational burden.

2 Problem description

2.1 Constrained dynamics problem formulation

A generic constrained system dynamics problem is formulated by adding $m$ (holonomic in the present case, without excessive loss of generality, and ideal) independent kinematic constraints as the set of algebraic equations

$$\begin{aligned} \mathbf {c}(\mathbf {x}, t) &= \mathbf {0} \end{aligned}$$

(1)

with $\mathbf{c}\in {\mathbb{R}}^m$, to a set of $n$ ordinary differential equations ($n \ge m$, but usually $n > m$) that express the dynamics of an unconstrained system of $n$ coordinates $\mathbf{x}\in {\mathbb{R}}^n$,

$$\begin{aligned} {\mathbf {M}} \ddot{\mathbf {x}} &= \mathbf {f} \end{aligned}$$

(2)

subjected to a set of generalized forces $\mathbf{f}\in {\mathbb{R}}^n$, energetically conjugated to a virtual perturbation of the coordinates $\delta \mathbf {x}$, where $\mathbf{M}\in {\mathbb{R}}^{n\times n}$ is the symmetric, positive-definite mass matrix. These equations are modified by the addition of the constraint reactions $\mathbf {f}_{c} = - \mathbf {c}_{/\mathbf {x}}^{T} \mathrm {\boldsymbol {\lambda}}$ as follows:

$$\begin{aligned} {\mathbf {M}} \ddot{\mathbf {x}} + \mathbf {c}_{/\mathbf {x}}^{T} \mathrm {\boldsymbol {\lambda}} &= \mathbf {f}, \end{aligned}$$

(3)

where ${\mathbf{c}}_{/\mathbf{x}}=\mathbf{A}\in {\mathbb{R}}^{m\times n}$ is the partial derivative of the constraint equations $\mathbf {c}$ with respect to the coordinates $\mathbf {x}$, namely the constraint Jacobian matrix, which is expected to be full-rank thanks to the previously assumed independence of the constraints, and $\mathit{\lambda }\in {\mathbb{R}}^m$ is the vector of the corresponding Lagrange multipliers.

The rank of the constraint Jacobian matrix could reduce in case singular configurations are reached. This condition would be critical irrespective of the formulation in use, and thus it is not specific to the present discussion. For this reason, it is not explicitly treated.

2.2 Minimal coordinate set approach

The minimal coordinate set approach consists in defining a suitable subspace $\mathbf{T}\in {\mathbb{R}}^{n\times (n-m)}$ of the space spanned by the coordinates $\mathbf {x}$ that is tangent to the constraint manifold, namely ${\mathbf{T}}^T{\mathbf{A}}^T\equiv \mathbf{0}\in {\mathbb{R}}^{(n-m)\times m}$, such that

$$\begin{aligned} \dot{\mathbf {x}} &= {\mathbf {T}} \dot{\mathbf {q}} + \mathrm {\boldsymbol {\beta}}' \end{aligned}$$

(4a)

$$\begin{aligned} \ddot{\mathbf {x}} &= {\mathbf {T}} \ddot{\mathbf {q}} + \mathrm {\boldsymbol {\beta}}'', \end{aligned}$$

(4b)

where $\mathbf{q}\in {\mathbb{R}}^{n-m}$ are local, truly independent coordinates, with $\mathrm {\boldsymbol {\beta}}'$ and $\mathrm {\boldsymbol {\beta}}''$ defined accordingly, the former being nonzero only in case of rheonomous constraints such that

$${\mathbf{c}}_{/\mathbf{x}}\dot{\mathbf{x}}+{\mathbf{c}}_{/t}=\mathbf{0}\to {\mathbf{c}}_{/\mathbf{x}}(\mathbf{T}\dot{\mathbf{q}}+{\mathit{\beta }}^{\prime })+{\mathbf{c}}_{/t}=\mathbf{0}\to {\mathbf{c}}_{/\mathbf{x}}{\mathit{\beta }}^{\prime }=-{\mathbf{c}}_{/t}$$

(5)

as $\mathbf {c}_{/\mathbf {x}} {\mathbf {T}} = {\mathbf {A}} {\mathbf {T}} \equiv {\mathbf {0}}$ holds, and analogously

$${\mathbf{c}}_{/\mathbf{x}}\ddot{\mathbf{x}}+{\left(\dot{\mathbf{c}}\right)}_{/\mathbf{x}}\dot{\mathbf{x}}+{\left(\dot{\mathbf{c}}\right)}_{/t}=\mathbf{0}\to {\mathbf{c}}_{/\mathbf{x}}{\mathit{\beta }}^{″}=-{\left(\dot{\mathbf{c}}\right)}_{/\mathbf{x}}\dot{\mathbf{x}}-{\left(\dot{\mathbf{c}}\right)}_{/t}.$$

(6)

Suitable expressions of $\mathrm {\boldsymbol {\beta}}'$ and $\mathrm {\boldsymbol {\beta}}''$ are determined later.

The constrained dynamics problem, projected in such a subspace, yields

(7)

The solution is sought by first integrating Eq. (7) to obtain the generalized velocities $\dot{\mathbf {q}}$; then, Eq. (4a) is integrated to obtain an estimate of $\mathbf {x}$, which needs to be subsequently refined by enforcing the constraint at the position level, Eq. (1).

The cancellation of the term ${\mathbf {T}}^{T} {\mathbf {A}}^{T}$ in Eq. (7) is guaranteed by the constraint ideality assumption. When this is not the case, e.g., in the presence of friction, the Lagrange multipliers need to be computed from local equilibria. This problem is common to all MCS formulations and not specific to the present discussion.

2.3 QR factorization for optimal minimal coordinate set selection

Among the several approaches proposed in the literature [8], a suitable choice for matrix ${\mathbf {T}}$ is obtained through the QR factorization [9] of the transpose of the constraint Jacobian matrix

$$\begin{aligned} {\mathbf {A}}^{T} &= {\mathbf {Q}} {\mathbf {R}} = \left [ \textstyle\begin{array}{ c@{\quad}c } {\mathbf {Q}}_{1} &{\mathbf {Q}}_{2} \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} {\mathbf {R}}_{1} \\ {\mathbf {0}} \end{array}\displaystyle \right ] = {\mathbf {Q}}_{1} {\mathbf {R}}_{1}, \end{aligned}$$

(8)

where matrix $\mathbf{Q}\in {\mathbb{R}}^{n\times n}$ is orthogonal and submatrix ${\mathbf{R}}_1\in {\mathbb{R}}^{m\times m}$ is upper triangular. Submatrix ${\mathbf{Q}}_2\in {\mathbb{R}}^{n\times (n-m)}$, although not uniquely determined, represents an optimal choice for ${\mathbf {T}}$, as discussed in the following.

The velocities can then be expressed as

$$\begin{aligned} \dot{\mathbf {x}} &= {\mathbf {Q}}_{2} \dot{\mathbf {q}} + {\mathbf {Q}}_{1} \mathbf {p}' \end{aligned}$$

(9)

with ${\mathbf {Q}}_{1} \mathbf {p}' = \mathrm {\boldsymbol {\beta}}'$, such that

$$\begin{aligned} \mathbf {0} &= {\mathbf {A}} \dot{\mathbf {x}} + \mathbf {c}_{/t} \\ & = {\mathbf {R}}_{1}^{T} {\mathbf {Q}}_{1}^{T} \left ( {\mathbf {Q}}_{2} \dot{\mathbf {q}} + {\mathbf {Q}}_{1} \mathbf {p}' \right ) + \mathbf {c}_{/t} \\ & = {\mathbf {R}}_{1}^{T} \mathbf {p}' + \mathbf {c}_{/t}, \end{aligned}$$

(10)

which yields

$$\begin{aligned} \mathbf {p}' = - {\mathbf {R}}_{1}^{-T} \mathbf {c}_{/t}, \end{aligned}$$

(11)

whereas the accelerations can be expressed as

$$\begin{aligned} \ddot{\mathbf {x}} &= {\mathbf {Q}}_{2} \ddot{\mathbf {q}} + {\mathbf {Q}}_{1} \mathbf {p}'' \end{aligned}$$

(12)

with ${\mathbf {Q}}_{1} \mathbf {p}'' = \mathrm {\boldsymbol {\beta}}''$, such that

$$\begin{aligned} \mathbf {0} &= {\mathbf {A}} \ddot{\mathbf {x}} + \left ( \dot{\mathbf {c}} \right ) _{/\mathbf {x}} \dot{\mathbf {x}} + \left ( \dot{\mathbf {c}} \right ) _{/t} \\ & = {\mathbf {R}}_{1}^{T} {\mathbf {Q}}_{1}^{T} \left ( {\mathbf {Q}}_{2} \ddot{\mathbf {q}} + {\mathbf {Q}}_{1} \mathbf {p}'' \right ) + \left ( \dot{\mathbf {c}} \right ) _{/\mathbf {x}} \dot{\mathbf {x}} + \left ( \dot{\mathbf {c}} \right ) _{/t} \\ & = {\mathbf {R}}_{1}^{T} \mathbf {p}'' + \left ( \dot{\mathbf {c}} \right ) _{/\mathbf {x}} \dot{\mathbf {x}} + \left ( \dot{\mathbf {c}} \right ) _{/t}, \end{aligned}$$

(13)

which yields

$$\begin{aligned} \mathbf {p}'' = - {\mathbf {R}}_{1}^{-T} \left [ \left ( \dot{\mathbf {c}} \right ) _{/ \mathbf {x}} \dot{\mathbf {x}} + \left ( \dot{\mathbf {c}} \right ) _{/t} \right ] . \end{aligned}$$

(14)

Notice that the inversion of matrix ${\mathbf {R}}_{1}$ is straightforward since it is triangular and nonsingular when the constraints are independent; thus, only a back substitution is needed.

According to Eqs. (7) and (9), the problem becomes

$$\begin{aligned} {\mathbf {Q}}_{2}^{T} {\mathbf {M}} {\mathbf {Q}}_{2} \ddot{\mathbf {q}} &= {\mathbf {Q}}_{2}^{T} \left ( \mathbf {f} - {\mathbf {M}} \mathrm{\boldsymbol{\beta }}'' \right ) \end{aligned}$$

(15a)

$$\begin{aligned} \dot{\mathbf {x}} &= {\mathbf {Q}}_{2} \dot{\mathbf {q}} + {\mathbf {Q}}_{1} \mathbf {p}', \end{aligned}$$

(15b)

a form that resembles the one originally devised by Maggi [10, 11] and subsequently reproposed by Kane [12], nowadays known as Maggi–Kane equations.

Its integration from time $t_{k}$ to $t_{k+1}$ yields

$$\begin{aligned} \mathbf {x}_{k+1}^{(0)} &= {\mathbf {Q}}_{2_{k}} \mathbf {q}_{k+1} + {\mathbf {Q}}_{1_{k}} \mathbf {p}^{(0)}, \end{aligned}$$

(16)

where the superscript $(\cdot )^{(0)}$ indicates an estimate of the final value, pending verification that it complies with the constraint of Eq. (1). The final value of the unknown $\mathbf {p}$ results from the iterative solution of

$$\begin{aligned} \mathbf {c}(\mathbf {x}_{k+1}, t_{k+1}) &= \mathbf {0}, \end{aligned}$$

(17)

namely

$$\begin{aligned} \mathbf {c} \left ( \mathbf {x}_{k+1}^{(i)}, t_{k+1} \right ) + \left ( \mathbf {c}_{/\mathbf {x}} \right ) _{k+1}^{(i)} {\mathbf {Q}}_{1_{k}} \Delta \mathbf {p} &= \mathbf {0}, \end{aligned}$$

(18)

i.e.,

$$\begin{aligned} \Delta \mathbf {p} &= - \left ( \left ( \mathbf {c}_{/\mathbf {x}} \right ) _{k+1}^{(i)} \mathbf {Q}_{1_{k}} \right ) ^{-1} \mathbf {c} \left ( \mathbf {x}_{k+1}^{(i)}, t_{k+1} \right ) \end{aligned}$$

(19a)

$$\begin{aligned} \mathbf {p} &\text{ $+${}$=$ } \Delta \mathbf {p}, \end{aligned}$$

(19b)

where $\left ( \mathbf {c}_{/\mathbf {x}} \right ) _{k+1}^{(i)}$ is the constraint Jacobian matrix at time $t_{k+1}$ during the $i$th constraint enforcement iteration, evaluated as a function of $\mathbf {x}_{k+1}^{(i)}$. Operator $\text{ $+${}$=$ }$ signifies the incremental update of the left-hand side by way of the right-hand side.

2.4 Other choices for subspace selection

Other methods have been conceived to determine a suitable subspace of the unconstrained coordinates space that is intrinsically tangent to the constraint manifold. Two of them are reported here for completeness. The continuation algorithm proposed in Sect. 2.5 could be easily adapted to them.

Singular value decomposition (SVD)

The singular value decomposition [13] decomposes a matrix ${\mathbf {A}}^{T}$ into three matrices:

$$\begin{aligned} {\mathbf {A}}^{T} &= {\mathbf {U}} \mathrm {\boldsymbol {\Sigma}} {\mathbf {V}}^{T} = \left [ \textstyle\begin{array}{ c@{\quad}c } {\mathbf {U}}_{1} &{\mathbf {U}}_{2} \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} \mathrm{\boldsymbol{\Sigma }}_{1} \\ {\mathbf {0}} \end{array}\displaystyle \right ] {\mathbf {V}}^{T} = {\mathbf {U}}_{1} \mathrm {\boldsymbol {\Sigma}}_{1} {\mathbf {V}}^{T}, \end{aligned}$$

(20)

where matrices ${\mathbf {U}}$ and ${\mathbf {V}}$ are orthonormal (namely ${\mathbf {U}}^{T} {\mathbf {U}} \equiv {\mathbf {I}}$, ${\mathbf {V}}^{T} {\mathbf {V}} \equiv {\mathbf {I}}$), whereas submatrix $\mathrm {\boldsymbol {\Sigma}}_{1}$ is square and diagonal and contains the singular values of ${\mathbf {A}}$ on the main diagonal. Submatrix ${\mathbf {U}}_{2}$ plays the role of the projection matrix ${\mathbf {T}}$, as originally presented in [14].

Zero-eigenvalue theorem

Consider the spectral representation of matrix ${\mathbf {A}}^{T} {\mathbf {A}}$,

$$\begin{aligned} {\mathbf {A}}^{T} {\mathbf {A}} &= {\mathbf {U}} \mathrm {\boldsymbol {\Lambda}} {\mathbf {U}}^{T} = \left [ \textstyle\begin{array}{ c@{\quad}c } {\mathbf {U}}_{1} &{\mathbf {U}}_{2} \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{ c@{\quad}c } \mathrm{\boldsymbol{\Lambda }}_{1} &{\mathbf {0}} \\ {\mathbf {0}} &{\mathbf {0}} \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} {\mathbf {U}}_{1}^{T} \\ {\mathbf {U}}_{2}^{T} \end{array}\displaystyle \right ] , \end{aligned}$$

(21)

where the eigenvalue and eigenvector matrices ${\mathbf {U}}$ and $\mathrm {\boldsymbol {\Lambda}}$ have been partitioned to isolate the blocks related to zero-valued eigenvalues from those of the nonzero ones. Submatrix ${\mathbf {U}}_{2}$ plays the role of the projection matrix ${\mathbf {T}}$ [15].

2.5 Tangent subspace selection and continuation

Submatrices ${\mathbf {Q}}_{1}$ and ${\mathbf {R}}_{1}$ are uniquely determined¹ once matrix ${\mathbf {A}}$ is known. Submatrix ${\mathbf {Q}}_{2}$, instead, is only subjected to matrix ${\mathbf {Q}}$’s general constraint of being orthogonal, namely ${\mathbf{Q}}_2^T{\mathbf{Q}}_2\equiv \mathbf{I}\in {\mathbb{R}}^{(n-m)\times (n-m)}$ and ${\mathbf{Q}}_2^T{\mathbf{Q}}_1\equiv \mathbf{0}\in {\mathbb{R}}^{(n-m)\times m}$, but otherwise undefined; the dashed lines originating from the solution at point $O$ in Fig. 1(a) represent a possible, arbitrary choice of the directions represented by the columns of submatrix ${\mathbf {Q}}_{2}$ in a 3D point mass pendulum problem. Specifically, it is defined in excess of post-multiplication by an arbitrary orthogonal matrix, $\mathbf{P}\in {\mathbb{R}}^{(n-m)\times (n-m)}$: $\tilde{{\mathbf {Q}}}_{2} = {\mathbf {Q}}_{2} {\mathbf {P}}$ also complies with the orthogonality requirement since $\tilde{{\mathbf {Q}}}_{2}^{T} \tilde{{\mathbf {Q}}}_{2} = {\mathbf {P}}^{T} {\mathbf {Q}}_{2}^{T} {\mathbf {Q}}_{2} {\mathbf {P}} = {\mathbf {P}}^{T} {\mathbf {P}} \equiv {\mathbf {I}}$ and $\tilde{{\mathbf {Q}}}_{2}^{T} {\mathbf {Q}}_{1} = {\mathbf {P}}^{T} {\mathbf {Q}}_{2}^{T} {\mathbf {Q}}_{1} = {\mathbf {P}}^{T} {\mathbf {0}} = {\mathbf {0}}$.

Fig. 1

Subspace selection and continuation process description

In fact, the QR factorization produces a “local” representation of the constraint Jacobian matrix; as such, the generalized coordinates associated with the subspace ${\mathbf {T}} = {\mathbf {Q}}_{2}$, which do not have any specific physical meaning, represent a local reparameterization of the subspace of the coordinates that is tangent to the constraint manifold; for example, local axes $x$ and $y$ originating from point $O$ in Fig. 1(b). When the QR factorization is computed at different time steps $t_{k}$, if $n - m > 1$, then the columns of the resulting ${\mathbf {Q}}_{2_{k}}$ are completely unrelated, their resulting value being solely dictated by the internal intricacies of the QR factorization algorithm.

This work proposes a simple and intuitive algorithm that tracks the evolution of the subspace spanned by ${\mathbf {Q}}_{2}$ using a form of differential “continuation” to preserve some sort of spatial continuity of the generalized coordinates $\mathbf {q}$ by minimizing the amount of deviation of the subspace that is intrinsically required to maintain ${\mathbf {Q}}_{2}$ tangent to the constraint manifold across time steps; for example, the axes originating from point $O'$ after the solution moved there from point $O$ in Fig. 1(c), without altering the quality of the solution.

Consider the time derivative of the transpose of the constraint Jacobian matrix in its QR factorized form:

$$\begin{aligned} \dot{{\mathbf {A}}}^{T} &= \dot{{\mathbf {Q}}} {\mathbf {R}} + {\mathbf {Q}} \dot{{\mathbf {R}}}. \end{aligned}$$

(22)

The derivative of matrix ${\mathbf {Q}}$ may be expressed as $\dot{{\mathbf {Q}}} = {\mathbf {Q}} \mathrm {\boldsymbol {\Omega}}$, where the skew-symmetric nature of matrix $\mathbf{\Omega }\in {\mathbb{R}}^{n\times n}$, namely $\mathrm {\boldsymbol {\Omega}}^{T} = - \mathrm {\boldsymbol {\Omega}}$, descends from the orthogonality of matrix ${\mathbf {Q}}$:

$$\frac{\mathrm{d}}{\mathrm{d}t}\left({\mathbf{Q}}^T\mathbf{Q}\right)={\dot{\mathbf{Q}}}^T\mathbf{Q}+{\mathbf{Q}}^T\dot{\mathbf{Q}}={\left({\mathbf{Q}}^T\dot{\mathbf{Q}}\right)}^T+{\mathbf{Q}}^T\dot{\mathbf{Q}}=\mathbf{0}\to {\mathbf{Q}}^T\dot{\mathbf{Q}}=\mathbf{\Omega }.$$

(23)

When the problem is integrated numerically, the solution from time step $t_{k}$ to time step $t_{k+1}$ is computed. The QR factorization at time $t_{k}$ yields submatrices ${\mathbf {Q}}_{1_{k}}$ and ${\mathbf {R}}_{1_{k}}$. The generalized velocities at time $t_{k}$ are computed with reference to the subspace spanned by ${\mathbf {Q}}_{2_{k}}$. After computing the solution at the new time step, the Jacobian matrix at time $t_{k+1}$, ${\mathbf {A}}_{k+1}$, is known. As such, through the economy QR factorization of its transpose, submatrices ${\mathbf {Q}}_{1_{k+1}}$ and ${\mathbf {R}}_{1_{k+1}}$ are determined. Instead of computing also submatrix ${\mathbf {Q}}_{2_{k+1}}$ through the full QR factorization, the proposed continuation algorithm is used as illustrated in the following. Consider

$$\begin{aligned} {\mathbf {Q}}_{1}^{T} \dot{{\mathbf {A}}}^{T} {\mathbf {R}}_{1}^{-1} &= {\mathbf {Q}}_{1}^{T} \dot{{\mathbf {Q}}}_{1} + \dot{{\mathbf {R}}}_{1} {\mathbf {R}}_{1}^{-1}. \end{aligned}$$

(24)

Matrix $\dot{{\mathbf {R}}}_{1} {\mathbf {R}}_{1}^{-1}$ is the product of two upper triangular matrices, thus it is itself an upper triangular matrix. Matrix ${\mathbf{Q}}_1^T{\dot{\mathbf{Q}}}_1={\mathbf{\Omega }}_1\in {\mathbb{R}}^{m\times m}$ is skew-symmetric for the orthogonality of matrix ${\mathbf {Q}}_{1}$; it can be seen as $\mathrm {\boldsymbol {\Omega}}_{1} = \mathrm {\boldsymbol {\Omega}}_{1_{L}} - \mathrm {\boldsymbol {\Omega}}_{1_{L}}^{T}$, where $\mathrm {\boldsymbol {\Omega}}_{1_{L}} = \mathrm{stril}(\mathrm {\boldsymbol {\Omega}}_{1})$ is the strictly lower triangular part of matrix $\mathrm {\boldsymbol {\Omega}}_{1}$, which can be obtained as

$$\begin{aligned} \mathrm {\boldsymbol {\Omega}}_{1_{L}} &= \mathrm{stril} \left ( {\mathbf {Q}}_{1}^{T} \dot{{\mathbf {A}}}^{T} {\mathbf {R}}_{1}^{-1} \right ) \end{aligned}$$

(25)

since $\mathrm{stril} \left ( \dot{{\mathbf {R}}}_{1} {\mathbf {R}}_{1}^{-1} \right ) \equiv {\mathbf {0}}$ by definition, $\dot{{\mathbf {R}}}_{1} {\mathbf {R}}_{1}^{-1}$ being upper triangular. From Eq. (23), one can show that the derivative of matrix ${\mathbf {Q}}$,

(26)

is entirely known (the top right block of the rightmost matrix $\mathrm {\boldsymbol {\Omega}}$ contains matrix $- \mathrm {\boldsymbol {\Omega}}_{21}^{T}$ instead of matrix $\mathrm {\boldsymbol {\Omega}}_{12}$ since $\mathrm {\boldsymbol {\Omega}}_{12} \equiv - \mathrm {\boldsymbol {\Omega}}_{21}^{T}$ owing to the skew-symmetry of $\mathrm {\boldsymbol {\Omega}}$). In fact, the bottom right block of matrix $\mathrm {\boldsymbol {\Omega}}$ should contain an unknown skew-symmetric contribution ${\mathbf{Q}}_2^T{\dot{\mathbf{Q}}}_2={\mathbf{\Omega }}_2\in {\mathbb{R}}^{(n-m)\times (n-m)}$. Such a matrix is arbitrarily set to zero, which corresponds to requiring that the subspace ${\mathbf {Q}}_{2}$ is modified as little as possible; specifically,

$$\begin{aligned} \dot{{\mathbf {Q}}}_{2} &= -{\mathbf {Q}}_{1} \mathrm {\boldsymbol {\Omega}}_{21}^{T} = - {\mathbf {A}}^{+} \dot{{\mathbf {A}}} {\mathbf {Q}}_{2}. \end{aligned}$$

(27)

$\mathrm {\boldsymbol {\Omega}}_{2}$ may be interpreted as the angular velocity of subspace ${\mathbf {Q}}_{2}$, the rate of reorientation with respect to itself, whereas $\mathrm {\boldsymbol {\Omega}}_{21}$ expresses the rate of reorientation with respect to ${\mathbf {Q}}_{1}$ that is required to remain orthogonal to it during its evolution.

Thus, the subspace ${\mathbf {Q}}_{2}$ can be integrated starting from an arbitrary value, provided it is orthogonal to the initial value of ${\mathbf {Q}}_{1}$ (the value resulting from Matlab’s implementation of the QR factorization was used in the numerical examples of Sect. 3), taking appropriate measures (e.g., using Munthe–Kaas’ method [16]) to guarantee that the resulting matrix ${\mathbf {Q}}$ preserves orthogonality and submatrix ${\mathbf {Q}}_{1}$ matches that resulting from the factorization of the transpose of the constraint Jacobian matrix. For example, for $\mathrm {\boldsymbol {\Omega}}$ constant across a time step of duration $t_{k+1} - t_{k} = h$,

$$\begin{aligned} {\mathbf {Q}}_{k+1} &= {\mathbf {Q}}_{k} \mathrm{e}^{\mathrm {\boldsymbol {\Omega}} h} \end{aligned}$$

(28)

or

$$\begin{aligned} {\mathbf {Q}}_{2_{k+1}} = \mathrm{e}^{- {\mathbf {A}}^{+} \dot{{\mathbf {A}}} h} {\mathbf {Q}}_{2_{k}}, \end{aligned}$$

(29)

the latter being only a first-order approximation of the former since the intrinsic skew-symmetric structure of the exponent matrix $\mathrm {\boldsymbol {\Omega}}$ is lost.

Submatrix ${\mathbf {Q}}_{2_{k+1}}$ resulting from the proposed integration, e.g., from Eq. (28), may need to be corrected to guarantee its orthogonality with respect to submatrix ${\mathbf {Q}}_{1_{k+1}}$ obtained from the economy QR factorization of ${\mathbf {A}}_{k+1}^{T}$; a Gram–Schmidt reorthogonalization [9] may be used.

3 Results

Simple examples are analyzed to illustrate how the proposed method produces a more regular and intuitive choice of the projection subspace during the integration of the solution.

Single-degree-of-freedom problems represent a trivial case in the context of the present discussion; in fact, the corresponding submatrix ${\mathbf {Q}}_{2}$ consists of a single column, which is thus exactly determined, except for its sign.

3.1 Single-degree-of-freedom problems: planar pendulum

A simple, single-degree-of-freedom example is considered to exemplify how the continuation algorithm avoids the occasional reversing of the sign of ${\mathbf {Q}}_{2}$ that occurs when the QR factorization of matrix ${\mathbf {A}}^{T}$ is blindly performed.

Consider the equations of motion of a simple point mass pendulum of mass $M$ and length $\ell$, subjected to a uniform gravity field $g$ along the negative $z$ direction:

$$\begin{aligned} M \ddot{x} + 2 x \lambda &= 0 \end{aligned}$$

(30a)

$$\begin{aligned} M \ddot{z} + 2 z \lambda + M g &= 0 \end{aligned}$$

(30b)

$$\begin{aligned} x^{2} + z^{2} - \ell ^{2} &= 0, \end{aligned}$$

(30c)

where $x$ and $z$ are the horizontal and vertical components of the point mass position, the unconstrained coordinates in the present context, and $\lambda$ is the Lagrange multiplier associated with the constraint of Eq. (30c). The corresponding constraint Jacobian matrix is

$$\begin{aligned} {\mathbf {A}} &= \left [ \textstyle\begin{array}{ c@{\quad}c } 2 x &2 z \end{array}\displaystyle \right ] . \end{aligned}$$

(31)

The QR factorization of its transpose can be written as

$$\begin{aligned} {\mathbf {A}}^{T} &= \left [ \textstyle\begin{array}{c} 2 x \\ 2 z \end{array}\displaystyle \right ] = \left [ \textstyle\begin{array}{ c@{\ }|@{\ }c } Q_{11} &Q_{12} \\ Q_{21} &Q_{22} \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} R_{1} \\ \hline 0 \end{array}\displaystyle \right ] = \left [ \textstyle\begin{array}{ c@{\ }|@{\ }c } -\cos \theta &-\sin \theta \\ \sin \theta &-\cos \theta \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} R_{1} \\ \hline 0 \end{array}\displaystyle \right ] , \end{aligned}$$

(32)

where matrix ${\mathbf {Q}}$ is structurally orthonormal and defined by the single parameter $\theta$, with

$$\theta =-{tan}^{-1}\left(\frac{z}{x}\right),R_1=2\ell$$

(33)

as a possible determination of the parameters $\theta$ and $R_{1}$. Submatrices ${\mathbf {Q}}_{1}$ and ${\mathbf {Q}}_{2}$ can then be written as

$${\mathbf{Q}}_1=\left[\begin{array}{c}x/\ell \\ z/\ell \end{array}\right],{\mathbf{Q}}_2=\left[\begin{array}{c}-z/\ell \\ x/\ell \end{array}\right].$$

(34)

The velocity and acceleration vectors of the point mass are thus

$$\left\{\begin{array}{c}\dot{x}\\ \dot{z}\end{array}\right\}=\left[\begin{array}{c}-z/\ell \\ x/\ell \end{array}\right]\dot{q}\left\{\begin{array}{c}\ddot{x}\\ \ddot{z}\end{array}\right\}=\left[\begin{array}{c}-z/\ell \\ x/\ell \end{array}\right]\ddot{q}-\left[\begin{array}{c}x/\ell \\ z/\ell \end{array}\right]\frac{{\dot{q}}^2}{\ell }.$$

(35)

The projected equation of motion is

$$\begin{aligned} M \ddot{q} &= - \cfrac{x}{\ell} M g. \end{aligned}$$

(36)

One may observe that redefining $\dot{q} = \ell \dot{\theta}$ the velocity vector can be written as

$$\begin{aligned} \left \{ \textstyle\begin{array}{c} \dot{x} \\ \dot{z} \end{array}\displaystyle \right \} &= - \ell \left [ \textstyle\begin{array}{c} \sin \theta \\ \cos \theta \end{array}\displaystyle \right ] \dot{\theta}, \end{aligned}$$

(37)

which is integrable, yielding

$$\begin{aligned} \left \{ \textstyle\begin{array}{c} x \\ z \end{array}\displaystyle \right \} &= \ell \left [ \textstyle\begin{array}{c} \cos \theta \\ -\sin \theta \end{array}\displaystyle \right ] . \end{aligned}$$

(38)

It is worth recalling that when solving for $\theta$ and $R_{1}$ from Eq. (32), ${\mathbf {A}}^{T} = {\mathbf {Q}} {\mathbf {R}}$ yields

$$\begin{aligned} \left [ \textstyle\begin{array}{c} 2 x \\ 2 z \end{array}\displaystyle \right ] &= \left [ \textstyle\begin{array}{c} -R_{1} \cos \theta \\ R_{1} \sin \theta \end{array}\displaystyle \right ] . \end{aligned}$$

(39)

To extract $R_{1}$, consider the norm of both sides

$$4(x^2+y^2)=R_1^2({cos}^2\theta +{sin}^2\theta )\to 4{\ell }^2=R_1^2$$

(40)

or

$$\begin{aligned} R_{1} &= \pm 2 \ell , \end{aligned}$$

(41)

where we arbitrarily chose $R_{1} = + 2 \ell$, but the choice with the negative sign is also legitimate. To extract $\theta$, consider the ratio of the second and first elements:

$$\frac{2z}{2x}=-\frac{R_{1}sin\theta }{R_{1}cos\theta }\to \theta =-{tan}^{-1}\left(\frac{z}{x}\right)+\frac{\pi }{2}(1-\mathrm{sign}(R_1)).$$

(42)

The culprit lies in the fact that in Eq. (32) matrix ${\mathbf {Q}}$ could have been alternatively defined as

$$\begin{aligned} {\mathbf {Q}} &= \left [ \textstyle\begin{array}{ c@{\ }|@{\ }c } -\cos \theta &\sin \theta \\ \sin \theta &\cos \theta \end{array}\displaystyle \right ] , \end{aligned}$$

(43)

i.e., changing the sign of its second column, submatrix ${\mathbf {Q}}_{2}$, at the only cost of no longer representing a rotation matrix (since its determinant would now be −1 instead of $+1$), but without impacting its ability to represent the transpose of the constraint Jacobian matrix as ${\mathbf {Q}}_{1} {\mathbf {R}}_{1}$, nor that of ${\mathbf {Q}}_{2}$ to represent a suitable subspace tangent to the constraint manifold.

If an intermittent change of sign occurs between consecutive QR factorizations, the sign of the derivative of the generalized coordinate $\dot{q}$ also changes, resulting in unnecessary discontinuities, although harmless for what concerns the result in terms of physical variables. Consider $s_{1} = \pm 1$ as the (arbitrary) sign of $R_{1}$ and $s_{2} = \pm 1$ as the (arbitrary, independent from $s_{1}$) sign of submatrix ${\mathbf {Q}}_{2}$ such that $s_{1}^{2} = s_{2}^{2} = 1$. The QR factorization of ${\mathbf {A}}^{T}$ then becomes

$$\begin{aligned} \left [ \textstyle\begin{array}{c} 2x \\ 2z \end{array}\displaystyle \right ] &= \left [ \textstyle\begin{array}{ c@{\ }|@{\ }c } -s_{1} \cos \theta &-s_{2} \sin \theta \\ s_{1} \sin \theta &-s_{2} \cos \theta \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} s_{1} 2 \ell \\ \hline 0 \end{array}\displaystyle \right ] . \end{aligned}$$

(44)

In this case, the proposed continuation algorithm yields the “angular velocity” submatrix

$$\begin{aligned} \mathrm {\boldsymbol {\Omega}}_{21} &= {\mathbf {Q}}_{2}^{T} \dot{{\mathbf {A}}}^{T} {\mathbf {R}}_{1}^{-1} = s_{2} \left [ \textstyle\begin{array}{ c@{\quad}c } -\sin \theta &-\cos \theta \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} 2 \dot{x} \\ 2 \dot{z} \end{array}\displaystyle \right ] \cfrac{s_{1}}{2 \ell}. \end{aligned}$$

(45)

Considering, from Eq. (37),

$$\begin{aligned} \left \{ \textstyle\begin{array}{c} \dot{x} \\ \dot{z} \end{array}\displaystyle \right \} &= - \ell s_{2} \left [ \textstyle\begin{array}{c} \sin \theta \\ \cos \theta \end{array}\displaystyle \right ] \dot{\theta}, \end{aligned}$$

(46)

one obtains

$$\begin{aligned} \mathrm {\boldsymbol {\Omega}}_{21} &= {\mathbf {Q}}_{2}^{T} \dot{{\mathbf {A}}}^{T} {\mathbf {R}}_{1}^{-1} = - s_{2} \left [ \textstyle\begin{array}{ c@{\quad}c } -\sin \theta &-\cos \theta \end{array}\displaystyle \right ] 2 \ell s_{2} \left [ \textstyle\begin{array}{c} \sin \theta \\ \cos \theta \end{array}\displaystyle \right ] \dot{\theta} \cfrac{s_{1}}{2 \ell} = s_{1} \dot{\theta}. \end{aligned}$$

(47)

As a consequence, matrix $\mathrm {\boldsymbol {\Omega}}$ from Eq. (23) by construction becomes

$$\begin{aligned} \mathrm {\boldsymbol {\Omega}} &= s_{1} \dot{\theta} \left [ \textstyle\begin{array}{ r@{\quad}r } 0 &-1 \\ 1 &0 \end{array}\displaystyle \right ] \end{aligned}$$

(48)

and the continued integration of matrix ${\mathbf {Q}}$ simply consists in choosing the sign of the new vector ${\mathbf {Q}}_{2}$ such that it forms the smallest possible angle with the previous one without any indetermination nor arbitrariness in the choice of $s_{2}$ and regardless of the choice of $s_{1}$.

3.2 Single-degree-of-freedom problems: three-dimensional slider crank

Here the proposed continuation scheme is applied to the simulation of the spatial slider-crank mechanism shown in Fig. 2. This is a rigid multibody benchmark proposed by IFToMM’s Technical Committee for Multibody Dynamics² [17] (https://www.iftomm-multibody.org/) and analyzed for example in [18]. The mechanism consists of a rigid crank AB of length 0.08 m, a connecting rod BC of length 0.3 m, and a rigid sliding block. The crank, connected to the ground by revolute joint A, can rotate freely from the initial position, corresponding to an angle $\theta = 0\text{ rad}$ with an initial angular velocity of 6 rad/s. The block is constrained to the ground by a translational joint D that allows it to slide along the $x$ axis. A spherical joint at B and a universal joint at C connect the link to the crank and the slider, respectively. A uniform gravity field of magnitude $9.81\text{ m}/\text{s}{^{2}}$ is assumed in the negative $z$ direction. No other load is applied to the system. The crank and sliding block masses are $m_{c} = 0.12\text{ kg}$ and $m_{s} = 2.0\text{ kg}$, respectively. The mass moments of inertia of the two bodies are

$$ {\mathbf{J}}_{c}=10^{-4} \left ( \textstyle\begin{array}{c@{\quad}c@{\quad}c} 1 & 0 & 0 \\ 0 & 0.1 & 0 \\ 0 & 0 & 1 \end{array}\displaystyle \right ),\qquad {\mathbf{J}}_{s}=10^{-4}\left ( \textstyle\begin{array}{c@{\quad}c@{\quad}c} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\displaystyle \right ) ; $$

the mass of the connecting rod is $m_{r} = 0.5\text{ kg}$, and its mass moments of inertia are

$$ {\mathbf{J}}_{r}=10^{-3}\left ( \textstyle\begin{array}{c@{\quad}c@{\quad}c} 4 & 0 & 0 \\ 0 & 0.4 & 0 \\ 0 & 0 & 4 \end{array}\displaystyle \right ). $$

Fig. 2

Spatial slider-crank mechanism

The problem is described by 21 coordinates, ${\mathbf{r}}_n\in {\mathbb{R}}^3$ and ${\mathbf{e}}_n\in {\mathbb{R}}^4$ ($n = 1,2,3$), and 20 constraint equations, thus possesses one degree of freedom $q_{1}$. During the simulation, a time step size $h = 0.00001\text{ s}$ is considered. The motion of the slider along the $x$ direction is compared to the result proposed by Ramin Masoudi in the mentioned website and to those obtained with the traditional implementation of the QR projection method, also with $h = 0.00001\text{ s}$, which is shown in Fig. 3. The corresponding crank angle is shown in Fig. 4. The resulting $q_{1}$ and $\dot{q}_{1}$ are compared in Fig. 5 and Fig. 6 to those obtained with the traditional implementation of the QR projection method. One can notice that the result of the traditional implementation differs from the proposed one only by occasional sign changes. Since the two algorithms were initialized in the same manner, using Matlab’s QR factorization and the same constraint Jacobian matrix, the initial value of $q_{1}$ is negative in both solutions. With the proposed implementation it remains such, whereas with the traditional one it occasionally turns positive and then again negative, as dictated by the intricacies of the blind execution of the QR factorization algorithm.

Fig. 3

Slider position

Fig. 4

Crank angle

Fig. 5

Slider crank generalized coordinate $q_{1}$

Fig. 6

Slider crank generalized velocity $\dot{q}_{1}$

3.3 Multi-degree-of-freedom problems: spatial pendulum

Consider a simple point mass spherical pendulum of mass $M$ and length $\ell$, subjected to a uniform gravity field $g = 9.81\text{ m}/\text{s}^{2}$, directed along the negative $z$ axis. Its equations of motion are

$$\begin{aligned} \left [ \textstyle\begin{array}{ c@{\quad}c@{\quad}c } M &0 &0 \\ 0 &M &0 \\ 0 &0 &M \end{array}\displaystyle \right ] \left \{ \textstyle\begin{array}{c} \ddot{x} \\ \ddot{y} \\ \ddot{z} \end{array}\displaystyle \right \} + \left [ \textstyle\begin{array}{c} 2 x \\ 2 y \\ 2 z \end{array}\displaystyle \right ] \lambda &= \left \{ \textstyle\begin{array}{c} 0 \\ 0 \\ -M g \end{array}\displaystyle \right \} \end{aligned}$$

(49a)

$$\begin{aligned} x^{2} + y^{2} + z^{2} - \ell ^{2} &= 0 \end{aligned}$$

(49b)

The unconstrained problem has three degrees of freedom $x$, $y$, and $z$, and one constraint, Eq. (49a); thus, the constrained problem has two degrees of freedom. Consequently, ${\mathbf{Q}}_1\in {\mathbb{R}}^{3\times 1}$ and ${\mathbf{Q}}_2\in {\mathbb{R}}^{3\times 2}$. The constraint Jacobian matrix and its time derivative are

$$\begin{aligned} {\mathbf {A}} &= \left [ \textstyle\begin{array}{ c@{\quad}c@{\quad}c } 2x &2y &2z \end{array}\displaystyle \right ] \end{aligned}$$

(50)

$$\begin{aligned} \dot{{\mathbf {A}}} &= \left [ \textstyle\begin{array}{ c@{\quad}c@{\quad}c } 2 \dot{x} &2 \dot{y} &2 \dot{z} \end{array}\displaystyle \right ] . \end{aligned}$$

(51)

The QR factorization of ${\mathbf {A}}^{T}$ in a given initial configuration $(x,y,z) = (x_{0},y_{0},z_{0})$ yields

$$\begin{aligned} {\mathbf {A}}^{T} &= \left [ \textstyle\begin{array}{c} 2 x_{0} \\ 2 y_{0} \\ 2 z_{0} \end{array}\displaystyle \right ] = \left [ \textstyle\begin{array}{ c@{\ }|@{\ }c@{\quad}c } x_{0}/\ell &q_{12} &q_{13} \\ y_{0}/\ell &q_{22} &q_{23} \\ z_{0}/\ell &q_{32} &q_{33} \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} 2 \ell \\ \hline 0 \\ 0 \end{array}\displaystyle \right ] = {\mathbf {Q}} {\mathbf {R}}, \end{aligned}$$

(52)

where the six coefficients $q_{ij}$, $i=1,2,3$, $j=2,3$ are related by five orthogonality conditions, leaving only one undetermined parameter.

Without loss of generality, let us assume that $(x_{0},y_{0},z_{0}) = (\ell ,0,0)$, which complies with the constraint equation; this yields

$$\begin{aligned} {\mathbf {A}}^{T} &= \left [ \textstyle\begin{array}{c} 2 \ell \\ 0 \\ 0 \end{array}\displaystyle \right ] = \left [ \textstyle\begin{array}{ c@{\ }|@{\ }c@{\quad}c } 1 &0 &0 \\ 0 &\cos \alpha _{0} &-\sin \alpha _{0} \\ 0 &\sin \alpha _{0} &\cos \alpha _{0} \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} 2 \ell \\ \hline 0 \\ 0 \end{array}\displaystyle \right ] = {\mathbf {Q}} {\mathbf {R}}, \end{aligned}$$

(53)

where $\alpha _{0}$ is an arbitrary parameter. Clearly, when $\alpha _{0} = 0$, the two vectors that span the subspace of ${\mathbf {Q}}_{2}$ are the coordinate axes $y$ and $z$.

The projected equations of motion in the initial configuration are

$$\begin{aligned} \left [ \textstyle\begin{array}{ c@{\quad}c } M &0 \\ 0 &M \end{array}\displaystyle \right ] \left \{ \textstyle\begin{array}{c} \ddot{q}_{1} \\ \ddot{q}_{2} \end{array}\displaystyle \right \} &= \left \{ \textstyle\begin{array}{c} -\sin \alpha _{0} \\ -\cos \alpha _{0} \end{array}\displaystyle \right \} M g. \end{aligned}$$

(54)

Without loss of generality, it is assumed that $(\dot{x}_{0},\dot{y}_{0},\dot{z}_{0}) = (0, v_{0}, 0)$, which complies with the derivative of the constraint equation in the initial configuration

$$\begin{aligned} 0 &= {\mathbf {A}} \dot{\mathbf {x}} = \left [ \textstyle\begin{array}{ c@{\quad}c@{\quad}c } 2 \ell &0 &0 \end{array}\displaystyle \right ] \left \{ \textstyle\begin{array}{c} 0 \\ v_{0} \\ 0 \end{array}\displaystyle \right \} ; \end{aligned}$$

(55)

in this case, one obtains

$$\begin{aligned} \mathrm {\boldsymbol {\Omega}}_{1_{L}} &= 0, \end{aligned}$$

(56)

as one would expect for a diagonal element of a skew-symmetric matrix, and

$$\begin{aligned} \mathrm {\boldsymbol {\Omega}}_{21} &= \left [ \textstyle\begin{array}{ c@{\quad}c@{\quad}c } 0 &\cos \alpha _{0} &\sin \alpha _{0} \\ 0 &-\sin \alpha _{0} &\cos \alpha _{0} \end{array}\displaystyle \right ] \left [ \textstyle\begin{array}{c} 0 \\ 2 v_{0} \\ 0 \end{array}\displaystyle \right ] \cfrac{1}{2 \ell} = \left [ \textstyle\begin{array}{c} \cos \alpha _{0} \\ -\sin \alpha _{0} \end{array}\displaystyle \right ] \cfrac{v_{0}}{\ell}, \end{aligned}$$

(57)

thus

$$\begin{aligned} \mathrm {\boldsymbol {\Omega}} &= \cfrac{v_{0}}{\ell} \left [ \textstyle\begin{array}{ c@{\quad}c@{\quad}c } 0 &-\cos \alpha _{0} &\sin \alpha _{0} \\ \cos \alpha _{0} &0 &0 \\ -\sin \alpha _{0} &0 &0 \end{array}\displaystyle \right ] \end{aligned}$$

(58)

and

$$\begin{aligned} \Delta {\mathbf {Q}} &= \left [ \textstyle\begin{array}{ c@{\quad}c@{\quad}c } \cos \Delta \theta &-\sin \Delta \theta \cos \alpha _{0} &\sin \Delta \theta \sin \alpha _{0} \\ \sin \Delta \theta \cos \alpha _{0} &\cos \Delta \theta \cos \alpha _{0}^{2} - \cos \alpha _{0}^{2} + 1 &-\sin (2 \alpha _{0}) \left ( \cos \Delta \theta - 1 \right ) / 2 \\ -\sin \Delta \theta \sin \alpha _{0} &-\sin (2 \alpha _{0}) \left ( \cos \Delta \theta - 1 \right ) / 2 &\cos \Delta \theta + \cos \alpha _{0}^{2} - \cos \Delta \theta \cos \alpha _{0}^{2} \end{array}\displaystyle \right ] \end{aligned}$$

(59)

with $\Delta \theta = v_{0} h / \ell$, which, for $\alpha _{0} = 0$, reduces to

$$\begin{aligned} \Delta {\mathbf {Q}} &= \left [ \textstyle\begin{array}{ c@{\quad}c@{\quad}c } \cos \Delta \theta &-\sin \Delta \theta &0 \\ \sin \Delta \theta &\cos \Delta \theta &0 \\ 0 &0 &1 \end{array}\displaystyle \right ] \end{aligned}$$

(60)

namely, a finite rotation about the $z$ axis by an angle $\Delta \theta$.

In the present example, the pendulum’s mass is $M = 1.0\text{ kg}$ and its length is $\ell = 0.08\text{ m}$. The initial position of the mass is $\mathbf {r}_{0} = [0.08, 0, 0]^{T}\text{ m}$, and the initial condition on the velocity is set as $v_{0} = 0.7895\text{ m}/\text{s}$.

The trajectory of the mass simulated using the explicit Runge–Kutta scheme proposed by Dormand and Prince [19] and implemented in Matlab’s ode45 function with $h = 0.001\text{ s}$ is compared to that resulting from the integration of the original DAE governing equations with the free general-purpose multibody solver MBDyn³ [20] using a second-order accurate implicit linear multistep integration method with algorithmic dissipation (asymptotic spectral radius $\rho _{\infty }= 0.6$) [21, 22] and a projection method based on the QR factorization of the transpose of the Jacobian matrix performed at each time step, without any knowledge of its evolution, using ode45 with a time step $h = 0.000005\text{ s}$ to act as a reference solution, as shown in Fig. 7. The resulting minimal set generalized coordinates $q_{1}$ and $q_{2}$ and the projected generalized velocities $\dot{q}_{1}$ and $\dot{q}_{2}$ ($\dot{q}_{i} = {\mathbf {Q}}_{2}(:, i)^{T} \dot{\mathbf {x}}$) are compared to the results obtained from what is here termed “traditional QR method” (i.e., without subspace continuation), as shown in Fig. 8 and Fig. 9, respectively. The same time step is used for integration with both the traditional and proposed coordinate selections not only for fairness of comparison, but also because the latter merely consists of a recombination of the coordinates’ subspace, without any impact on the characteristics of the equations that are integrated. One may observe that the coordinates $q_{i}$ resulting from the proposed method are much more regular than those resulting from the traditional QR factorization. Specifically, those resulting from the proposed method appear to be continuous and differentiable, whereas those resulting from the traditional QR factorization show discontinuities in their first derivatives $\dot{q}_{i}$.

Fig. 7

Trajectory of the spatial pendulum’s center of mass: traditional and proposed QR approach results are compared to those obtained from DAE integration using MBDyn

Fig. 8

Spatial pendulum minimal coordinates $q_{i}$ ($i = 1, 2$): comparison of traditional and proposed QR factorization method

Fig. 9

Spatial pendulum projected velocities $\dot{q}_{i}$ ($i = 1, 2$): comparison of traditional and proposed QR factorization method

This is well explained by the continuity and regularity of the evolution of each column of matrix ${\mathbf {Q}}_{2}$ for the proposed method, compared to the discontinuity of those resulting from the traditional QR factorization, as depicted in Fig. 10.

Fig. 10

${\mathbf {Q}}_{2}(:,i)$ ($i = 1, 2$) from the proposed (top) and the traditional QR method (bottom)

Furthermore, from Fig. 10 one can observe that for $t = 0$ the first column of matrix ${\mathbf {Q}}_{2}$ corresponds to $[0, 1, 0]^{T}$, i.e., the unit vector along the $y$ axis, whereas the second column of matrix ${\mathbf {Q}}_{2}$ corresponds to $[0, 0, 1]^{T}$, i.e., the unit vector along the $z$ axis, i.e., the QR algorithm chose $\alpha _{0} = 0$ in Eq. (53) when initializing the subspace ${\mathbf {Q}}_{2}$. Indeed, considering the initial velocity of the mass, one can observe that $\dot{q}_{1}(0) \equiv v_{0}$ and $\dot{q}_{2}(0) \equiv 0$, which is consistent with the given initial conditions.

3.4 Multi-degree-of-freedom problems: spin top

Consider an axisymmetric spin top whose tip is constrained to be at unit distance from the origin of the global coordinate system, i.e., lying on a sphere of unit radius centered in the origin. This problem was recently proposed by Haug [6]. The tip of the spin top is $1\text{ m}$ far away from its center of mass. The problem is sketched in Fig. 11. The inertia properties of the spin top are $m = 30\text{ kg}$ and $\mathbf {J} = \mathrm {diag} (90,90,30)\text{ kg}\cdot \text{ m}^{2}$. The initial position of the spin top is $\mathbf {r}_{0} = [0,-1,0]^{T}\text{ m}$. The local coordinate system $x'$-$y'$-$z'$ is initially coincident with the global coordinate system $x$-$y$-$z$. A uniform gravity field of magnitude $9.81\text{ m}/\text{s}{^{2}}$ is assumed in the negative $z$ direction. In this model, the Euler parameters $\mathbf {e}$ are used to describe the rotation of the spin top; their initial value is $\mathbf {e}_{0} = [1,0,0,0]^{T}$. The initial velocity is $\dot{\mathbf {r}}_{0} = \mathbf {0}\text{ m}/\text{s}$ and the initial angular velocity is ${\mathbf {\omega}}_{0} = [0.1, 0.1, 0.3]^{T}\text{ rad}/\text{s}$. The initial derivative of the Euler parameters $\dot{\mathbf {e}}_{0}$ is computed from $1/2\cdot {\mathbf {E}}(\mathbf {e}_{0})\mathbf {\omega}_{0}$. The constrained equations of motion of the spin top are

$$\begin{aligned} m\ddot{\mathbf {r}} +c^{T}_{\mathbf {r}}{\mathbf {\lambda}} &= \mathbf {f} \end{aligned}$$

(61a)

$$\begin{aligned} 4{\mathbf {E}}^{T}{{\mathbf {J}}}{\mathbf {E}}\ddot{\mathbf {e}} + 8\dot{{\mathbf {E}}}^{T}{{\mathbf {J}}} {\mathbf {E}}{\mathbf {\omega}} + c^{T}_{\mathbf {e}}\lambda + 2{\mathbf {e}}^{T}\mu &= 2 {\mathbf {E}}^{T}{\mathbf {t}} \end{aligned}$$

(61b)

$$\begin{aligned} c(t,\mathbf {r}, \mathbf {e}) &= 0 \end{aligned}$$

(61c)

$$\begin{aligned} \mathbf {e}^{T}\mathbf {e} - 1 &= 0, \end{aligned}$$

(61d)

where $\mathbf {f}$ and $\mathbf {t}$ are the applied force and torque at the mass center, respectively. Since only gravity is applied, $\mathbf {f} = (0,0,-9.81 m)$ and $\mathbf {t} = \mathbf {0}$ with

$$ {{\mathbf {E}}}=\left ( \textstyle\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} -e_{1} & e_{0} & e_{3} & -e_{2} \\ -e_{2} & -e_{3} & e_{0} & e_{1} \\ -e_{3} & e_{2} & -e_{1} & e_{0} \end{array}\displaystyle \right ), $$

(62)

$c$ denotes the constraint equation, ${\mathbf {\lambda}}$ and $\mathbf {\mu}$ are the Lagrange multipliers corresponding to the constraint equation $c$, Eq. (61c) and the Euler parameter normalization, Eq. (61d), respectively. The absolute position of the tip is $\mathbf {r}_{\mathrm{T}} = \mathbf {r} + {\mathbf {A}}\mathbf {u}_{0}$, where $\mathbf {u}_{0} = [0, 0, -1]^{T}\text{ m}$ and $\mathbf{A}=\bar{\mathbf{E}}\mathbf{E}^{T}$ is the orientation matrix with

$$ {\bar{{\mathbf {E}}}}=\left ( \textstyle\begin{array}{c@{\quad}c@{\quad}c@{\quad}c} -e_{1} & e_{0} & -e_{3} & e_{2} \\ -e_{2} & e_{3} & e_{0} & -e_{1} \\ -e_{3} & -e_{2} & e_{1} & e_{0} \end{array}\displaystyle \right ). $$

(62)

Fig. 11

Spin top

Therefore, the constraint equation can be expressed as

$$ c = \mathbf {r}^{T}_{\mathrm{T}}\mathbf {r}_{\mathrm{T}} - 1 = \mathbf {r}^{T}\mathbf {r} + 2 \mathbf {r}^{T}{\mathbf {A}}\mathbf {u}_{0} $$

(62)

since $\mathbf {u}_{0}^{T} \mathbf {u}_{0} \equiv 1$. The problem is described by seven coordinates, $\mathbf{r}\in {\mathbb{R}}^3$ and $\mathbf{e}\in {\mathbb{R}}^4$, and two constraint equations, Eqs. (62) and (61d), thus possesses five degrees of freedom, $q_{i}$ ($i = 1, \ldots , 5$).

The trajectory of the centroid resulting from the simulation using the previously mentioned method with $h = 0.0001\text{ s}$ is compared in Fig. 12 to those obtained by integrating the original DAE system using MBDyn and by using the traditional QR method, also with $h = 0.0001\text{ s}$. The projection motion of $q_{i}$ ($i = 1,\ldots ,5$) is compared to the results of the traditional QR method without any projection in Fig. 13, whereas that of $\dot{q}_{i}$ ($i = 1,\ldots ,5$) is compared to the results of the traditional QR method without any projection in Fig. 14. Again, one can notice the much greater regularity of the coordinates and their derivatives as they result from the proposed method.

Fig. 12

Trajectory of the spin top’s center of mass: traditional and proposed QR approach results are compared to those obtained from DAE integration using MBDyn

Fig. 13

Spin top minimal coordinates $q_{i}$ ($i = 1,\ldots ,5$): comparison of traditional and proposed QR factorization method

Fig. 14

Spin top projected velocities $\dot{q}_{i}$ ($i = 1,\ldots ,5$): comparison of traditional and proposed QR factorization method

4 Conclusions

This paper presented a continuation algorithm for the redefinition of the subspace of minimal coordinates that is tangent to the constraint manifold. It is based on the full QR factorization of the constraint Jacobian matrix to initialize the subspace through the portion of the space defined by the orthogonal matrix Q that is orthogonal to the constraint Jacobian matrix. The economy QR factorization is then used to exactly factor the subspace in which the constraint Jacobian matrix lies, while the evolution of the tangent subspace is tracked by integrating the time derivative of matrix Q, eventually re-orthogonalizing the result to eliminate possible drift from the integrated tangent subspace. Numerical examples show that the analysis results are unchanged, but the generalized velocities no longer show the discontinuities that occasionally characterize them when the tangent subspace is recomputed without considering its value at the previous time step.