7.9 Rigid Connectors

A rigid connector connects two rigid bodies denoted by \(\left(1\right)\) and \(\left(2\right)\). The connector origin (e.g., its insertion point) on rigid body \(\left(i\right)\) is located at

\[ \begin{equation} \mathbf{x}^{\left(i\right)}=\mathbf{r}^{\left(i\right)}+\boldsymbol{\Lambda}^{\left(i\right)}\cdot\mathbf{Z}^{\left(i\right)}=\mathbf{r}^{\left(i\right)}+\mathbf{z}^{\left(i\right)}\,,\label{eq:rc-origin-positions} \end{equation} \]

where \(\mathbf{z}^{\left(i\right)}\) is the connector origin position relative to the center of mass at the current time, whereas \(\mathbf{Z}^{\left(i\right)}\) is its relative position in the reference configuration, when the rotation tensor \(\boldsymbol{\Lambda}^{\left(i\right)}\) is equal to the identity tensor. The connector is exerting a reaction force \(\mathbf{f}^{\left(i\right)}\) at \(\mathbf{x}^{\left(i\right)}\) and a reaction moment \(\mathbf{m}^{\left(i\right)}\) on rigid body \(\left(i\right)\), such that \(\mathbf{f}^{\left(1\right)}+\mathbf{f}^{\left(2\right)}=\mathbf{0}\) and \(\mathbf{m}^{\left(1\right)}+\mathbf{m}^{\left(2\right)}=\mathbf{0}\).

Rigid body joints, such as spherical, revolute, prismatic, cylindrical and planar joints, are a special category of rigid connectors that have a large stiffness spring connecting the joint origins on each rigid body, in a manner that only allows relative motion along the joint translational degree(s) of freedom. Similarly, a large stiffness torsional spring connects the rigid bodies in a manner that only allows relative rotation along the joint rotational degree(s) of freedom.

Other connectors include springs, dampers, and contractile forces that may connect rigid bodies. Optionally, a joint may include a linear damper connecting its origins, and an angular damper restricting its relative rotation.

Virtual Work

The virtual work of a connector represents the work of external forces on the rigid body; it is given by

\[ \begin{equation} \delta G=\sum_{i=1}^{2}\delta\mathbf{v}^{\left(i\right)}\cdot\mathbf{f}^{\left(i\right)}+\delta\boldsymbol{\theta}^{\left(i\right)}\cdot\mathbf{m}^{\left(i\right)}\,,\label{eq:rc-virtual-work} \end{equation} \]

where \(\delta\mathbf{v}^{\left(i\right)}\) is the virtual velocity of the joint origin and \(\delta\boldsymbol{\theta}^{\left(i\right)}\) is the virtual angular velocity of rigid body \(\left(i\right)\). Using the above relations for \(\mathbf{f}^{\left(i\right)}\) and \(\mathbf{m}^{\left(i\right)}\), it reduces to

\[ \begin{equation} \delta G=\left(\delta\mathbf{v}^{\left(1\right)}-\delta\mathbf{v}^{\left(2\right)}\right)\cdot\mathbf{f}^{\left(1\right)}+\left(\delta\boldsymbol{\theta}^{\left(1\right)}-\delta\boldsymbol{\theta}^{\left(2\right)}\right)\cdot\mathbf{m}^{\left(1\right)}\,.\label{eq:rc-virtual-work-redux} \end{equation} \]

The analysis thus returns the values of the reaction force and moment acting on rigid body \(\left(1\right)\). The virtual velocities at the joint may be evaluated from \eqref{eq:rc-origin-positions} as

\[ \begin{equation} \delta\mathbf{v}^{\left(i\right)}=\delta\mathbf{r}^{\left(i\right)}-\hat{\mathbf{z}}^{\left(i\right)}\cdot\delta\boldsymbol{\theta}^{\left(i\right)}\,,\label{eq:rc-origin-virtual-displacement} \end{equation} \]

where \(\hat{\mathbf{z}}\) is the skew-symmetric tensor whose dual vector is \(\mathbf{z}\), such that \(\hat{\mathbf{z}}\cdot\mathbf{v}=\mathbf{z}\times\mathbf{v}\) for any vector \(\mathbf{v}\). Substituting this expression into \eqref{eq:rc-virtual-work-redux} allows us to express the virtual work in terms of the virtual velocities of the centers of mass and the virtual angular velocities of the rigid bodies,

\[ \begin{equation} \delta G=\left[\begin{array}{cccc} \delta\mathbf{r}^{\left(1\right)} & \delta\boldsymbol{\theta}^{\left(1\right)} & \delta\mathbf{r}^{\left(2\right)} & \delta\boldsymbol{\theta}^{\left(2\right)}\end{array}\right]\left[\begin{array}{c} \mathbf{f}^{\left(1\right)}\\ \hat{\mathbf{z}}^{\left(1\right)}\cdot\mathbf{f}^{\left(1\right)}+\mathbf{m}^{\left(1\right)}\\ -\mathbf{f}^{\left(1\right)}\\ -\hat{\mathbf{z}}^{\left(2\right)}\cdot\mathbf{f}^{\left(1\right)}-\mathbf{m}^{\left(1\right)} \end{array}\right]\,.\label{eq:rc-virtual-work-final} \end{equation} \]

When using time discretization in the interval \(\left[t_{n},t_{n+1}\right]\), the external forces and moments may be evaluated at the intermediate time point \(t_{n+\alpha}\) using

\[ \begin{aligned}\mathbf{f}_{n+\alpha}^{\left(1\right)} & =\alpha\mathbf{f}_{n+1}^{\left(1\right)}+\left(1-\alpha\right)\mathbf{f}_{n}^{\left(1\right)}\\ \left(\hat{\mathbf{z}}^{\left(1\right)}\cdot\mathbf{f}^{\left(1\right)}\right)_{n+\alpha} & =\alpha\hat{\mathbf{z}}_{n+1}^{\left(1\right)}\cdot\mathbf{f}_{n+1}^{\left(1\right)}+\left(1-\alpha\right)\hat{\mathbf{z}}_{n}^{\left(1\right)}\cdot\mathbf{f}_{n}^{\left(1\right)}\\ \mathbf{m}_{n+\alpha}^{\left(1\right)} & =\alpha\mathbf{m}_{n+1}^{\left(1\right)}+\left(1-\alpha\right)\mathbf{m}_{n}^{\left(1\right)} \end{aligned} \]

We solve for \(\delta G=0\) using Newton's method in the usual manner, by evaluating the linearization of \(\delta G\) along increments in the rigid body degrees of freedom at \(t_{n+1}\),

\[ \begin{equation} \delta G+\sum_{i=1}^{2}D\delta G\left[\Delta\mathbf{r}^{\left(i\right)}\right]+D\delta G\left[\Delta\boldsymbol{\theta}^{\left(i\right)}\right]\approx0\,.\label{eq:rc-Newton-method} \end{equation} \]

Assuming that

\[ \begin{equation} D\mathbf{f}_{n+1}^{\left(1\right)}=\left[\begin{array}{cccc} \mathbf{K}_{fr}^{\left(1\right)} & \mathbf{K}_{f\theta}^{\left(1\right)} & \mathbf{K}_{fr}^{\left(2\right)} & \mathbf{K}_{f\theta}^{\left(2\right)}\end{array}\right]\left[\begin{array}{c} \Delta\mathbf{r}^{\left(1\right)}\\ \Delta\boldsymbol{\theta}^{\left(1\right)}\\ \Delta\mathbf{r}^{\left(2\right)}\\ \Delta\boldsymbol{\theta}^{\left(2\right)} \end{array}\right]\,,\label{eq:rc-Df} \end{equation} \]

and

\[ \begin{equation} D\mathbf{m}_{n+1}^{\left(1\right)}=\left[\begin{array}{cccc} \mathbf{K}_{mr}^{\left(1\right)} & \mathbf{K}_{m\theta}^{\left(1\right)} & \mathbf{K}_{mr}^{\left(2\right)} & \mathbf{K}_{m\theta}^{\left(2\right)}\end{array}\right]\left[\begin{array}{c} \Delta\mathbf{r}^{\left(1\right)}\\ \Delta\boldsymbol{\theta}^{\left(1\right)}\\ \Delta\mathbf{r}^{\left(2\right)}\\ \Delta\boldsymbol{\theta}^{\left(2\right)} \end{array}\right]\,,\label{eq:rc-Dm} \end{equation} \]

it follows that

\[ \begin{equation} \begin{aligned}D\delta G= & \left[\begin{array}{cccc} \delta\mathbf{r}^{\left(1\right)} & \delta\boldsymbol{\theta}^{\left(1\right)} & \delta\mathbf{r}^{\left(2\right)} & \delta\boldsymbol{\theta}^{\left(2\right)}\end{array}\right]\times\\ & \alpha\left[\begin{array}{cccc} \mathbf{K}_{fr}^{\left(1\right)} & \mathbf{K}_{f\theta}^{\left(1\right)} & \mathbf{K}_{fr}^{\left(2\right)} & \mathbf{K}_{f\theta}^{\left(2\right)}\\ \hat{\mathbf{z}}_{n+1}^{\left(1\right)}\cdot\mathbf{K}_{fr}^{\left(1\right)}+\mathbf{K}_{mr}^{\left(1\right)} & \hat{\mathbf{f}}_{n+\alpha}^{\left(1\right)}\hat{\mathbf{z}}_{n+1}^{\left(1\right)}+\hat{\mathbf{z}}_{n+1}^{\left(1\right)}\cdot\mathbf{K}_{f\theta}^{\left(1\right)}+\mathbf{K}_{m\theta}^{\left(1\right)} & \hat{\mathbf{z}}_{n+1}^{\left(1\right)}\cdot\mathbf{K}_{fr}^{\left(2\right)}+\mathbf{K}_{mr}^{\left(2\right)} & \hat{\mathbf{z}}_{n+1}^{\left(1\right)}\cdot\mathbf{K}_{f\theta}^{\left(2\right)}+\mathbf{K}_{m\theta}^{\left(2\right)}\\ -\mathbf{K}_{fr}^{\left(1\right)} & -\mathbf{K}_{f\theta}^{\left(1\right)} & -\mathbf{K}_{fr}^{\left(2\right)} & -\mathbf{K}_{f\theta}^{\left(2\right)}\\ -\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\cdot\mathbf{K}_{fr}^{\left(1\right)}-\mathbf{K}_{mr}^{\left(1\right)} & -\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\cdot\mathbf{K}_{f\theta}^{\left(1\right)}-\mathbf{K}_{m\theta}^{\left(1\right)} & -\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\cdot\mathbf{K}_{fr}^{\left(2\right)}-\mathbf{K}_{mr}^{\left(2\right)} & \hat{\mathbf{-f}}_{n+\alpha}^{\left(1\right)}\hat{\mathbf{z}}_{n+1}^{\left(2\right)}-\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\cdot\mathbf{K}_{f\theta}^{\left(2\right)}-\mathbf{K}_{m\theta}^{\left(2\right)} \end{array}\right]\\ & \times\left[\begin{array}{c} \Delta\mathbf{r}^{\left(1\right)}\\ \Delta\boldsymbol{\theta}^{\left(1\right)}\\ \Delta\mathbf{r}^{\left(2\right)}\\ \Delta\boldsymbol{\theta}^{\left(2\right)} \end{array}\right] \end{aligned} \label{eq:rc-linearized-work} \end{equation} \]

It becomes immediately apparent that the stiffness matrix for a rigid connector is not symmetric. Therefore, rigid body dynamics should be analyzed using non-symmetric solvers.

Joint Axes

Joint axes are used to define the directions of degrees of freedom in rigid joints, which represent one of the major categories of rigid connectors. The axes are defined with respect to a body-based coordinate system centered at the origin of a joint, given by the orthonormal triad \(\left\{ \mathbf{e}_{1}^{\left(i\right)},\mathbf{e}_{2}^{\left(i\right)},\mathbf{e}_{3}^{\left(i\right)}\right\}\) for rigid body \(i\) (\(i=1,2\)). In the reference configuration, the bases coincide on both rigid bodies, \(\mathbf{e}_{j}^{\left(1\right)}=\mathbf{e}_{j}^{\left(2\right)}\equiv\mathbf{E}_{j}\) (\(j=1,2,3\)), where \(\mathbf{E}_{j}\) is the \(j-\)th basis vector in the reference configuration. Thus, at any time \(t\), we may evaluate the basis vectors as

\[ \begin{equation} \mathbf{e}_{j}^{\left(i\right)}=\boldsymbol{\Lambda}^{\left(i\right)}\cdot\mathbf{E}_{j}\label{eq:ja-basis-currnet-time} \end{equation} \]

Using this relation, the linearization of a basis vector along increments in the rigid body motion is given by

\[ \begin{equation} D\mathbf{e}_{j}^{\left(i\right)}=-\hat{\mathbf{e}}_{j}^{\left(i\right)}\cdot\Delta\boldsymbol{\theta}^{\left(i\right)}\,,\label{eq:ja-basis-vector-linearization} \end{equation} \]

which shows a dependence only on \(\Delta\boldsymbol{\theta}^{\left(i\right)}\).

Relative Joint Motion

The position of a joint in each rigid body \(i\) is given in \eqref{eq:rc-origin-positions}. The relative motion across a joint is given by the relative translation

\[ \begin{equation} \mathbf{x}=\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}\,,\label{eq:rjm-relative-translation} \end{equation} \]

and the relative rotation

\[ \begin{equation} \mathbf{Q}=\boldsymbol{\Lambda}^{\left(2\right)}\cdot\left(\boldsymbol{\Lambda}^{\left(1\right)}\right)^{T}\equiv\exp\left[\hat{\boldsymbol{\theta}}\right]\,,\label{eq:rjm-relative-rotation} \end{equation} \]

where \(\hat{\boldsymbol{\theta}}=-\boldsymbol{\mathcal{E}}\cdot\boldsymbol{\theta}\) is the skew-symmetric tensor with dual vector \(\boldsymbol{\theta}\). As usual, \(\boldsymbol{\theta}\) is a vector whose direction represents the axis of rotation and whose magnitude is the angle of (counter-clockwise) rotation about that axis. To report the relative motion of a joint, we may project \(\mathbf{x}\) and \(\boldsymbol{\theta}\) along the basis vectors \(\mathbf{e}_{j}^{\left(1\right)}=\boldsymbol{\Lambda}^{\left(1\right)}\cdot\mathbf{E}_{j}\) of the first rigid body. Thus,

\[ \begin{equation} \begin{aligned}x_{j} & =\mathbf{x}\cdot\mathbf{e}_{j}^{\left(1\right)}\\ \theta_{j} & =\boldsymbol{\theta}\cdot\mathbf{e}_{j}^{\left(1\right)} \end{aligned} \,.\label{eq:rjm-projected-motion} \end{equation} \]

Joint Reaction Forces and Moments

Reaction forces are used to constrain the degrees of freedom of a joint that connects two rigid bodies. Typically, these forces are generated by very stiff springs and dampers that only allow unrestricted motion along the joint degrees of freedom.

Reaction Forces from Springs

The reaction force \(\mathbf{f}^{\left(1\right)}\) generated by a spring acting on rigid body \(\left(1\right)\) is given by

\[ \begin{equation} \boxed{\mathbf{f}^{\left(1\right)}=\boldsymbol{\lambda}+\varepsilon_{c}\mathbf{P}\cdot\mathbf{g}}\,,\label{eq:jf-spring-force} \end{equation} \]

where

\[ \begin{equation} \boxed{\mathbf{g}=\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}}\label{eq:jf-gap-function} \end{equation} \]

is a gap function that represents the vector distance between the joint origins, \(\boldsymbol{\lambda}\) is the (optional) Lagrange multiplier used when invoking the augmented Lagrangian method, and \(\varepsilon_{c}\) is a penalty parameter that represents the spring stiffness. The tensor \(\mathbf{P}\) is a projection that limits the reaction force to the directions that are not free to move. In general, there are three possible options for \(\mathbf{P}\):

\[ \begin{equation} \mathbf{P}=\begin{cases} \mathbf{P}_{1}=\mathbf{I} & \text{constrain relative translation along all directions}\\ \mathbf{P}_{2}=\mathbf{I}-\mathbf{n}^{\left(1\right)}\otimes\mathbf{n}^{\left(1\right)} & \text{constrain relative translation within plane normal to }\mathbf{n}^{\left(1\right)}\\ \mathbf{P}_{3}=\mathbf{n}^{\left(1\right)}\otimes\mathbf{n}^{\left(1\right)} & \text{constrain relative translation along }\mathbf{n}^{\left(1\right)} \end{cases}\,.\label{eq:jf-P-projection} \end{equation} \]

For example, \(\mathbf{P}_{1}=\mathbf{I}\) in a spherical or revolute joint, whereas \(\mathbf{P}_{2}=\mathbf{I}-\mathbf{n}^{\left(1\right)}\otimes\mathbf{n}^{\left(1\right)}\) in an unconstrained prismatic joint, with \(\mathbf{n}^{\left(1\right)}\) representing the axis of motion; and \(\mathbf{P}_{3}=\mathbf{n}^{\left(1\right)}\otimes\mathbf{n}^{\left(1\right)}\) in an unconstrained planar joint, with \(\mathbf{n}^{\left(1\right)}\) representing the normal to the plane. Note that in all cases we choose to define the axis in the basis of rigid body \(\left(1\right)\). If the constraint is enforced properly, then \(\mathbf{n}^{\left(1\right)}\) should be the same as \(\mathbf{n}^{\left(2\right)}\), within a user-defined numerical tolerance. In practice, we let \(\mathbf{n}^{\left(1\right)}\equiv\mathbf{e}_{1}^{\left(1\right)}\).

The linearization of \(\mathbf{f}^{\left(1\right)}\) in \eqref{eq:jf-spring-force} is given by

\[ \begin{equation} D\mathbf{f}^{\left(1\right)}=\varepsilon_{c}\left(\mathbf{P}\cdot D\mathbf{g}+D\mathbf{P}\cdot\mathbf{g}\right)\,.\label{eq:jf-linearized-spring-force} \end{equation} \]

The linearization of the gap function produces

\[ \begin{equation} \mathbf{P}\cdot D\mathbf{g}=\left[\begin{array}{cccc} -\mathbf{P} & \mathbf{P}\cdot\hat{\mathbf{z}}^{\left(1\right)} & \mathbf{P} & -\mathbf{P}\cdot\hat{\mathbf{z}}^{\left(2\right)}\end{array}\right]\left[\begin{array}{c} \Delta\mathbf{r}^{\left(1\right)}\\ \Delta\boldsymbol{\theta}^{\left(1\right)}\\ \Delta\mathbf{r}^{\left(2\right)}\\ \Delta\boldsymbol{\theta}^{\left(2\right)} \end{array}\right]\,,\label{eq:jf-linearization-1st} \end{equation} \]

whereas the linearization of the three possible projections yields

\[ \begin{equation} D\mathbf{P}\cdot\mathbf{g}=\mathbf{Q}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}\,,\label{eq:jf-linearization-2nd} \end{equation} \]

where

\[ \begin{equation} \mathbf{Q}=\begin{cases} \mathbf{Q}_{1} & =\mathbf{0}\\ \mathbf{Q}_{2} & =\left[\left(\mathbf{n}^{\left(1\right)}\cdot\mathbf{g}\right)\mathbf{I}+\mathbf{n}^{\left(1\right)}\otimes\mathbf{g}\right]\cdot\hat{\mathbf{n}}^{\left(1\right)}\\ \mathbf{Q}_{3} & =-\left[\left(\mathbf{n}^{\left(1\right)}\cdot\mathbf{g}\right)\mathbf{I}+\mathbf{n}^{\left(1\right)}\otimes\mathbf{g}\right]\cdot\hat{\mathbf{n}}^{\left(1\right)} \end{cases}\,.\label{eq:jf-linearization-Q} \end{equation} \]

Therefore, in the expression for \(D\mathbf{f}^{\left(1\right)}\) in \eqref{eq:rc-Df}, we have

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{K}_{fr}^{\left(1\right)} & =-\varepsilon_{c}\mathbf{P}\\ \mathbf{K}_{f\theta}^{\left(1\right)} & =\varepsilon_{c}\left(\mathbf{P}\cdot\hat{\mathbf{z}}^{\left(1\right)}+\mathbf{Q}\right)\\ \mathbf{K}_{fr}^{\left(2\right)} & =\varepsilon_{c}\mathbf{P}\\ \mathbf{K}_{f\theta}^{\left(2\right)} & =-\varepsilon_{c}\mathbf{P}\cdot\hat{\mathbf{z}}^{\left(2\right)} \end{aligned} }\,.\label{eq:jf-spring-stiffness} \end{equation} \]

Reaction Moments from Torsional Springs

The reaction moment \(\mathbf{m}^{\left(1\right)}\) generated by a torsional spring acting on rigid body \(\left(1\right)\) is given by

\[ \begin{equation} \boxed{\mathbf{m}^{\left(1\right)}=\boldsymbol{\mu}+\varepsilon_{r}\boldsymbol{\gamma}}\label{eq:jm-joint-moment} \end{equation} \]

where

\[ \begin{equation} \boxed{\boldsymbol{\gamma}=\frac{1}{2}\sum_{j=1}^{k}\mathbf{e}_{j}^{\left(1\right)}\times\mathbf{e}_{j}^{\left(2\right)}}\label{eq:jm-angular-gap} \end{equation} \]

is the angular gap function between the bases of rigid bodies \(\left(1\right)\) and \(\left(2\right)\), \(\boldsymbol{\mu}\) is the (optional) Lagrange multiplier used when invoking the augmented Lagrangian method, and \(\varepsilon_{r}\) is a penalty parameter representing the torsional spring stiffness. We consider three cases for the choice of \(\boldsymbol{\gamma}\):

\[ \begin{equation} \boldsymbol{\gamma}=\begin{cases} k=3 & \text{constrain relative rotation along all directions}\\ k=1 & \text{maintain free relative rotation along }\mathbf{e}_{1}^{\left(i\right)}\\ k=0 & \text{maintain free relative rotation along all directions} \end{cases}\label{eq:jm-cases} \end{equation} \]

For example, \(k=3\) is used to model the reaction moment in a prismatic joint, whereas \(k=1\) is used with revolute, cylindrical and planar joints.

Now, the linearization produces

\[ D\mathbf{m}^{\left(1\right)}=\varepsilon_{r}D\boldsymbol{\gamma}\,, \]

where

\[ D\boldsymbol{\gamma}=\mathbf{W}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}-\mathbf{W}^{T}\cdot\Delta\boldsymbol{\theta}^{\left(2\right)} \]

and

\[ \begin{aligned}\mathbf{W} & =\frac{1}{2}\sum_{j=1}^{k}\hat{\mathbf{e}}_{j}^{\left(2\right)}\cdot\hat{\mathbf{e}}_{j}^{\left(1\right)}\end{aligned} \,. \]

Therefore, in the expression for \(D\mathbf{m}^{\left(1\right)}\) in \eqref{eq:rc-Dm}, we have

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{K}_{mr}^{\left(1\right)} & =\mathbf{0}\\ \mathbf{K}_{m\theta}^{\left(1\right)} & =\varepsilon_{r}\mathbf{W}\\ \mathbf{K}_{mr}^{\left(2\right)} & =\mathbf{0}\\ \mathbf{K}_{m\theta}^{\left(2\right)} & =-\varepsilon_{r}\mathbf{W}^{T} \end{aligned} }\,.\label{eq:jm-spring-stiffness} \end{equation} \]

Reaction Forces from Dampers

The reaction force \(\mathbf{f}^{\left(1\right)}\) on a damper is

\[ \begin{equation} \boxed{\mathbf{f}^{\left(1\right)}=\chi_{c}\mathbf{P}\cdot\dot{\mathbf{g}}}\,,\label{eq:df-damper-force} \end{equation} \]

where \(\dot{\mathbf{g}}\) is the time rate of change of the gap function,

\[ \begin{equation} \boxed{\dot{\mathbf{g}}=\mathbf{v}^{\left(2\right)}-\mathbf{v}^{\left(1\right)}}\,,\label{eq:df-relative-velocity} \end{equation} \]

and \(\mathbf{P}\) represents a projection as described in Section Reaction Forces from Springs. In particular, \(\mathbf{P}_{1}\) is used with spherical and revolute joints; \(\mathbf{P}_{2}\) is used with prismatic and cylindrical joints; and \(\mathbf{P}_{3}\) is used with planar joints. The parameter \(\chi_{c}\) represents the damping coefficient.

The velocities of the insertion points are given by \(\mathbf{v}^{\left(i\right)}=\dot{\mathbf{r}}^{\left(i\right)}+\boldsymbol{\omega}^{\left(i\right)}\times\mathbf{z}^{\left(i\right)}\), where \(\boldsymbol{\omega}^{\left(i\right)}\) is the rigid body angular velocity. The linearization of \(\mathbf{f}^{\left(1\right)}\) is

\[ \begin{equation} D\mathbf{f}^{\left(1\right)}=\varepsilon_{c}\left(\mathbf{P}\cdot D\dot{\mathbf{g}}+D\mathbf{P}\cdot\dot{\mathbf{g}}\right)\,,\label{eq:df-linearization} \end{equation} \]

where

\[ \begin{equation} \mathbf{P}\cdot D\dot{\mathbf{g}}=\left[\begin{array}{cccc} -\mathbf{A} & \mathbf{B}^{\left(1\right)} & \mathbf{A} & -\mathbf{B}^{\left(2\right)}\end{array}\right]\left[\begin{array}{c} \Delta\mathbf{r}^{\left(1\right)}\\ \Delta\boldsymbol{\theta}^{\left(1\right)}\\ \Delta\mathbf{r}^{\left(2\right)}\\ \Delta\boldsymbol{\theta}^{\left(2\right)} \end{array}\right]\,,\label{eq:df-linearization-1st} \end{equation} \]

with

\[ \begin{equation} \begin{aligned}\mathbf{A} & =\frac{\gamma}{\beta\Delta t}\mathbf{P}\\ \mathbf{B}^{\left(i\right)} & =\mathbf{P}\cdot\left(\frac{\gamma}{\beta\Delta t}\hat{\mathbf{z}}^{\left(i\right)}\cdot\mathbf{T}^{T}\left(\boldsymbol{\theta}^{\left(i\right)}\right)+\hat{\boldsymbol{\omega}}\cdot\hat{\mathbf{z}}^{\left(i\right)}\right) \end{aligned} \,.\label{eq:df-A-B} \end{equation} \]

Recall that \(\beta\) and \(\gamma\) are the Newmark parameters, and \(\mathbf{T}\left(\boldsymbol{\theta}\right)\) is given in (6.3-35). Similarly,

\[ \begin{equation} D\mathbf{P}\cdot\dot{\mathbf{g}}=\mathbf{V}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}\,,\label{eq:df-linearization-2nd} \end{equation} \]

where

\[ \begin{equation} \mathbf{V}=\begin{cases} \mathbf{V}_{1} & =\mathbf{0}\\ \mathbf{V}_{2} & =\left[\left(\mathbf{n}^{\left(1\right)}\cdot\dot{\mathbf{g}}\right)\mathbf{I}+\mathbf{n}^{\left(1\right)}\otimes\dot{\mathbf{g}}\right]\cdot\hat{\mathbf{n}}^{\left(1\right)}\\ \mathbf{V}_{3} & =-\left[\left(\mathbf{n}^{\left(1\right)}\cdot\dot{\mathbf{g}}\right)\mathbf{I}+\mathbf{n}^{\left(1\right)}\otimes\dot{\mathbf{g}}\right]\cdot\hat{\mathbf{n}}^{\left(1\right)} \end{cases}\,.\label{eq:df-linearization-V} \end{equation} \]

Combining these results produces

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{K}_{fr}^{\left(1\right)} & =-\chi_{c}\mathbf{A}\\ \mathbf{K}_{f\theta}^{\left(1\right)} & =\chi_{c}\left(\mathbf{B}^{\left(1\right)}+\mathbf{V}\right)\\ \mathbf{K}_{fr}^{\left(2\right)} & =\chi_{c}\mathbf{A}\\ \mathbf{K}_{f\theta}^{\left(2\right)} & =-\chi_{c}\mathbf{B}^{\left(2\right)} \end{aligned} }\label{eq:df-stiffness} \end{equation} \]

Reaction Moments from Torsional Dampers

The reaction moment \(\mathbf{m}^{\left(1\right)}\) arising from a torsional damper is

\[ \begin{equation} \boxed{\mathbf{m}^{\left(1\right)}=\chi_{r}\mathbf{P}\cdot\boldsymbol{\omega}}\,,\label{eq:td-moment} \end{equation} \]

where

\[ \begin{equation} \boldsymbol{\omega}=\boldsymbol{\omega}^{\left(2\right)}-\boldsymbol{\omega}^{\left(1\right)}\label{eq:td-relative-velocity} \end{equation} \]

is the angular velocity of body \(\left(2\right)\) relative to body \(\left(1\right)\), and \(\mathbf{P}\) is the projection used in Section Reaction Forces from Springs. In particular, \(\mathbf{P}_{1}\) is used for joints that cannot undergo relative rotations along any direction, such as prismatic joints; \(\mathbf{P}_{2}\) is used for joints that can rotate freely along a single axis, such as revolute, cylindrical and planar joints; in addition, \(\mathbf{P}=\mathbf{0}\) for spherical joints. The linearization of \(\mathbf{m}^{\left(1\right)}\) is

\[ \begin{equation} D\mathbf{m}^{\left(1\right)}=\chi_{r}\left(\mathbf{P}\cdot D\boldsymbol{\omega}+D\mathbf{P}\cdot\boldsymbol{\omega}\right)\,,\label{eq:td-linearization} \end{equation} \]

where

\[ \begin{equation} \mathbf{P}\cdot D\boldsymbol{\omega}=\mathbf{C}^{\left(2\right)}\cdot\Delta\boldsymbol{\theta}^{\left(2\right)}-\mathbf{C}^{\left(1\right)}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}\label{eq:td-linearization-1st} \end{equation} \]

with

\[ \begin{equation} \mathbf{C}^{\left(i\right)}=\frac{\gamma}{\beta\Delta t}\mathbf{P}\cdot\mathbf{T}^{T}\left(\boldsymbol{\theta}^{\left(i\right)}\right)\,,\label{eq:td-B} \end{equation} \]

and

\[ \begin{equation} D\mathbf{P}\cdot\boldsymbol{\omega}=\mathbf{W}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}\,,\label{eq:td-linearization-2nd} \end{equation} \]

where

\[ \begin{equation} \mathbf{W}=\begin{cases} \mathbf{W}_{1} & =\mathbf{0}\\ \mathbf{W}_{2} & =\left[\left(\mathbf{n}^{\left(1\right)}\cdot\boldsymbol{\omega}\right)\mathbf{I}+\mathbf{n}^{\left(1\right)}\otimes\boldsymbol{\omega}\right]\cdot\hat{\mathbf{n}}^{\left(1\right)}\\ \mathbf{W}_{3} & =-\left[\left(\mathbf{n}^{\left(1\right)}\cdot\boldsymbol{\omega}\right)\mathbf{I}+\mathbf{n}^{\left(1\right)}\otimes\boldsymbol{\omega}\right]\cdot\hat{\mathbf{n}}^{\left(1\right)} \end{cases}\,.\label{eq:td-W} \end{equation} \]

Combining these results now produces

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{K}_{mr}^{\left(1\right)} & =\mathbf{0}\\ \mathbf{K}_{m\theta}^{\left(1\right)} & =\chi_{r}\left(-\mathbf{C}^{\left(1\right)}+\mathbf{W}\right)\\ \mathbf{K}_{mr}^{\left(2\right)} & =\mathbf{0}\\ \mathbf{K}_{m\theta}^{\left(2\right)} & =\chi_{r}\mathbf{C}^{\left(2\right)} \end{aligned} }\,.\label{eq:td-stiffness} \end{equation} \]

Summary of Reaction Forces and Moment in Joints

Joint	Spherical	Revolute	Prismatic	Cylindrical	Planar	Lock
linear spring	\(\mathbf{P}_{1},\mathbf{Q}_{1}\)	\(\mathbf{P}_{1},\mathbf{Q}_{1}\)	\(\mathbf{P}_{2},\mathbf{Q}_{2}\)	\(\mathbf{P}_{2},\mathbf{Q}_{2}\)	\(\mathbf{P}_{3},\mathbf{Q}_{3}\)	\(\mathbf{P}_{1},\mathbf{Q}_{1}\)
torsional spring	\(k=0\)	\(k=1\)	\(k=3\)	\(k=1\)	\(k=1\)	\(k=3\)
linear damper	\(\mathbf{P}_{1},\mathbf{V}_{1}\)	\(\mathbf{P}_{1},\mathbf{V}_{1}\)	\(\mathbf{P}_{2},\mathbf{V}_{2}\)	\(\mathbf{P}_{2},\mathbf{V}_{2}\)	\(\mathbf{P}_{3},\mathbf{V}_{3}\)	\(\mathbf{P}_{1},\mathbf{V}_{1}\)
torsional damper	\(\mathbf{0},\mathbf{0}\)	\(\mathbf{P}_{2},\mathbf{W}_{2}\)	\(\mathbf{P}_{1},\mathbf{W}_{1}\)	\(\mathbf{P}_{2},\mathbf{W}_{2}\)	\(\mathbf{P}_{2},\mathbf{W}_{2}\)	\(\mathbf{P}_{1},\mathbf{W}_{1}\)

Prescribed Joint Forces and Moments

Prescribed Force at Joint

When a joint has a translational degree of freedom along \(\mathbf{n}^{\left(1\right)}\), there may be a need to prescribe the force \(f\) along that direction. This means that the reaction force \(\mathbf{f}^{\left(1\right)}\) may be supplemented with the force \(f\,\mathbf{n}^{\left(1\right)}\),

\[ \begin{equation} \mathbf{f}^{\left(1\right)}=f\mathbf{n}^{\left(1\right)}\,,\label{eq:pfj-1} \end{equation} \]

and the linearization produces

\[ \begin{equation} D\mathbf{f}^{\left(1\right)}=-f\,\hat{\mathbf{n}}^{\left(1\right)}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}\,.\label{eq:pfj-2} \end{equation} \]

Thus,

\[ \begin{equation} \boxed{\mathbf{K}_{f\theta}^{\left(1\right)}=-f\,\hat{\mathbf{n}}^{\left(1\right)}}\,,\label{eq:pfj-3} \end{equation} \]

and

\[ \begin{equation} \boxed{\mathbf{K}_{fr}^{\left(1\right)}=\mathbf{K}_{fr}^{\left(2\right)}=\mathbf{K}_{f\theta}^{\left(2\right)}=\mathbf{0}}\,.\label{eq:pfj-4} \end{equation} \]

Prescribed Moment at Joint

When a joint has a rotational degree of freedom along \(\mathbf{n}^{\left(1\right)}\), there may be a need to prescribe the moment \(m\) along that direction. This means that the reaction moment \(\mathbf{m}^{\left(1\right)}\) may be supplemented with the moment \(m\,\mathbf{n}^{\left(1\right)}\),

\[ \begin{equation} \mathbf{m}^{\left(1\right)}=m\,\mathbf{n}^{\left(1\right)}\,,\label{eq:pmj-1} \end{equation} \]

and the linearization produces

\[ \begin{equation} D\mathbf{m}^{\left(1\right)}=-m\,\hat{\mathbf{n}}^{\left(1\right)}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}\,.\label{eq:pmj-2} \end{equation} \]

Thus,

\[ \begin{equation} \boxed{\mathbf{K}_{m\theta}^{\left(1\right)}=-m\,\hat{\mathbf{n}}^{\left(1\right)}}\,,\label{eq:pmj-3} \end{equation} \]

and

\[ \begin{equation} \boxed{\mathbf{K}_{mr}^{\left(1\right)}=\mathbf{K}_{mr}^{\left(2\right)}=\mathbf{K}_{m\theta}^{\left(2\right)}=\mathbf{0}}\,.\label{eq:pmj-4} \end{equation} \]

Prescribed Joint Motion

Prescribed Displacement at Joint

When a joint has a translational degree of freedom along \(\mathbf{n}^{\left(1\right)}\), there may be a need to prescribe the relative displacement \(d\) along that direction. This can be achieved by supplementing the joint reaction force with a force that closes the gap between the current and desired translation,

\[ \begin{equation} \mathbf{f}^{\left(1\right)}=\varepsilon_{c}\left[\left(\mathbf{n}^{\left(1\right)}\otimes\mathbf{n}^{\left(1\right)}\right)\cdot\mathbf{g}-d\,\mathbf{n}^{\left(1\right)}\right]=\varepsilon_{c}\left(\mathbf{P}_{3}\cdot\mathbf{g}-d\,\mathbf{n}^{\left(1\right)}\right)\,,\label{eq:pdj-1} \end{equation} \]

where \(\mathbf{P}_{3}\) is given in \eqref{eq:jf-P-projection}. Then,

\[ \begin{equation} D\mathbf{f}^{\left(1\right)}=D\mathbf{P}_{3}\cdot\mathbf{g}+\mathbf{P}_{3}\cdot D\mathbf{g}+d\,\hat{\mathbf{n}}^{\left(1\right)}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}\label{eq:pdj-2} \end{equation} \]

Using the results of Section Reaction Forces from Springs, the stiffnesses are supplemented with

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{K}_{fr}^{\left(1\right)} & =-\varepsilon_{c}\mathbf{P}_{3}\\ \mathbf{K}_{f\theta}^{\left(1\right)} & =\varepsilon_{c}\left(\mathbf{P}_{3}\cdot\hat{\mathbf{z}}^{\left(1\right)}+\mathbf{Q}+d\,\hat{\mathbf{n}}^{\left(1\right)}\right)\\ \mathbf{K}_{fr}^{\left(2\right)} & =\varepsilon_{c}\mathbf{P}_{3}\\ \mathbf{K}_{f\theta}^{\left(2\right)} & =-\varepsilon_{c}\mathbf{P}_{3}\cdot\hat{\mathbf{z}}^{\left(2\right)} \end{aligned} }\,.\label{eq:pdj-3} \end{equation} \]

Prescribed Rotation at Joint

The relative rotation between the rigid bodies is

\[ \begin{equation} \mathbf{Q}=\boldsymbol{\Lambda}^{\left(2\right)}\cdot\boldsymbol{\Lambda}^{\left(1\right)T}=\sum_{j}\mathbf{e}_{j}^{\left(2\right)}\otimes\mathbf{e}_{j}^{\left(1\right)}\,.\label{eq:prj-1} \end{equation} \]

We want it to be equal to a rotation by \(\boldsymbol{\chi}\) as expressed in the basis \(\mathbf{e}_{j}^{\left(1\right)}\),

\[ \begin{equation} \mathbf{A}=\exp\left[\boldsymbol{\chi}\right]\,.\label{eq:prj-2} \end{equation} \]

We enforce this constraint by requiring that

\[ \begin{equation} \mathbf{A}\cdot\mathbf{Q}^{T}=\mathbf{I}\,.\label{eq:prj-3} \end{equation} \]

We may thus evaluate

\[ \begin{equation} \mathbf{R}=\mathbf{A}\cdot\mathbf{Q}^{T}\,,\label{eq:prj-4} \end{equation} \]

and expect that \(\mathbf{R}=\mathbf{I}\) when the constraint is enforced. Note that \(\mathbf{R}^{T}\cdot\mathbf{R}=\mathbf{I}\) because of the orthogonality of \(\mathbf{Q}\) and \(\mathbf{A}\), i.e., \(\mathbf{R}\) is always orthogonal, even when the constraint is not enforced. The axial vector of \(\mathbf{R}\) may be denoted by \(\boldsymbol{\xi}\), i.e., \(\mathbf{R}=\exp\left[\boldsymbol{\xi}\right]\), and the constraint is enforced when \(\boldsymbol{\xi}=\mathbf{0}\). Therefore, a moment \(\varepsilon_{r}\boldsymbol{\xi}\) needs to be prescribed,

\[ \begin{equation} \mathbf{m}^{\left(1\right)}=\varepsilon_{r}\boldsymbol{\xi}\,.\label{eq:prj-5} \end{equation} \]

Since \(\mathbf{R}\) is orthogonal, we can linearize it along an increment \(\Delta\boldsymbol{\xi}\), \(D\mathbf{R}\left[\Delta\boldsymbol{\xi}\right]=\Delta\hat{\boldsymbol{\xi}}\cdot\mathbf{R}\), from which it follows that \(\Delta\hat{\boldsymbol{\xi}}=D\mathbf{R}\cdot\mathbf{R}^{T}\) with

\[ \begin{equation} D\mathbf{R}=\mathbf{A}\cdot D\mathbf{Q}^{T}=\mathbf{A}\cdot\left(\Delta\hat{\boldsymbol{\theta}}^{\left(1\right)}\cdot\mathbf{Q}^{T}-\mathbf{Q}^{T}\cdot\Delta\hat{\boldsymbol{\theta}}^{\left(2\right)}\right)\,,\label{eq:prj-6} \end{equation} \]

so that

\[ \begin{equation} \Delta\hat{\boldsymbol{\xi}}=\mathbf{A}\cdot\Delta\hat{\boldsymbol{\theta}}^{\left(1\right)}\cdot\mathbf{A}^{T}-\mathbf{R}\cdot\Delta\hat{\boldsymbol{\theta}}^{\left(2\right)}\cdot\mathbf{R}^{T}\,.\label{eq:prj-7} \end{equation} \]

From this expression we get

\[ \begin{equation} \Delta\boldsymbol{\xi}=\mathbf{A}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}-\mathbf{R}\cdot\Delta\boldsymbol{\theta}^{\left(2\right)}\,.\label{eq:prj-8} \end{equation} \]

Thus,

\[ \begin{equation} D\mathbf{m}^{\left(1\right)}=\varepsilon_{r}\left(\mathbf{A}\cdot\Delta\boldsymbol{\theta}^{\left(1\right)}-\mathbf{R}\cdot\Delta\boldsymbol{\theta}^{\left(2\right)}\right)\,.\label{eq:prj-9} \end{equation} \]

It follows that

\[ \begin{equation} \boxed{\mathbf{K}_{mr}^{\left(1\right)}=\mathbf{K}_{mr}^{\left(2\right)}=\mathbf{0}}\,,\label{eq:prj-10} \end{equation} \]

and

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{K}_{m\theta}^{\left(1\right)} & =\varepsilon_{r}\mathbf{A}\\ \mathbf{K}_{m\theta}^{\left(2\right)} & =-\varepsilon_{r}\mathbf{R} \end{aligned} }\,.\label{eq:prj-11} \end{equation} \]

Other Rigid Connectors

Spring Between Rigid Bodies

Consider a spring between points \(\mathbf{x}^{\left(i\right)}\) on rigid bodies \(\left(1\right)\) and \(\left(2\right)\). The spring force is oriented along \(\mathbf{n}\), where

\[ \begin{equation} \mathbf{n}=\frac{\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}}{\left|\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}\right|}\,.\label{eq:rbs-1} \end{equation} \]

We may write

\[ \begin{equation} L=\left|\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}\right|\,,\quad L_{0}=\left|\mathbf{X}^{\left(2\right)}-\mathbf{X}^{\left(2\right)}\right|\,.\label{eq:rbs-2} \end{equation} \]

Then, the spring force acting on body \(\left(1\right)\) is

\[ \begin{equation} \mathbf{f}^{\left(1\right)}=k\left(L-L_{0}\right)\mathbf{n}=k\left(\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}-L_{0}\mathbf{n}\right)\,,\label{eq:rbs-3} \end{equation} \]

where \(k\) is the spring constant. It is understood that the special case \(L_{0}=0\) produces \(\mathbf{f}^{\left(1\right)}=k\left(\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}\right)\). We do not need to use the augmented Lagrangian method here, therefore \(\mathbf{f}^{\left(1\right)}=k\mathbf{c}\), where

\[ \begin{equation} \mathbf{c}=\mathbf{x}_{n+\alpha}^{\left(2\right)}-\mathbf{x}_{n+\alpha}^{\left(1\right)}-L_{0}\frac{\mathbf{x}_{n+\alpha}^{\left(2\right)}-\mathbf{x}_{n+\alpha}^{\left(1\right)}}{\left|\mathbf{x}_{n+\alpha}^{\left(2\right)}-\mathbf{x}_{n+\alpha}^{\left(1\right)}\right|}\,.\label{eq:rbs-4} \end{equation} \]

Since the spring may freely pivot about its insertion point, we let \(\mathbf{m}^{\left(1\right)}=\mathbf{0}\). Thus,

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{f}^{\left(1\right)} & =k\left(\mathbf{x}_{n+\alpha}^{\left(2\right)}-\mathbf{x}_{n+\alpha}^{\left(1\right)}-L_{0}\frac{\mathbf{x}_{n+\alpha}^{\left(2\right)}-\mathbf{x}_{n+\alpha}^{\left(1\right)}}{\left|\mathbf{x}_{n+\alpha}^{\left(2\right)}-\mathbf{x}_{n+\alpha}^{\left(1\right)}\right|}\right)\\ \mathbf{m}^{\left(1\right)} & =\mathbf{0} \end{aligned} }\label{eq:rbs-5} \end{equation} \]

and the virtual work associated with this spring is

\[ \begin{equation} \delta G=\left[\begin{array}{cccc} \delta\mathbf{r}^{\left(1\right)} & \delta\boldsymbol{\omega}^{\left(1\right)} & \delta\mathbf{r}^{\left(2\right)} & \delta\boldsymbol{\omega}^{\left(2\right)}\end{array}\right]\left[\begin{array}{c} \mathbf{f}^{\left(1\right)}\\ \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{f}^{\left(1\right)}\\ -\mathbf{f}^{\left(1\right)}\\ -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{f}^{\left(1\right)} \end{array}\right]\,.\label{eq:rbs-6} \end{equation} \]

The linearization of \(\delta G\) may be expressed as

\[ \begin{equation} -D\delta G=\left[\begin{array}{cccc} \delta\mathbf{r}^{\left(1\right)} & \delta\boldsymbol{\omega}^{\left(1\right)} & \delta\mathbf{r}^{\left(2\right)} & \delta\boldsymbol{\omega}^{\left(2\right)}\end{array}\right]\left[\mathbf{K}\right]\left[\begin{array}{c} \Delta\mathbf{r}^{\left(1\right)}\\ \Delta\boldsymbol{\theta}^{\left(1\right)}\\ \Delta\mathbf{r}^{\left(2\right)}\\ \Delta\boldsymbol{\theta}^{\left(2\right)} \end{array}\right]\,,\label{eq:rbs-7} \end{equation} \]

where the stiffness matrix is

\[ \begin{equation} \left[\mathbf{K}\right]=k\alpha\left[\begin{array}{cccc} \mathbf{P} & -\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(1\right)} & -\mathbf{P} & \mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\\ \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{P} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(1\right)} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{P} & \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\\ -\mathbf{P} & \mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(1\right)} & \mathbf{P} & -\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\\ -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{P} & \hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(1\right)} & \hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{P} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(2\right)} \end{array}\right]\,,\label{eq:rbs-8} \end{equation} \]

with

\[ \begin{equation} \mathbf{P}=\left(1-\frac{L_{0}}{L_{n+\alpha}}\right)\mathbf{I}+\frac{L_{0}}{L_{n+\alpha}}\mathbf{n}_{n+\alpha}\otimes\mathbf{n}_{n+\alpha}\,.\label{eq:rbs-9} \end{equation} \]

Damper Between Rigid Bodies

Consider a damper (e.g., a dashpot) inserted between points on two rigid bodies. The force generated by this dashpot is

\[ \begin{equation} \mathbf{f}^{\left(1\right)}=\varepsilon_{c}\dot{\mathbf{c}}\,.\label{eq:rbd-1} \end{equation} \]

where \(\dot{\mathbf{c}}\) is the relative velocity between insertion points,

\[ \begin{equation} \dot{\mathbf{c}}=\mathbf{v}_{n+\alpha}^{\left(2\right)}-\mathbf{v}_{n+\alpha}^{\left(1\right)}\,.\label{eq:rbd-2} \end{equation} \]

This relative velocity is related to the rigid body degrees of freedom by

\[ \begin{equation} \begin{aligned}\mathbf{x}_{n+\alpha}^{\left(i\right)} & =\mathbf{r}_{n+\alpha}^{\left(i\right)}+\mathbf{z}_{n+\alpha}^{\left(i\right)}\\ \mathbf{v}_{n}^{\left(i\right)} & =\dot{\mathbf{r}}_{n}^{\left(i\right)}+\boldsymbol{\omega}_{n}\times\mathbf{z}_{n}^{\left(i\right)}\\ \mathbf{v}_{n+1}^{\left(i\right)} & =\dot{\mathbf{r}}_{n+1}^{\left(i\right)}+\boldsymbol{\omega}_{n+1}\times\mathbf{z}_{n+1}^{\left(i\right)}\\ \mathbf{v}_{n+\alpha}^{\left(i\right)} & =\alpha\mathbf{v}_{n+1}^{\left(i\right)}+\left(1-\alpha\right)\mathbf{v}_{n}^{\left(i\right)} \end{aligned} \,.\label{eq:rbd-3} \end{equation} \]

Since the dashpot may freely rotate about its insertion points, we set \(\mathbf{m}^{\left(1\right)}=\mathbf{0}\). It follows that

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{f}^{\left(1\right)} & =\varepsilon_{c}\left(\mathbf{v}_{n+\alpha}^{\left(2\right)}-\mathbf{v}_{n+\alpha}^{\left(1\right)}\right)\\ \mathbf{m}^{\left(1\right)} & =\mathbf{0} \end{aligned} }\,,\label{eq:rbd-4} \end{equation} \]

and the virtual work resulting from the dashpot is

\[ \begin{equation} \delta G=\left[\begin{array}{cccc} \delta\mathbf{r}^{\left(1\right)} & \delta\boldsymbol{\omega}^{\left(1\right)} & \delta\mathbf{r}^{\left(2\right)} & \delta\boldsymbol{\omega}^{\left(2\right)}\end{array}\right]\left[\begin{array}{c} \mathbf{f}^{\left(1\right)}\\ \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{f}^{\left(1\right)}\\ -\mathbf{f}^{\left(1\right)}\\ -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{f}^{\left(1\right)} \end{array}\right]\,.\label{eq:rbd-5} \end{equation} \]

The linearization of \(\delta G\) may be written as

\[ \begin{equation} \begin{aligned}-D\delta G & =\left[\begin{array}{cccc} \delta\mathbf{r}^{\left(1\right)} & \delta\boldsymbol{\omega}^{\left(1\right)} & \delta\mathbf{r}^{\left(2\right)} & \delta\boldsymbol{\omega}^{\left(2\right)}\end{array}\right]\varepsilon_{c}\alpha\left[\begin{array}{cccc} \mathbf{A} & -\mathbf{B}^{\left(1\right)} & -\mathbf{A} & \mathbf{B}^{\left(2\right)}\\ \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{A} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{B}^{\left(1\right)} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{A} & \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{B}^{\left(2\right)}\\ -\mathbf{A} & \mathbf{B}^{\left(1\right)} & \mathbf{A} & -\mathbf{B}^{\left(2\right)}\\ -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{A} & \hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{B}^{\left(1\right)} & \hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{A} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{B}^{\left(2\right)} \end{array}\right]\left[\begin{array}{c} \Delta\mathbf{r}^{\left(1\right)}\\ \Delta\boldsymbol{\theta}^{\left(1\right)}\\ \Delta\mathbf{r}^{\left(2\right)}\\ \Delta\boldsymbol{\theta}^{\left(2\right)} \end{array}\right]\end{aligned} \,,\label{eq:rbd-6} \end{equation} \]

where

\[ \begin{equation} \left[\mathbf{K}\right]=\varepsilon_{c}\alpha\left[\begin{array}{cccc} \mathbf{A} & -\mathbf{B}^{\left(1\right)} & -\mathbf{A} & \mathbf{B}^{\left(2\right)}\\ \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{A} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{B}^{\left(1\right)} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{A} & \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{B}^{\left(2\right)}\\ -\mathbf{A} & \mathbf{B}^{\left(1\right)} & \mathbf{A} & -\mathbf{B}^{\left(2\right)}\\ -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{A} & \hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{B}^{\left(1\right)} & \hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{A} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{B}^{\left(2\right)} \end{array}\right]\,,\label{eq:rbd-7} \end{equation} \]

and

\[ \begin{equation} \begin{aligned}\mathbf{A} & =\frac{\gamma}{\beta\Delta t}\mathbf{I}\\ \mathbf{B}^{\left(i\right)} & =\frac{\gamma}{\beta\Delta t}\hat{\mathbf{z}}_{n+1}^{\left(i\right)}\cdot\mathbf{T}^{T}\left(\boldsymbol{\theta}^{\left(i\right)}\right)+\hat{\boldsymbol{\omega}}_{n+1}\cdot\hat{\mathbf{z}}_{n+1}^{\left(i\right)} \end{aligned} \,.\label{eq:rbd-8} \end{equation} \]

Contractile Force Between Rigid Bodies

Consider a contractile force between points \(\mathbf{x}^{\left(i\right)}\) on rigid bodies \(\left(1\right)\) and \(\left(2\right)\). The force is oriented along \(\mathbf{n}\), where

\[ \begin{equation} \mathbf{n}=\frac{\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}}{\left|\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}\right|}\,.\label{eq:rcf-1} \end{equation} \]

Let \(L=\left|\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}\right|\) and \(L_{0}=\left|\mathbf{X}^{\left(2\right)}-\mathbf{X}^{\left(2\right)}\right|\), so that the contractile force acting on body \(\left(1\right)\) is given by

\[ \begin{equation} \mathbf{f}^{\left(1\right)}=f_{0}\mathbf{n}=\frac{f_{0}}{L}\left(\mathbf{x}^{\left(2\right)}-\mathbf{x}^{\left(1\right)}\right)\,.\label{eq:rcf-2} \end{equation} \]

We do not need to use the augmented Lagrangian method here, thus

\[ \begin{equation} \boxed{\begin{aligned}\mathbf{f}^{\left(1\right)} & =f_{0}\frac{\mathbf{x}_{n+\alpha}^{\left(2\right)}-\mathbf{x}_{n+\alpha}^{\left(1\right)}}{\left|\mathbf{x}_{n+\alpha}^{\left(2\right)}-\mathbf{x}_{n+\alpha}^{\left(1\right)}\right|}\\ \mathbf{m}^{\left(1\right)} & =\mathbf{0} \end{aligned} }\label{eq:rcf-3} \end{equation} \]

where we assume that the contractile force pivots freely at the rigid body insertiones. The virtual work resulting from this contractile force is thus

\[ \begin{equation} \delta G=\left[\begin{array}{cccc} \delta\mathbf{r}^{\left(1\right)} & \delta\boldsymbol{\omega}^{\left(1\right)} & \delta\mathbf{r}^{\left(2\right)} & \delta\boldsymbol{\omega}^{\left(2\right)}\end{array}\right]\left[\begin{array}{c} \mathbf{f}^{\left(1\right)}\\ \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{f}^{\left(1\right)}\\ -\mathbf{f}^{\left(1\right)}\\ -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{f}^{\left(1\right)} \end{array}\right]\,,\label{eq:rcf-4} \end{equation} \]

and its linearization is

\[ \begin{equation} -D\delta G=\left[\begin{array}{cccc} \delta\mathbf{r}^{\left(1\right)} & \delta\boldsymbol{\omega}^{\left(1\right)} & \delta\mathbf{r}^{\left(2\right)} & \delta\boldsymbol{\omega}^{\left(2\right)}\end{array}\right]\left[\mathbf{K}\right]\left[\begin{array}{c} \Delta\mathbf{r}^{\left(1\right)}\\ \Delta\boldsymbol{\theta}^{\left(1\right)}\\ \Delta\mathbf{r}^{\left(2\right)}\\ \Delta\boldsymbol{\theta}^{\left(2\right)} \end{array}\right]\,.\label{eq:rcf-5} \end{equation} \]

Here, the stiffness matrix is given by

\[ \begin{equation} \left[\mathbf{K}\right]=\alpha\frac{f_{0}}{L_{n+\alpha}}\left[\begin{array}{cccc} \mathbf{P} & -\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(1\right)} & -\mathbf{P} & \mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\\ \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{P} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(1\right)} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{P} & \hat{\mathbf{z}}_{n+\alpha}^{\left(1\right)}\cdot\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\\ -\mathbf{P} & \mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(1\right)} & \mathbf{P} & -\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(2\right)}\\ -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{P} & \hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(1\right)} & \hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{P} & -\hat{\mathbf{z}}_{n+\alpha}^{\left(2\right)}\cdot\mathbf{P}\cdot\hat{\mathbf{z}}_{n+1}^{\left(2\right)} \end{array}\right]\,,\label{eq:rcf-6} \end{equation} \]

where

\[ \begin{equation} \mathbf{P}=\mathbf{I}-\mathbf{n}_{n+\alpha}\otimes\mathbf{n}_{n+\alpha}\,.\label{eq:rcf-7} \end{equation} \]