Modelling and Integration Concepts of Multibody Systems on Lie Groups

Abstract

Lie group integration schemes for multibody systems (MBS) are attractive as they provide a coordinate-free and thus singularity-free approach to MBS modeling. The Lie group setting also allows for developing integration schemes that preserve motion integrals and coadjoint orbits. Most of the recently proposed Lie group integration schemes are based on variants of generalized alpha Newmark schemes. In this chapter constrained MBS are modeled by a system of differential-algebraic equations (DAE) on a configuration space being a subvariety of the Lie group $SE(3)^{n}$. This is transformed to an index 1 DAE system that is integrated with Munthe-Kaas (MK) integration scheme. The chapter further addresses geometric integration schemes that preserve integrals of motion. In this context, a non-canonical Lie-group Störmer-Verlet integration scheme with direct $SO(3)$ rotational update is presented. The method is 2nd order accurate and it is angular momentum preserving for a free-spinning body. Moreover, although being fully explicit, the method achieves excellent conservation of the angular momentum of a free rotational body and the motion integrals of the Lagrangian top. A higher-order coadjoint-preserving integration scheme on $SO(3)$ is also presented. This method exactly preserves spatial angular momentum of a free body and it is particularly numerically efficient.

6.1 Introduction

Multibody systems (MBS) are commonly modeled by motion equations evolving on the parameter manifold corresponding to a specific parameterization in terms of angles and displacements. Since the motion of rigid bodies, and thus of a MBS, evolves on a Lie group the kinematics and dynamics of a MBS can be modeled coordinate-free as a dynamic system on a Lie group, which avoids the well-known  problem of parameterization singularities. This allows for application of Lie group integration methods that are inherently coordinate-free. The original application of Lie group integration schemes to rigid body dynamics was the dynamics of a rotating body. To this end the Munthe-Kaas (MK) method based on a Runge-Kutta scheme is used [9, 14, 22, 25–27, 29], and later an amended Newmark-Verlet scheme was applied [15]. In this case the rotation group $SO(3)$ is readily identified as configuration space (c-space). From a MBS perspective this corresponds to a ‘minimal coordinate’ or ‘relative coordinate’ formulation in the sense that the body is free to move on $SO(3)$. From a more general point of view, considering the rigid body as being constrained by a spherical joint to rotate about a fixed point, $SO(3)$ is the isotropy group of the joint. Hence this is the Lie group of relative motions of two bodies connected by a spherical joint. This ‘relative coordinate’ approach applies to general tree-topology MBS where all joints describe motion subgroups of $SE(3)$. Along this line the MK scheme was applied to the dynamics of unconstrained serial manipulators in [30]. Instead of resorting to the motion (isotropy) groups of joints, the motion of individual bodies can be modeled on $SE(3)$ and a general MBS with topological loops be modeled by imposing corresponding constraints. This is equivalent to the ‘absolute coordinate’ formulation. That is, the configuration of an MBS comprising $n$ rigid bodies is a subvariety of $SE(3)^{n}$. Following the common MBS modeling approach, rigid body configurations are frequently assumed to belong to the Lie group $SO(3) \times \mathbb {R}^{3}$, however. Therewith in [33, 34] the MBS motion equations are formulated as an index 1 differential-algebraic equations (DAE) and solved with a MK scheme. In [4, 5, 19] a generalized alpha/Newmark scheme is used to integrate the motion equations of constrained MBS formulated on $SO(3) \times \mathbb {R}^{3}$. Since Lie group integrators exploit the geometry of the c-space, replacing $SE(3)$ by $SO(3) \times \mathbb {R}^{3}$ has consequences for the performance. This issue is addressed in [23], and it is shown that in specific cases, $SE(3)$ achieves perfect constraint satisfaction whereas in the general case both c-spaces lead to equivalent results. In the first part of this chapter a DAE index 1 MBS model on $SE(3)$ is introduced and its integration with a MK scheme described.

Besides a coordinate-free description the modeling of MBS on a Lie group provides a setting for structure preserving integration schemes. A non-canonical Lie-group Störmer-Verlet integration scheme on $SO(3)$ is presented in the second part of this chapter. The method is 2nd order accurate, it is angular momentum preserving, and it does not introduce an energy drift. It is shown that, although being fully explicit, the method conserves the angular momentum of a freely rotating body and the motion integrals of the Lagrangian top better than established conserving algorithms. Finally higher-order integration scheme is presented that preserves coadjoint orbits. The method exactly preserves spatial angular momentum of a free body and is numerically efficient since the rotation and momentum update use the same incremental rotation vector.

6.2 Lie Group Integration Schemes for Multibody Systems

6.2.1 Lie Group Modeling of Multibody Systems

The motion equations of a holonomically constrained multibody system (MBS) comprising $n$ rigid bodies in terms of non-holonomic velocities attain the form

$$\begin{aligned} \mathbf {M}(q) \dot{\mathbf {V}}+\mathbf {J}^{T}{\varvec{\lambda }}&= \mathbf {Q}\left( q,\mathbf {V},t\right) \end{aligned}$$

(6.1a)

$$\begin{aligned} \dot{q}&=q\cdot \mathbf {V} \end{aligned}$$

(6.1b)

$$\begin{aligned} g(q)&= \mathbf {0}. \end{aligned}$$

(6.1c)

Adopting the ‘absolute coordinate’ approach the configuration $q=\left( \mathbf {C}_{1},\ldots ,\mathbf {C}_{n}\right) \in G$ of a MBS is given in terms of the ‘absolute’ configuration $\mathbf {C}_{i}\in SE(3)$ of the $n$ bodies, where $G=SE(3)^{n}$ is the 6-dimensional Lie group representing the rigid body configuration. The MBS velocity $\mathbf {V} =\left( \mathbf {V}_{1},\ldots ,\mathbf {V}_{n}\right) \in \mathfrak {g}$ consists of the body-fixed velocities $\mathbf {V}_{i}=\left( {\varvec{\omega }} _{i},\mathbf {v}_{i}\right) , i=1,\ldots ,n$, where $\mathfrak {g}=se\left( 3\right) ^{n}$ is the Lie algebra of $G$. The Eqs. (6.1a–6.1c) form a system of differential-algebraic equations (DAE) on $G\times \mathfrak {g}$ with geometric constraints (6.1c). In the dynamic motion equations (6.1a) ${\varvec{\lambda }}$ are Lagrange multipliers and $\mathbf {J}$ is the constraint Jacobian. The system (6.1b), relating the MBS velocity and motion, is called the kinematic reconstruction equations. Its solution is the motion $q(t)$ of the MBS. Traditionally, describing the MBS configuration in terms of position vectors $\mathbf {r}_{i}$ and certain rotation parameters ${\varvec{\theta }}_{i}$, the kinematic reconstruction equations assume the decoupled form ${\varvec{\omega }}_{i}=\mathbf {B}_{i}\left( \mathbf {q}\right) \dot{\varvec{\theta }}_{i}$ and $\mathbf {v}_{i}=\dot{\mathbf {r}}_{i},i=1,\ldots ,n$. In the Lie group approach using $SE(3)$ the geometry of rigid body motions is respected. This is not the case when rotations and translations are treated independently. It has been shown [23] that this leads to constraint violations.

Background on the Lie group modeling of rigid body motions can be found in the books by Selig [31] and Murray et al. [28] for instance.

6.2.2 Configuration Space Lie Group

Body-fixed reference frames attached to the $n$ rigid bodies represent the configuration of the MBS. The configuration of body $i$ with respect to a world-fixed inertial frame (IFR) is described by the pair $C_{i}=\left( \mathbf {R}_{i},\mathbf {r}_{i}\right)$, which can be represented as matrix

$$\begin{aligned} \mathbf {C}_{i}=\left( \begin{array}{cc} \mathbf {R}_{i} &{} \mathbf {r}_{i} \\ \mathbf {0} &{} 1 \end{array} \right) \end{aligned}$$

(6.2)

where $\mathbf {r}_{i}\in \mathbb {R}^{3}$ is the position vector of its origin and $\mathbf {R}_{i}\in SO(3)$ is the absolute rotation matrix. Therewith the combination of two successive rigid-body motions is given by

$$\begin{aligned} \mathbf {C}_{2}\mathbf {C}_{1}=\left( \begin{array}{cc} \mathbf {R}_{2}\mathbf {R}_{1} &{} \mathbf {r}_{2}+\mathbf {R}_{2}\mathbf {r}_{1} \\ {\mathbf {0}} &{} 1 \end{array} \right) . \end{aligned}$$

(6.3)

Such rigid body motions constitute the 6-dimensional Lie group $SE\left( 3\right) = SO(3) \ltimes \mathbb {R}^{3}$ of isometric orientation preserving transformations of $E^{3}$.

The ambient configuration space of an MBS consisting of $n$ rigid bodies is the $6n$-dimensional Lie group

$$\begin{aligned} G:=SE(3)^{n} \end{aligned}$$

(6.4)

where $q=(\mathbf {C}_{1},\ldots ,\mathbf {C}_{n})\in G$ represents the configuration of $n$ bodies in a coordinate-free way. Multiplication is componentwise $q^{\prime }\cdot q^{\prime \prime }=(\mathbf {C}_{1}^{\prime } \mathbf {C}_{1}^{\prime \prime },\ldots ,\mathbf {C}_{n}^{\prime }\mathbf {C} _{n}^{\prime \prime })$, inherited from $SE(3)$. The inverse is $q^{-1}=(\mathbf {C}_{1}^{-1},\ldots ,\mathbf {C}_{n}^{-1})$.

The Lie group $SE(3)$ is generated from its Lie algebra $se(3)$ via the exp mapping. Since any rigid body motion is a screw motion the exp mapping gives a finite screw motion corresponding to an instantaneous screw motion $\mathbf {X}\left( t\right)$. Explicitly this is

$$\begin{aligned} \mathbf {X}=\left( {\varvec{\xi }},{\varvec{\eta }}\right) \longmapsto \exp \widehat{\mathbf {X}}=\left( \begin{array}{cc} \exp \widehat{\varvec{\xi }} &{} \;\;\;\frac{1}{\left\| {\varvec{\xi }} \right\| ^{2}}\left( I-\exp \widehat{\varvec{\xi }}\right) \left( {\varvec{ \xi }}\times {\varvec{\eta }}\right) +h{\varvec{\xi }} \qquad \\ 0 &{} 1 \end{array} \right) \end{aligned}$$

(6.5)

where

$$\begin{aligned} \exp \widehat{\varvec{\xi }}=\mathbf {I}+\frac{\sin \left\| \varvec{\xi } \right\| }{\left\| \varvec{\xi }\right\| }\widehat{\varvec{\xi }}+ \frac{1-\cos \left\| \varvec{\xi }\right\| }{\left\| \varvec{\xi } \right\| }\widehat{\varvec{\xi }}^{2} \end{aligned}$$

(6.6)

is the exponential mapping on $SO(3)$. $\mathbf {X}=\left( \varvec{\xi },{\varvec{\eta }}\right) \in \mathbb {R}^{6}$ is the instantaneous screw coordinate vector describing the motion of a rigid body. The six components serve as independent local canonical coordinates on $SE(3)$. It is important to observer that they are not just the scaled rotation axis and translation vector. $\varvec{\xi }$ is in fact the Euler-Rodrigues vector but ${\varvec{\eta }}$ is not the displacement vector.

Let the bodies of the MBS be subjected to a system of $m$ scleronomic geometric constraints

$$\begin{aligned} g\left( q\right) =\mathbf {0} \end{aligned}$$

(6.7)

defined by the constraint mapping $g{:}\,G\rightarrow \mathbb {R}^{m}$. Then the configuration space (c-space) of the MBS is the variety

$$\begin{aligned} \mathcal {V}:=\{q\in G|g\left( q\right) =\mathbf {0}\}. \end{aligned}$$

(6.8)

6.2.3 State Space Lie Group

A rigid body motion is a curve $\mathbf {C}\left( t\right)$ in $SE\left( 3\right)$ The body-fixed velocity of body $i$, expressed by the vector $\mathbf {V}_{i}=\left( {\varvec{\omega }}_{i},\mathbf {v}_{i}\right) ^{T}\in \mathbb {R}^{6}$ where ${\varvec{\omega }}_{i}$ is the body-fixed angular velocity and $\mathbf {v}_{i}$ is the translation velocity vector expressed in the body-fixed RFR, is defined as

$$\begin{aligned} \widehat{\mathbf {V}}_{i}:=\mathbf {C}_{i}^{-1}\dot{\mathbf {C}}_{i}=\left( \begin{array}{cc} \widehat{\varvec{\omega }}_{i} &{} \mathbf {v}_{i} \\ \mathbf {0} &{} 0 \end{array} \right) \in se(3) \end{aligned}$$

(6.9)

with $se(3)$ being the Lie algebra of $SE(3)$. $\widehat{\varvec{\omega }}_{i}:=\mathbf {R}_{i}^{T}\dot{\mathbf {R}}_{i}\in so\left( 3\right)$ is the skew symmetric (cross product) matrix associated to the vector ${\varvec{\omega }}$. Hence to any twist coordinate vector is assigned a $se(3)$-matrix via the ‘hat’ operator.

The right-translated differential of the exp mapping, $\mathrm {dexp} :se(3) \times se(3) \rightarrow se(3)$, can be introduced as $\mathrm {dexp}_{\widehat{\mathbf {X}}}\dot{\widehat{ \mathbf {X}}}=\dot{\mathbf {C}}\mathbf {C}^{-1}$, with $\mathbf {C}=\exp \widehat{\mathbf {X}}$. Replacing $\mathbf {X}$ with $-\mathbf {X}$ leads to the expression for the body-fixed twist:

$$\begin{aligned} \widehat{\mathbf {V}}=\mathrm {dexp}_{-\widehat{\mathbf {X}}}\dot{\widehat{ \mathbf {X}}}. \end{aligned}$$

(6.10)

Hence the vector of body-fixed velocity is given in terms of the time derivative of the screw coordinates $\mathbf {X}$. These are the kinematic reconstruction equations (6.1b) expressed in terms of screw coordinates, that must be solved in order to recover the finite motion from the velocity field. They will be the basis for the Munthe-Kaas method in Sect. 6.2.5.

The inverse of this mapping, which is subsequently needed, is in vector representation given by [31]

$$\begin{aligned} \mathbf {dexp}_{\widehat{\mathbf {X}}}^{-1}&=\mathbf {I}-\frac{1}{2}\mathbf {ad}_{\mathbf {X}}+\left( \frac{2}{\left\| {\varvec{\xi }}\right\| ^{2}}+ \frac{\left\| {\varvec{\xi }}\right\| +3\sin \left\| {\varvec{\xi }} \right\| }{4\left\| {\varvec{\xi }}\right\| \left( \cos \left\| {\varvec{\xi }}\right\| -1\right) }\right) \mathbf {ad}_{\mathbf {X}}^{2} \nonumber \\&\quad +\left( \frac{1}{\left\| {\varvec{\xi }}\right\| ^{4}}+ \frac{\left\| {\varvec{\xi }}\right\| +\sin \left\| {\varvec{\xi }} \right\| }{4\left\| {\varvec{\xi }}\right\| ^{3}\left( \cos \left\| {\varvec{\xi }}\right\| -1\right) }\right) \mathbf {ad}_{\mathbf {X }}^{4} \end{aligned}$$

(6.11)

with $\mathbf {X}=\left( \varvec{\xi },\varvec{\eta }\right)$. An alternative form can be determined, as reported e.g. in [30] , is

$$\begin{aligned} \mathbf {dexp}_{\mathbf {X}}^{-1}=\left( \begin{array}{cc} \mathbf {dexp}_{\varvec{\xi }}^{-1} &{} \mathbf {0} \qquad \\ \mathbf {U} &{} \mathbf {dexp}_{\varvec{\xi }}^{-1} \end{array} \right) \end{aligned}$$

(6.12)

with

$$\begin{aligned} \mathbf {U}\left( \mathbf {X}\right) =\frac{1-\gamma }{\left\| {\varvec{\xi }} \right\| ^{2}}\left( \widehat{\varvec{\eta }}\widehat{\varvec{\xi }}+ \widehat{\varvec{\xi }}\widehat{\varvec{\eta }}\right) +\frac{h_{\mathbf {X}} }{\left\| \varvec{\xi }\right\| ^{3}}\left( \frac{1}{\beta }+\gamma -2\right) \widehat{\varvec{\xi }}^{2}-\frac{1}{2}\widehat{\varvec{\eta }} \end{aligned}$$

(6.13)

$\beta :=\frac{4}{\left\| {\varvec{\xi }}\right\| ^{2}}\sin ^{2}\frac{ \left\| {\varvec{\xi }}\right\| }{2},$ $\gamma :=\frac{2}{\left\| {\varvec{\xi }}\right\| }\cot \frac{\left\| {\varvec{\xi }}\right\| }{2}$, and the pitch $h={\varvec{\xi }}\cdot {\varvec{\eta }}/\left\| {\varvec{\xi }}\right\| ^{2}$. Notice that for pure rotation, i.e. $h_{\mathbf {X}}=0$, ( 6.13) simplifies. In (6.12) $\mathbf {dexp}_{\varvec{\xi } }^{-1}$ is the inverse of the differential of the exp mapping (6.6) on $SO(3)$ for $\mathbf {R}=\exp \widehat{\varvec{\xi }}$ [6]

$$\begin{aligned} \mathbf {dexp}_{\varvec{\xi }}^{-1}=\mathbf {I}-\frac{1}{2}\widehat{\varvec{ \xi }}+\left( 1-\frac{\left\| {\varvec{\xi }}\right\| }{2}\cot \frac{ \left\| {\varvec{\xi } }\right\| }{2}\right) \frac{\widehat{\varvec{\xi }} ^{2}}{\left\| {\varvec{\xi }}\right\| ^{2}}. \end{aligned}$$

(6.14)

Notice finally, that therewith the exp mapping (6.5) can be expressed as

$$\begin{aligned} \mathbf {X}=\left( {\varvec{\xi } },{\varvec{\eta }}\right) \longmapsto \exp \widehat{\mathbf {X}}{}{=}\left( \begin{array}{cc} \exp \widehat{\varvec{\xi }} &{} \ \;\mathbf {dexp}_{\varvec{\xi }}{\varvec{\eta }} \\ \mathbf {0} &{} 1 \end{array} \right) . \end{aligned}$$

(6.15)

The velocities of the bodies of the MBS are collectively represented by $\mathbf {V}=(\mathbf {V}_{1},\ldots ,\mathbf {V}_{n})\in \mathbb {R}^{6n}$. Making use of the isomorphism of $se^{n}(3)$ and $\mathbb {R} ^{6n}$ the body-fixed velocities are determined as $\widehat{\mathbf {V}} =q^{-1}\dot{q}$, denoting $\widehat{\mathbf {V}}=(\widehat{\mathbf {V}} _{1},\ldots ,\widehat{\mathbf {V}}_{n})$. This allows to introduce the ambient state space of the MBS as

$$\begin{aligned} S:=SE(3)^{n}\times \mathbb {R}^{6n}, \end{aligned}$$

(6.16)

which is a $6\cdot 2\cdot n$-dimensional Lie group. This is the left-trivialized tangent bundle of $G$. The state of the MBS is then represented by $X=\left( q,\mathbf {V}\right) =(\mathbf {C}_{1},\ldots , \mathbf {C}_{n},\mathbf {V}_{1},\ldots ,\mathbf {V}_{n})\in S$. The multiplication is understood componentwise: $X^{\prime }\cdot X^{\prime \prime }=\left( \mathbf {C}_{1}^{\prime }\mathbf {C}_{1}^{\prime \prime },\ldots ,\mathbf {C}_{n}^{\prime }\mathbf {C}_{n}^{\prime \prime },\mathbf {V} _{1}^{\prime }+\mathbf {V}_{1}^{\prime \prime },\ldots ,\mathbf {V} _{n}^{\prime }+\mathbf {V}_{n}^{\prime \prime }\right)$.

The Lie algebra of the Lie group $S$ is

$$\begin{aligned} \mathfrak {s}:=se(3)^{n}\times \mathbb {R}^{6n}, \end{aligned}$$

(6.17)

with elements $x=(\mathbf {V}_{1},\ldots ,\mathbf {V}_{n},\mathbf {A} _{1},\ldots ,\mathbf {A}_{n})\in \mathfrak {s}$, which is isomorphic to the tangent space of $S$ via left translation, i.e. $\dot{X}=X\cdot x$. Addition in $\mathfrak {s}$ is also componentwise: $x^{\prime }+x^{\prime \prime }=( \mathbf {V}_{1}^{\prime }+\mathbf {V}_{1}^{\prime \prime },\ldots ,\mathbf {V} _{n}^{\prime }+\mathbf {V}_{n}^{\prime \prime },\mathbf {A}_{1}^{\prime }+ \mathbf {A}_{1}^{\prime \prime },\ldots ,\mathbf {A}_{n}^{\prime }+\mathbf {A} _{n}^{\prime \prime })$. The exponential mapping on the ambient state space Lie group is

$$\begin{aligned} \exp x=\left( \exp \mathbf {V}_{1},\ldots ,\exp \mathbf {V}_{n},\mathbf {A} _{1},\ldots ,\mathbf {A}_{n}\right) \in S \end{aligned}$$

(6.18)

with (6.5). Its right-translated differential $\mathrm {dexp}: \mathfrak {s}\times \mathfrak {s}\rightarrow \mathfrak {s}$ is

$$\begin{aligned} \mathrm {dexp}_{x^{\prime }}x^{\prime \prime }=(\mathrm {dexp}_{\mathbf {V} _{1}^{\prime }}\mathbf {V}_{1}^{\prime \prime },\ldots ,\mathrm {dexp}_{ \mathbf {V}_{n}^{\prime }}\mathbf {V}_{n}^{\prime \prime },\mathbf {A} _{1}^{\prime \prime },\ldots ,\mathbf {A}_{n}^{\prime \prime }) \end{aligned}$$

(6.19)

where for the first $n$ components $\mathrm {dexp}:se(3) \times se(3) \rightarrow se(3)$ is the right-translated differential on $SE(3)$.

Time differentiation of the geometric constraints (6.7) yields the corresponding velocity constraints

$$\begin{aligned} \mathbf {J}\left( q\right) \mathbf {V}=\mathbf {0} \end{aligned}$$

(6.20)

where $\mathbf {J}(q){:}\,\mathbb {R}^{6n}\rightarrow \mathbb {R}^{m}$ is the Jacobian of $g$ in vector representation. Together with the geometric constraints they define the MBS state space

$$\begin{aligned} \mathcal {S}:=\{X=\left( q,\mathbf {V}\right) \in S|g\left( q\right) =0\mathbf {,J}\left( q\right) \mathbf {V}=0\}. \end{aligned}$$

(6.21)

The state space $S$ being a Lie group admits to apply Lie group integration schemes.

6.2.4 Motion Equations of Constrained MBS in Lie Group Descriptor Form

The motion equations (6.1a–6.1c) form an index 3 system of differential-algebraic equations (DAE) on the state space Lie group $\mathcal {S}$. The latter factors into the vector space $\mathbb {R}^{6n}$ and the the Lie group $G$. In particular the kinematic reconstruction equations ( 6.1b) form a system on $G$ consisting of the $n$ equations $\dot{ \mathbf {C}}_{i}=\mathbf {C}_{i}\widehat{\mathbf {V}}_{i}$. The dynamic equations (6.1a) evolve on the vector space $\mathbb {R}^{n}$.

A common approach in MBS modeling is to transform the system (6.1a) together with (6.1c) to the index 1 system

$$\begin{aligned} \left( \begin{array}{cc} \mathbf {M} &{} \mathbf {J}^{T} \\ \mathbf {J} &{} \mathbf {0} \end{array} \right) \left( \begin{array}{c} \dot{\mathbf {V}} \\ {\varvec{\lambda }} \end{array} \right) =\left( \begin{array}{c} \mathbf {Q} \\ {\varvec{\eta }} \end{array} \right) \end{aligned}$$

(6.22)

using the acceleration constraints $\mathbf {J}\left( q\right) \cdot \dot{ \mathbf {V}}={\varvec{\eta }}\left( q,\mathbf {V}\right)$. For a given state $X=\left( q,\mathbf {V}\right) \in \mathcal {S}$ the system (6.22) and thus (6.1a) can be solved for $\dot{\mathbf {V}}$. If $\dot{\mathbf {V}}$ is a solution of (6.22), then (6.1a, 6.1c) is equivalent to the ODE system

$$\begin{aligned} \dot{X}=XF\left( t,X\right) \end{aligned}$$

(6.23)

on the state space $S$, where the mapping $F{:}\,\mathbb {R}\times S\rightarrow \mathfrak {s}$ is introduced as $F\left( t,X\right) =(\mathbf {V},\dot{\mathbf { V}})$ with a solution $\dot{\mathbf {V}}$ of (6.22). The equivalence follows from $XF\left( t,X\right) =(q\cdot \mathbf {V},\dot{\mathbf {V}})$. Evaluation of $XF\left( t,X\right)$ thus amounts to solving (6.22) for $\dot{\mathbf {V}}$ and evaluating (6.1b).

6.2.5 Munthe-Kaas Method for Constrained MBS Dynamics

As the dynamical system evolves on a Lie group its solution can be expressed in the form $X\left( t\right) =X_{0}\exp \Phi \left( t\right)$. Hence at the integration step $i$ the original system (6.23) can be replaced by the system

$$\begin{aligned} \dot{\Phi }^{\left( i\right) }=\mathrm {dexp}_{-\Phi ^{\left( i\right) }}^{-1}F(t,X_{i-1}\exp \Phi ^{\left( i\right) }),\ t\in [t_{i-1},t_{i}],\text { with }\Phi ^{\left( i\right) }\left( t_{i-1}\right) =0 \end{aligned}$$

(6.24)

with initial condition $X_{i-1}$, and be solved with a numerical integration scheme. The Munthe-Kaas (MK) method [12] uses a Runge-Kutta (RK) scheme to determine a solution $\Phi ^{\left( i\right) }\left( t_{i}\right)$ that leads to a numerical solution $X_{i}:=X_{i-1}\exp \Phi ^{\left( i\right) }\left( t_{i}\right)$ of (6.23). The $\Phi ^{\left( i\right) }$ constitute local coordinates on the state space that are valid in a neighborhood of $X_{i-1}$. An $s$-stage MK scheme at time step $i$ follows immediately from the RK method as

$$\begin{aligned}&X_{i}:=X_{i-1}\exp \Phi ^{\left( i\right) },\quad \Phi ^{\left( i\right) }:= h\sum _{j=1}^{s}b_{j}k_{j} \\&k_{j}:=\mathrm {dexp}_{-\Psi _{j}}^{-1}F\left( t_{i-1}+c_{j}h,X_{i-1}\exp \Psi _{j}\right) ,\quad \Psi _{j}:=h\sum _{l=1}^{j-1}a_{jl}k_{l},\; \Psi _{1}=0, \nonumber \end{aligned}$$

(6.25)

where $a_{jl},b_{j}$, and $c_{j}$ are the Butcher coefficients of the $s$ -stage RK method, and $k_{j},\Psi _{j}\in \mathfrak {s}$. MK methods can be applied to ODE on Lie groups. Using the formulation (6.23) the MK can also be applied to the time integration of dynamics equations of constrained MBS.

Remark 1

While it is clear that $SE(3)$ is the c-space Lie group of a rigid body, all original MBS formulations implicitly use the Lie group $SO(3) \times \mathbb {R}^{3}$ since it is customary to use rotation angles and displacement vectors as parameters which are clearly not screw parameters. This leads to an artificial decoupling of rotational and translational motion. This is reflected by the dexp mapping which for $SO(3) \times \mathbb {R}^{3}$ is

$$\begin{aligned} \mathrm {dexp}_{\mathbf {X}}(\mathbf {Y})=(\mathrm {dexp}_{\varvec{\xi }}{\varvec{\eta }},\mathbf {v}) \end{aligned}$$

(6.26)

for $\mathbf {X}=\left( {\varvec{\xi }},{\mathbf {u}}\right)$ and $\mathbf {Y}=\left( {\varvec{\eta }},\mathbf {v}\right)$. Even more, also the recently proposed Lie group generalized alpha schemes [4, 5] and Lie group DAE formulation [33] use $SO(3) \times \mathbb {R}^{3}$ as c-space. As consequence the configuration update step in (6.25) does not respect the screw motion that is encoded in the velocity $\mathbf {V}$, which leads in particular to constraint violations as shown in [23]. However, it is shown in [23] that the difference of the two c-spaces becomes significant for the satisfaction of joint constraints only when a rigid body is connected to the ground by lower-pair joints. In this case the joint constraints are perfectly satisfied independently of the integration accuracy and step size.

Remark 2

It is well-known that the numerical solution of the index 1 formulation (6.23) may not stay in $\mathcal {V}$. Nevertheless, the index 1 formulation (6.22) is convenient for treating the dynamics of constrained MBS. For the classical vector space formulation several constraint stabilization methods have been proposed to cope with the drift phenomenon (an overview can be found in [2]). Within the Lie group formulation these have to be amended as reported in [24, 34]. In [24] a constraint stabilization method is presented that uses the local coordinates $\Phi ^{\left( i\right) }$ of the MK scheme. In [34] stabilisation algorithm based on constrained least square minimization algorithm in Lie groups state space is introduced.

6.2.6 Examples

6.2.6.1 Heavy Top with External Force

The model of a heavy top (Fig. 6.1) consists of a rigid body pivoted to the ground at point $Q$. A spring is attached between its COM and a space-fixed point $P$. The spring force is $\mathbf {F}^{\text {s}}=c(\mathbf {p}_{0}-\mathbf {r})+m \mathbf {g}$, with 10 N/mm, and gravity vector $\mathbf {g}=\left( 0,0,-9.81\right) ^{T}$, with $\mathbf {p}_{0}=\left( 1,0,0.5\right) ^{T}$ m being the space-fixed position vector of the spring suspension point $P$. The body-fixed force vector (6.30) is $\mathbf {F}=\mathbf {R}^{T}\mathbf {F}^{\text {s}}.$ The inertia of the body’s COM is ${\varvec{\Theta }}_{0}=\mathrm {diag}\left( 0.36,{0.306,0.09}\right) \,\text {kg}\,\text {m}^{2}$, and its mass $m=21.6$ kg. Denote with $\mathbf {r}_{0}=(-0.5,0,0)^{T}\,\text {m}$ the position vector of the pivot point measured in the body-fixed reference frame. The configuration of the reference frame is represented by $C=\left( \mathbf {R},\mathbf {r}\right)$, with rotation matrix $\mathbf {R}$ and $\mathbf {r}$ denoting the position of the COM expressed in the spatial inertial frame (IFR).

Fig. 6.1

Rigid body constrained to rotate about fixed point subject to an applied spring force

The geometric constraints imposed by the spherical joint (pivot) are

$$\begin{aligned} g(C)=\mathbf {r}-\mathbf {Rr}_{0}=\mathbf {0.} \end{aligned}$$

(6.27)

Time differentiation, and assuming (6.27), yields the velocity constraints (6.20)

$$\begin{aligned} \left( \begin{array}{cc} \widehat{\mathbf {r}}_{0}&-\mathbf {I} \end{array} \right) \left( \begin{array}{c} {\varvec{\omega }} \\ \mathbf {v} \end{array} \right) =\mathbf {JV}=\mathbf {0} \end{aligned}$$

(6.28)

and the acceleration constraints

$$\begin{aligned} \left( \begin{array}{cc} \widehat{\mathbf {r}}_{0}&-\mathbf {I}\end{array} \right) \left( \begin{array}{c} \dot{\varvec{\omega }} \\ \dot{\mathbf {v}} \end{array} \right) =\widehat{\varvec{\omega }}\widehat{\varvec{\omega }}\mathbf {r}_{0}+ \widehat{\varvec{\omega }}\mathbf {v} \end{aligned}$$

(6.29)

where $\mathbf {V}=\left( {\varvec{\omega }},\mathbf {v}\right)$ is the body-fixed velocity. The body-fixed Newton-Euler equations w.r.t. to the COM combined with (6.29) yield the overall index 1 DAE system

$$\begin{aligned} \left( \begin{array}{ccc} {\varvec{\Theta }}_{0} &{} \mathbf {0} &{} \widehat{\mathbf {r}}_{0}^{T} \\ \mathbf {0} &{} m\mathbf {I} &{} -\mathbf {I} \\ \widehat{\mathbf {r}}_{0} &{} -\mathbf {I} &{} \mathbf {0}\end{array} \right) \left( \begin{array}{c} \dot{\varvec{\omega }} \\ \dot{\mathbf {v}} \\ {\varvec{\lambda }} \end{array} \right) =\left( \begin{array}{c} \widehat{\varvec{\omega }}{\varvec{\Theta }}_{0}\varvec{\omega } \\ \mathbf {F}-m\widehat{\varvec{\omega }}\mathbf {v} \\ \widehat{\varvec{\omega }}\widehat{\varvec{\omega }}\mathbf {r}_{0}+\widehat{ \varvec{\omega }}\mathbf {v} \end{array} \right) . \end{aligned}$$

(6.30)

Fig. 6.2

Error $\varepsilon$ in the position constraints of the top

The motion equations (6.30) were integrated with an MK scheme based on RK4 method for 6 s starting from initial configuration $C_{0}=\left( \mathbf {I},\mathbf {r} _{0}\right)$ and initial angular velocity ${\varvec{\omega }} _{0}=(0,0,0.5)^{T}$ rad/s. The integration method shows 4th order convergence as expected. It is further interesting to observe the exact satisfaction of the position constraints independently of the step size. Figure 6.2 shows the error of the position constraints $\varepsilon \left( q\right) :=\left\| g\left( q\right) \right\|$ for step sizes $\Delta t=10^{-2},10^{-3},10^{-4}~$s. The satisfaction of the constraints up to computation precision is due to the use of the correct c-space Lie group, namely $SE(3)$, that accounts for rigid body motions. This is discussed in [23].

6.2.6.2 Planar 4-Bar Mechanism

A closed loop planar 4-bar mechanism is considered (Fig. 6.3) comprising three revolute joints and one spherical joint. The geometry is chosen as indicated in the Fig. 6.3 with $L_{0}=0.5\,$m.

Fig. 6.3

Planar 4-bar mechanism: comprising two revolute joints with parallel axes and a spherical joint

Fig. 6.4

Violation of position constraints of revolute joint 1 (a) and 2 (b)

Fig. 6.5

Violation of position constraints of revolute joint 3 (a) and spherical joint 4 (b)

The initial configuration is shown in Fig. 6.3. Initially the input crank (body 1) rotates with angular velocity $\omega _{0}=10\,\pi$  rad/s. The motion equations are integrated numerically with the MK/RK4 method with step sizes $\Delta t=10^{-2},10^{-3},10^{-4}~$s. Figures 6.4 and 6.5 show the satisfaction of joint position constraints. Apparently they are satisfied with computation accuracy for the two joints 1 and 4 that are connected to the ground. The orientation constraints of the revolute joints are exactly satisfied. As mentioned above this phenomenon is due to the use of the correct c-space Lie group as discussed in [23].

6.3 Geometric Schemes that Preserve Integrals of Motion

6.3.1 Coadjoint Modeling of Rotational Dynamics

In the geometric schemes presented in the previous chapters, the kinematic reconstruction equations have been solved on the c-space Lie group while the dynamical equations were discretised via ‘classical’ vector-space-based numerical methods. Although pursued by several authors [4, 5, 23], this practice does not utilize the geometrical properties of dynamics on the c-space that gives rise to integration algorithms with additional useful properties, such as the preservation of the integrals of motion. Therefore, in the sequel we describe the geometric schemes that extend the coadjoint orbit preserving integration method for SO(3) [8, 16, 18]. Another possibility of constructing the structure-preserving algorithms is to follow the variational approach, see, for example [20, 36], and references cited there.

We start from the Euler equation of free rigid body rotation given as Lie-Poisson’s system [21] in the form

$$\begin{aligned} \dot{\mathbf{y }}=-\hat{\varvec{\upomega }}\mathbf{y }, \end{aligned}$$

(6.31)

where $\mathbf{y }\in {\varvec{\mathcal {R}}}^{3}$ represents the angular momentum in the body attached frame and $\hat{\varvec{\upomega }}\in so(3)$ is the body angular velocity with so(3) being the Lie algebra of SO(3). By following [13], $\mathbf{y }\in {\varvec{\mathcal {R}}}^{3}$can be identified with ${\varvec{\breve{y}}}\in so^{*}(3)$, where $so^{*}(3)$ is the dual space of the Lie algebra so(3). In a more formal form (6.31) can be expressed as the coadjoint operator [14, 18] on the dual space of the Lie-algebra $so^{*}(3)$ as

$$\begin{aligned} \dot{\mathbf{y }}=\text {ad}_{\hat{\varvec{\upomega }}}^{*}\mathbf{y }={\hat{\mathbf{y }}}{\varvec{\upomega }}(\mathbf{y }), \end{aligned}$$

(6.32)

where ‘$\hbox {ad}^{*}$’ is the dual of the ‘$\hbox {ad}$’ operator $\hbox {ad}_{\hat{\mathbf{a }}} \mathbf{b }={\hat{\mathbf{a }}}\mathbf{b }$, which is the commutator

$$\begin{aligned} \text {ad}_{\hat{\mathbf{a }}} ({\hat{\mathbf{b }}})={\hat{\mathbf{a }}}{\hat{\mathbf{b }}}-{\hat{\mathbf{b }}}{\hat{\mathbf{a }}}=[{\hat{\mathbf{a }}},{\hat{\mathbf{b }}}],\quad \text {for all}\;{\hat{\mathbf{a }}},{\hat{\mathbf{b }}}\in so(3), \end{aligned}$$

(6.33)

in the Lie algebra so(3) identified here with ${\varvec{\mathcal {R}}}^{3}$. The solution of the angular momentum equation in the Lie-Poisson form (6.31) can be expressed within each integration step as an action of SO(3) on ${\varvec{\mathcal {R}}}^{3}$in the form

$$\begin{aligned}&\mathbf{y }^{n+1}=\mathbf{Q }^\mathrm{T } (t) \mathbf{y }^{n}, \quad n=0, 1, 2,\ldots ,\nonumber \\&\mathbf{Q }\in SO(3), \quad t\in \left[ {t_n ,t_{n+1} } \right] , \end{aligned}$$

(6.34)

which leads to solving of ODE on the Lie-group that reads

$$\begin{aligned}&{\dot{\mathbf{Q }}}(t)=\mathbf{Q }(t) {\hat{\varvec{\upomega }}} \left( {\mathbf{y }\left( t \right) } \right) , \quad t \ge t_n,\nonumber \\&\mathbf{Q }(t_n )=I. \end{aligned}$$

(6.35)

More formally, the update step (6.34) can be written as coadjoint action [13], denoted ‘${\mathrm {Ad}}^{\mathrm {*}}$’, of SO(3) on ${\varvec{\mathcal {R}}}^{3}$ in the form [8, 18]

$$\begin{aligned} \mathbf{y }^{n+1}=\hbox {Ad}_{\mathbf{Q }\left( \mathrm{{t}}\right) }^\mathrm{{*}} \mathbf{y }^{n}, \end{aligned}$$

(6.36)

where the relation ${\mathrm {Ad}}_\mathbf{Q}^{\mathrm {*}} \mathbf{y }=\mathbf{Q }^\mathrm{{T}}\mathbf{y }$ is valid and the coadjoint orbit is given as [13]

$$\begin{aligned} O= \{ {\mathrm {Ad}}_{\mathbf Q}^{*} {\mathbf y}\quad |\quad {\mathbf Q}\in SO(3) \} \subset {{\varvec{\mathcal {R}}}}^{3}. \end{aligned}$$

(6.37)

The motivation of constructing the update of $\mathbf{y }^{n}\in {\varvec{\mathcal {R}}}^{3}$ in the form of (6.34) [or (6.36)] is the exact preservation of the magnitude of free-body angular momentum in the body attached frame during the step. Indeed, since for the step initial condition $\mathbf{y }^{n}$ the coadjoint orbit $O_{\mathbf{Y }^{n}}$ is a sphere of radius $\left\| {\mathbf{y }^{n}} \right\|$, the magnitude of $\mathbf{y }^{n}$ will be exactly preserved, independently of the accuracy of the integration method for determining $\mathbf{Q }$ in (6.35).

However, although coadjoint orbits will be preserved independently of the accuracy of determining $\mathbf{Q }$, the rotation matrix $\mathbf{Q }$ has to be determined in order to complete the integration step, i.e. the ODE (6.35) has to be solved. To this end, we will adopt the Muthe-Kaas approach [12, 14, 25] and we seek a solution of (6.35) in the form

$$\begin{aligned} \mathbf{Q }(t)=\exp (\hat{\varvec{\uppsi }}(t)), \quad t\ge t_n, \end{aligned}$$

(6.38)

where the closed form of the exponential mapping on SO(3) is given by the Euler-Rodrigues formula, and ${\varvec{\uppsi }}(t)\in {\varvec{\mathcal {R}}}^{3}$ is the instantaneous rotation vector. Moreover, by following Magnus [12], a solution of (6.35) can be written in the form of (6.38) if $\hat{\varvec{\uppsi }}(t)\in so(3)$ is a solution of the ODE system in the Lie-algebra

$$\begin{aligned} {\dot{\hat{\varvec{\uppsi }}}}=\text {dexp}_{-\hat{\varvec{\uppsi }}}^{-1} (\hat{\varvec{\upomega }}(\mathbf{Q }(t))), \quad \hat{\varvec{\uppsi }}_0=\mathbf{0}, \end{aligned}$$

(6.39)

and operator $\text {dexp}_{-\hat{\varvec{\uppsi }}}^{-1}$ is defined by

$$\begin{aligned} \text {dexp}_{-\hat{\varvec{\uppsi }}}^{-1} ({\hat{\varvec{\upomega }}})={\hat{\varvec{\upomega }}}+\frac{1}{2}\left[ {\hat{\varvec{\uppsi }}},{\hat{\varvec{\upomega }}} \right] +\frac{1}{12}\left[ {\hat{\varvec{\uppsi }}},\left[ {\hat{\varvec{\uppsi }}},{\hat{\varvec{\upomega }}}\right] \right] +\cdots =\sum _{j=0}^{\infty }\frac{B_{j }}{j!}(-\text {ad}_{\hat{\varvec{\uppsi }}}^j ({\hat{\varvec{\upomega }}})), \end{aligned}$$

(6.40)

where the adjoint operator $\text {ad}_{\hat{\varvec{\uppsi }}}$ is given as Lie-bracket (6.33), and $B_{j}$ are Bernoulli numbers [12].

6.3.2 Modified Störmer-Verlet Integration Scheme on SO(3)

The algorithm for a free-spinning rigid body rotational dynamics with the direct update on SO(3), inspired by the Störmer-Verlet integration scheme in a linear vector space [11], can be written in the form [35]

$$\begin{aligned}&{\varvec{\upomega }}^{n+\frac{1}{2}}={\varvec{\upomega }}^{n}-\frac{h}{2}(\mathbf{I }^{-1}{\hat{\varvec{\upomega }}}^{n}\mathbf{I }{\varvec{\upomega }}^{n}) \end{aligned}$$

(6.41a)

$$\begin{aligned}&\mathbf{R }^{n+1}=\mathbf{R }^{n}\mathbf{Q }^{n}=\mathbf{R }^{n}\exp (h {\hat{\varvec{\upomega }}}^{n+\frac{1}{2}}) \end{aligned}$$

(6.41b)

$$\begin{aligned}&{\varvec{\upomega }}^{n+1}={\varvec{\upomega }}^{n+\frac{1}{2}}-\frac{h}{2}(\mathbf{I }^{-1}{\hat{\varvec{\upomega }}}^{n+1}\mathbf{I }{\varvec{\upomega }}^{n+1}) \end{aligned}$$

(6.41c)

Here, by following [35], the first order approximation ${\hat{\varvec{\uppsi }}}=h{\hat{\varvec{\uppsi }}}^{n+\frac{1}{2}}=h{\hat{\varvec{\upomega }}}^{n+\frac{1}{2}}$ is used in (6.38) for the incremental rotation vector and inserted in (6.41b) [while the first order approximation is also used for the discretisation in (6.39)]. The update expressions in Lie algebra (6.41a) and (6.41c) are constructed on the basis of the velocity field discretisation pattern of the original Störmer-Verlet algorithm. As discussed in [35], in order to preserve the system coadjoint orbits, the Eqs. (6.41a) and (6.41c) can be replaced by the expressions derived on the basis of the Eq. (6.34) [or (6.36)]. To this end, we obtain the modified Lie-Störmer-Verlet integration scheme for the forced unconstrained rigid body rotation in the form

$$\begin{aligned} {\varvec{\upomega }}^{n+\frac{1}{2}}&=\mathbf{I }^{-1}(\exp (-\frac{h}{2}{\hat{\varvec{\upomega }}}^{n})(\mathbf{I }{\varvec{\upomega }}^{n}+\frac{h}{2}\mathbf{T }^{n})), \end{aligned}$$

(6.42a)

$$\begin{aligned} \mathbf{R }^{n+1}&=\mathbf{R }^{n}\exp (h{\hat{\varvec{\upomega }}}^{n+\frac{1}{2}}), \end{aligned}$$

(6.42b)

$$\begin{aligned} {\varvec{\upomega }}^{n+1}&=\mathbf{I }^{-1}(\exp (-\frac{h}{2}{\hat{\varvec{\upomega }}}^{n+\frac{1}{2}}) (\exp (-\frac{h}{2}{\hat{\varvec{\upomega }}}^{n+\frac{1}{2}})(\mathbf{I }{\varvec{\upomega }}^{n} \\&\quad +\frac{h}{2}\mathbf{T }^{n})+\frac{h}{2}\exp (\frac{h}{2}{\hat{\varvec{\upomega }}}^{n+\frac{1}{2}})\mathbf{T }^{n+1})),\nonumber \end{aligned}$$

(6.42c)

where the forcing torque term T in (6.42c) is introduced at the both ends of the integration interval in order to obtain better conservation properties of the algorithm [35]. By inspection of (6.42), it is clear that for a free spinning body ($\mathbf{T }=0$) the relation $\mathbf{R }^{n+1}\mathbf{I }{\varvec{\upomega }}^{n+1}=\mathbf{R }^{n}\mathbf{I }{\varvec{\upomega }}^{n}$ is valid. That is, the proposed algorithm exactly preserves spatial angular momentum of a free body. This is confirmed in Fig. 6.6, where it is visible that preservation of the spatial angular momentum $\mathbf{Y }=\mathbf{RI }{\varvec{\upomega }}$ of a free-spinning body is satisfied and it is independent of the integration step-length.

Fig. 6.6

Convergence in the norm of the error in the spatial angular momentum of a free body rotational motion

Technically, Fig. 6.6 shows the convergence of the proposed formulation in the norm$\left\| {\mathbf{Y }-\mathbf{Y }_{\text {converged}} } \right\| _{2}$, where the reference value $\mathbf{Y }_{\text {converged}} =\mathbf{Y }(t=1)$ has been computed with a step size of $h=1e-5$. The norms of the error are evaluated at step sizes 8, 4, 2, 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128, 1/256, 1/512, 1/1024, 1/2048 and 1/4096. The other integration algorithms, whose results are presented in Fig. 6.6, are semi-explicit 2nd order Newmark method written in Lie-group setting [15] (currently one of the best-performing 2nd order geometric algorithms for rigid body rotational dynamics) and 4th order Runge-Kutta Munthe-Kaas method for the ODE integration on Lie groups. The angular velocity initial condition is set to ${\varvec{\upomega }}_0 =\left[ {{\begin{array}{lll} {0.45549} &{} {0.82623} &{} {0.03476} \\ \end{array} }} \right] ^{T}$ and the rigid body inertia tensor is given by matrix $\mathbf{I }=\text {diag}(0.9144, 1.098, 1.66)$.

Fig. 6.7

Convergence in the norm of the error in the rotation matrix (Lagrangian top)

The Fig. 6.7 illustrates the convergence of the Lie-Störmer-Verlet scheme for Lagrangian top in the norm of the error in the rotation matrix $\left\| {\mathbf{R }-\mathbf{R }_{\text {converged}} } \right\| _2$ for decreasing values of the integration step $h$(1/64, 1/128, 1/256, 1/512, 1/1024, 1/2048, 1/4096, 1/8192, 1/16384, 1/32768 and 1/65536). The reference solution for the rotation matrix $\mathbf{R }_{\text {converged}} =\mathbf{R }(t=1)$ is computed using the step size $h=1e-5$. In this figure, one can see that the proposed formulation exhibits a second-order convergence (graph ‘Lie-Störmer’). Also, the proposed algorithm clearly outperforms the Lie-Newmark and RATTLE algorithms (RATTLE algorithm is $2\mathrm{{nd}}$ order integration method for the rotational dynamics of a rigid body [17], whose starting point is also Störmer-Verlet scheme), while the RK-MK4 integration scheme, as a $4\mathrm{{th}}$ order scheme, yields the best accuracy, as expected.

In this example, the configuration space of heavy top is SO(3) [21] and dynamical model is formulated in the classical ODE form on the basis of Euler’s rotational equation: dynamical equilibrium of the top rotation around the base point is expressed in terms of $\dot{\varvec{\upomega }}$, rotation tensor and gravity force. In the standard units, the inertia tensor with respect to the fixed point has been set as $\mathbf{I }_{\text {FP}} =\text {diag}(\text {15.234375},\text {0.46875},\text { 15.234375})$, the body mass is set as 15 and the standard gravity acceleration is applied at the local reference point at unity distance along the axis. As it is shown in [35], the presented Lie-Störmer-Verlet method also preserves other integrals of motion of Lagrangian top algorithm in a very satisfactorily manner.

6.3.3 Higher-Order Coadjoint-Preserving Integration Scheme with the Simultaneous Kinematic Reconstruction on SO(3)

The modified Störmer-Verlet scheme in Lie group setting, described in the previous chapter, is a 2nd order integration method. In order to construct the coadjoint orbit preserving integration scheme of a higher order of accuracy, the instantaneous rotation vector ${\varvec{\uppsi }}(t)\in {\varvec{\mathcal {R}}}^{3}$ in (6.38) should be determined by solving ODE in the Lie algebra (6.39) via higher order integration algorithm. Here, any classical vector space higher order integration method can be used [8, 14].

Furthermore, in addition to an update step at the dynamical level on the basis of (6.34) or (6.36), the same instantaneous rotation vector ${\varvec{\uppsi }}(t)\in {\varvec{\mathcal {R}}}^{3}$ [that is determined for the step by solving (6.39)] can be also used for the step kinematic reconstruction on SO(3). This stems from the fact that the SO(3) kinematic reconstruction equation

$$\begin{aligned} \dot{\mathbf{R }}(t)=\mathbf{R }(t)\hat{\varvec{\upomega }}(t), \end{aligned}$$

(6.43)

has the same mathematical structure as Lie-Poisson’s system given by (6.31) or (6.32), meaning that it’s update can be written in the same form as (6.34) or (6.36). Indeed, for the n-th step we can write

$$\begin{aligned} \mathbf{R }_{n+1} =\mathbf{R }_n \mathbf{Q }(t)=\mathbf{R }_n \exp (\hat{\varvec{\uppsi }}(t)), \quad t\ge t_n, \end{aligned}$$

(6.44)

where $\hat{\varvec{\uppsi }}(t)\in so(3)$ is a solution of the ODE system in the Lie-algebra (6.39). This means that for the n-th step, the ODE system (6.39) has to be solved only ones and then both updates at the dynamical and kinematical level can be calculated in a straightforward manner by using (6.34) and (6.44) respectively. This makes the proposed scheme particularly efficient.

Since the update at the dynamical level is based on the coadjoint orbit preserving update (6.34), the magnitude of the angular momentum of a free-body in the body-attached frame will be ‘exactly’ conserved, no matter which integration method is used for solving (6.39). Moreover, by closer inspection of the algorithm, it is clear that for a free spinning body the relation $\mathbf{R }^{n+1}\mathbf{y }^{n+1}=\mathbf{R }^{n}\mathbf{y }^{n}$ is satisfied. That is, the proposed algorithm exactly preserves spatial angular momentum of a free body (similarly as the modified Störmer-Verlet scheme in the previous chapter).

For the integration purposes, in this chapter we have used 4th and 2nd order Runge-Kutta (RK) method i.e. the whole algorithm is based on the Munthe-Kaas method [14, 25] that operates on the Lie-group and uses aforementioned RK algorithm for solving the Lie algebra ODE equation (6.39).

The conservative character of the algorithm is shown in Fig. 6.8, where it is visible that preservation of the spatial angular momentum of a free-spinning body is independent of the integration step-length. Also, as it is expected since this integral of motion is exactly preserved by the proposed method (see above), this is equally true for the both order of the tested accuracies (RK-MK4 and RK-MK2).

Fig. 6.8

Convergence in the norm of the error in the spatial angular momentum of a free body rotational motion

Fig. 6.9

Convergence in the norm of the error in the rotation matrix

The 4th and 2nd order of accuracy (which is dependent on the RK method that is used for solving (6.39), which also, in turn, determines how many terms will be used at RHS in (6.40) for the $\text {dexp}_{-{\hat{\varvec{\uppsi }}}}^{-1}$ operator [12]) of the described algorithm are also visible in Fig. 6.9. Here, the both versions are compared within the framework of integration of the free rigid body rotation with the well-known 2nd order geometric algorithms described in the Refs. [1, 3, 32]. Figures 6.8 and 6.9 show the convergence of the proposed formulation in the norms $\left\| {\mathbf{Y }-\mathbf{Y }_{\text {converged}} } \right\| _2$, and $\left\| {\mathbf{R }-\mathbf{R }_{\text {converged}} } \right\| _2$ for the freely spinning body, where the spatial angular momentum $\mathbf{Y }_{\text {converged}} =\mathbf{Y }(t=10)$ and the rotation matrix $\mathbf{R }_{\text {converged}} =\mathbf{R }(t=10)$ have been computed with a step size of $h=1e-3$. The norms of the error are evaluated at step sizes 8, 4, 2, 1, 1/2, 1/4, 1/8, 1/16, 1/32, 1/64, 1/128 and 1/256.

6.4 Conclusion

A formulation of the motion equations of the constrained MBS as index 1 DAE system on a Lie group has been presented. It is shown that this system can be solved numerically with the Lie group ODE integration methods following the established MBS methodology. In this chapter, the equations are treated with the MK integration scheme and the Lie-group integration scheme that is based on the Störmer-Verlet algorithm with the direct SO(3) upgrade. This gives rise to a coordinate-free, thus singularity-free, modelling and integration of MBS motion equations.

The Lie-group-setting further provides a framework for the design of geometric integration schemes that preserve coadjoint orbits and conserve integrals of motion. To this end, the Lie-Störmer-Verlet integration scheme is presented. The method is 2nd order accurate and it is angular momentum preserving (it exactly preserves spatial angular momentum of a free body and magnitude of a free-body angular momentum in the body attached frame). Although the method is fully explicit, it generally outperforms two of the best-performing integral-conserving schemes of the 2nd order of accuracy (the semi-explicit Lie-Newmark algorithm and the implicit RATTLE algorithm). The method also performs better than the 4th order explicit RK-MK4 integration algorithm in terms of the conservation of the rigid body motion integrals (free body angular momentum and Lagrangian top motion integrals [35]). The higher-order coadjoint-preserving integration scheme with the simultaneous kinematic reconstruction on SO(3) is discussed in the last part of the chapter. Like the Lie-Störmer-Verlet scheme, this algorithm exactly preserves spatial angular momentum of a free body. It attains a numerically efficient form that makes it easily to be applied to MBS simulations.