CHAINS OF RIGID BODIES AND THEIR NUMERICAL SIMULATION BY LOCAL FRAME METHODS

. We consider the dynamics and numerical simulation of systems of linked rigid bodies (chains). We describe the system using the moving frame method approach of [18]. In this framework, the dynamics of the j th body is described in a frame relative to the ( j − 1)th one. Starting from the La- grangian formulation of the system on SO(3) N , the ﬁnal dynamic formulation is obtained by variational calculus on Lie groups. The obtained system is solved by using unit quaternions to represent rotations and numerical methods preserving quadratic integrals.

1. Introduction. We consider the dynamics and numerical simulation of systems of linked rigid bodies (chains). We describe the chain system using the moving frame method (MFM) of [18] which is a Lagrangian variational approach that yields Newton-type equation. The approach is targeted to engineering students in order to give an elementary understanding of how to do constraint variations on Lie groups in order to build more complex dynamical systems using simple building blocks [10].
There are two main approaches to computational rigid body mechanics, a Lagrangian (variational) approach and a Newtonian approach. Most engineering applications use a Newtonian approach, formulating the problem as the Newton-Euler equations. In this approach, the main tool is Newton's second law. Reaction forces have to be explicitly considered (which might be or might be not important, depending on the application), and the equations may result in a DAE systems, or ODE systems, provided that the reaction forces are embedded in the algorithm as intermediate variables [5]. If carefully implemented using a recursive algorithm, the resulting equations can be solved in O(N ) operations, where N is the number of rigid bodies, see for instance [19,5,15,4,22]. This complexity was achieved without using the knowledge of Lie theory. Generally, the complexity of the algorithms depends strongly on the formulation and the choice of variables and coordinate systems to represent the system. For reasons that are discussed in great detail in the relevant literature, most authors and practitioners in classical multibody dynamics follow the Newton-Euler approach and the recursive method, implemented in several research and industrial software packages.
Geometric Mechanics embraces instead the Lagrangian variational approach. In the Lagrangian approach, reaction forces are not needed because they are inherited by the formulation of the system. Holonomic constraints are dealt with either using Lagrange multipliers or using generalized coordinates, the minimal number of independent coordinates to describe the system.
Lie groups and Lie algebra methods have been used in the Lagrangian approach for several decades, see [16] for some history, and in the Newtonian approach to mechanics for at least three decades, see for instance the review article [17] and references therein.
The MFM is derived using the Lagrangian approach. It uses orthogonal matrices to describe rotations and skew-symmetric matrices to describe vector products of the type ω × v. The systems are constructed by adding a body at a time and the kinematic relations are obtained by describing each body orientation in the frame relative to the previous one. Conservative and non-conservative forces acting on the centers of mass are described in the inertial system, while torques are described in local body frames. The motion of each body is typically a rigid rotation or a translation.
The core of the MFM consists in relating the coordinates X used to describe the system to the generalized coordinates q, which are the independent coordinates in which variations are done. This is accomplished through the introduction of the B matrix, satisfying δX = Bδq andẊ = Bq, and its derivativeḂ. Problems treated with this method so far consist of few bodies, the rotations being either planar or treated by successive single-angle rotations (Euler angles), see for instance [18,6,9]. The resulting expressions for B andḂ are typically long expressions of sine and cosines and would typically be generated by a symbolic manipulator. However, beyond three or four joints, these expressions grow so large and unwieldy that even a powerful personal computer would have problems storing the symbolic expression for B andḂ [20]. Another issue with the application of Euler angles is the presence of singularities in the equations of motion when studying rotations with three degrees of freedom (for instance the gimbal lock) and produce significant difficulties.
The goal of this paper is to formulate the MFM approach using the theory of Lie groups and Lie algebras 1 to obtain a block matrix formulation for B andḂ that: i) is amenable for spherical joints (and not only planar ones); and ii) is amenable for an arbitrary number N of joints. Translations can also be considered in addition to rotations (prismatic joints) with some minor adjustments, as well as constraint rotations and tree-structures of rigid bodies, but, for sake of simplicity, in this paper we consider only chains of bodies with spherical joints.
Using computations on Lie groups and Lie algebras [12], the long expressions with sines and cosines are replaced by block matrices. Our preliminary experiments indicate that the cost of the approach is O(N 2 ), which is common to many methods in mechanical engineering. An optimization of the algorithm has been outside the scope of the present paper. It can be interesting to investigate whether a recursive approach that leads to O(N ) computations can be applied also to the proposed formulation.
The structure of the paper is as follows. In Section 2 we introduce the MFM of [18] and we show the variational derivation of the equation of motions in the general setting. We then describe the relation between the coordinates X used to describe the systems and the choice of the generalized coordinates q (rotation of frame j relative to frame j − 1). This relation is expressed as for both the variations and the velocities. This relation is at the core of the MFM. In Section 3 we derive the systematic expressions for B andḂ and formulate these as block matrices. We further introduce the quaternion notation for the rotations and derive the final form of the equations. Quaternions have also the advantage of removing the charts singularities due to the Euler angles. In Section 4 we show some numerical examples and applications to several type of problems and finally, in Section 5 we present some concluding remarks. 2. Background and notation. We consider a variational principle that allows for external and non conservative forces. Let L(X,Ẋ) be the Lagrangian of the system, L = K − U , where K is the kinetic energy and and U is a potential, and let W be the work done by external forces F including non-conservative forces. Then the motion of the system from time t 0 to time T is such that where δW is the virtual work of the forces in F . The typical approach of the MFM is to use a set of coordinates where x is the vector with the coordinates of the center of masses of the bodies in the inertial system, and θ represents one-dimensional rotation (the bodies can rotate only along a single given axis). As the bodies are rigid and linked to each other, these variables are not independent. Therefore, a central part to the derivation of the Euler-Lagrange equations in [18] relies on the fact that the velocities and virtual displacements are linearly related to the generalized velocity and generalized virtual displacement respectively. That is, where now the q are independent variables (generalized coordinates) with no constraints. It is precisely the computation of the matrix B (and its derivativeḂ) that in the original approach consists in complicated expressions of sine and cosine functions and that here we shall construct in a more general manner. As the variables are rotations, there are several ways to do variations. One way is to use constraints and Lagrange multipliers [7]. For Lie groups, which are usually parametrized using Lie-algebra coordinates as independent variables, the variations need to take into account the curvature of the underlying manifold, see for instance [14] for a thorough treatment of variations on manifolds.
For completeness, we summarize the derivation of the equations from [18], which uses the Lie-group formulation and the relations (1)- (2). The equations in [18] have been derived for one-parameter rotations, but it is straightforward to see the procedure extends in a similar manner to arbitrary rotations. We have T t0 δẊ MẊ + δX F dt = 0, (the mass-matrix M is symmetric). Since X contains both translational and rotational variables, we obtain where D is a block-diagonal matrix with zero blocks on the diagonal for the translational variables and skew-symmetric 3 × 3 blocks for the angular variables. The matrix D corresponds to the ad-operator when doing variations on Lie groups, see [14]. Performing integration by parts and enforcing the boundary conditions, we obtain Next, using the linear relationships (1) and (2) between theẊ and the generalized coordinatesq, Imposing that the above holds for all possible variations δq, we obtain the Euler-Lagrange equations: which we rewrite in the following form (Newton-type equations) where Note that B,Ḃ and D depend themselves on the generalized coordinates, therefore M * , N * and F * are time-dependent.

2.2.
Coordinates and generalized coordinates. Following [18], the translational and angular velocity and virtual displacements are assembled in the vectorṡ respectively. Similarly, the force vector is given as where f j forces apply to the center of mass the inertial frame (for instance gravity), while the τ j are torques and are applied in the local frame. The generalized coordinates can include linear and rotational variables. Linear variables could be for instance the centers of mass of prismatic joints. The coordinates of body j are resolved with respect to body j − 1 (explaining the name moving frame method ). This approach is also called mixed form representation in multibody system dynamics (see [17], pg. 56). Similarly, the generalized velocity and virtual essential displacement of the generalized coordinates are assembled in a vector: where we abuse notation denoting everything asq. It must be stressed for the translative generalized coordinates, theq j are the usual derivatives. In the case of rotational generalized coordinates (and in general Lie-group type variables), theq j are trivialized derivative in the algebra (angular velocities), and not in the tangent space.
From this point, we assume that the generalized variables are q j ≡ R j,j−1 , representing the relative rotation of body j with respect to the frame of body j − 1. The variable q j is naturally described by an element in the Lie group SO(3) of orthogonal matrices. We have Having introduced the generalized coordinates, the variation of the velocities can be written in the form where the 0 denotes the 3 × 3 zero matrix, and the hat-map represents the usual map that transforms a 3-dimensional vector to a 3 × 3 skew-symmetric matrix describing the exterior vector product [12,14]. Note that we are abusing notation, as theq j will be angular velocities.
With the above definition of the X, the kinetic energy of the system is easily expressed by the quadratic form: where M is the symmetric positive definite block-matrix where m j is the mass of body j, I is the 3 × 3 identity and the J j s are the moment of inertia tensors relative to the fixed body frame. The virtual work is given by In the MFM setting, the choice of generalized coordinates is q j ≡ R j,j−1 , the rotation of frame j relative to frame j − 1. These are parametrized with the help of the ω j,j−1 ∈ R 3 , so thatq 3. The B andḂ matrices.
3.1. Computing B. Let x j be the position of the jth center of mass at time t in the inertial system. We denote by s j,1 the vector determining center of mass j (cm j ) from the end of body j − 1 and s j,2 the vector from cm j to body j + 1, see Fig. 1. Both s j,1 and s j,2 are in local coordinates and are fixed in the jth body system. For the positions of the centers of mass in the inertial system, we have where R j is the overall rotation of body j with respect to the inertial system. Thus, where R j,j−1 is the relative rotation of frame j with respect to frame j − 1. Furthermore, we denote by the product of the relative rotation matrices from body j to body k ≥ j, that can be interpreted as the rotation of body k relative to the frame fixed with body j. Locally, we haveṘ j,j−1 = R j,j−1 ω j,j−1 , therefore, for the global rotation matrices, we haveṘ is a linear operator in ξ, see [12]. Identifying the right hand sides above with the right hand sides ofṘ j = R j ω j , we see that Since Ad Q ( · ) is linear in its argument and moreover for any 3 × 3 orthogonal matrix Q [12], the above relation becomes Setting up together, we can write where L R is the lower triangular matrix relating the angular velocities in the inertial frame to those in the local frames. Introducing the block matrices and (where the latter hat-map is intended blockwise), we havė To relate the velocities v j to theq j ≡ ω j,j−1 , we observe thaṫ x j =ẋ j−1 +Ṙ j−1 s j−1,2 +Ṙ j s j,1 .
In particular, for x 1 , we havė For the general term, we have already computed theṘ j . Furthermore, to make the above relation linear in the ω j s, we use (14) together with uv = − vu, to obtaiṅ We introduce the following block diagonal matrices (see Fig. 1), and the block shift matrix We have the relationẋ The matrix I 3N − ∆ − is invertible, moreover, we observe that where 3.2. TheḂ matrix. By direct differentiation, we havė From above, we haveṘ = R Ω. Then, the most difficult part is to findL R . Recall that the matrix L R is made of blocks of the form R j,i = (R i+1,i · · · R j,j−1 ) , j > i. We observe that Recalling the definition of Ω, we recognize that the above equations can be written in matrix form asL where ω j,i can be intended as the angular velocity for body j relative to the body frame i, as R j,i is the rotation matrix of body j relavive to the body frame i, so thatḂ rot j,i = − ω j,i R j,i . In summary, we havė 3.3. Setting up the equations using the quaternion formalism. As mentioned before, the generalized coordinates q j ≡ R j,j−1 are the relative rotations of body j relative to the body frame j − 1. To represent the rotations, we use unit quaternions. Letting H being the set of quaternions, we abuse notation and identify where q j,0 ∈ R and q j,v ∈ R 3 are the scalar and the vector components of the unit quaternion, respectively. Unit quaternions obey the differential equatioṅ where * represents the quaternion product and ξ j,v = ω j,j−1 ≡q j ∈ R 3 is the angular velocity vector. The relative rotations q j ≡ R j,j−1 can be reconstructed using the formulas R j,j−1 = I + 2q j,0 q j,v + 2 q 2 j,v . Writing we see that the quaternion q j can be also interpreted as a rotation by an angle θ j around the axis determined by the direction n ∈ R 3 .
Recall that the quaternion product can be expressed in terms of matrix-vector products, in particular, if r = [r 0 , r v ] T ∈ H, p ∈ H, one can write We denote and introduce the block-diagonal matrix Q = diag(Q(q 1 ), . . . , Q(q N )) where Q(q j ) is as in (28). In the quaternion variables, the system (5) readṡ with M * , N * , F * as in (6). We observe that the matrix M * is symmetric positive, since M * = B M B, where the lower block of B is L R which has identities blocks on the diagonal and is therefore full rank. This implies that B is full rank, therefore the map y → By is an injective map, mapping nonzero vectors to nonzero vectors. This implies that y M * y = y B M By = (By) M (By) > 0 ∀y = 0, since M is positive definite.
It follows that M * is invertible, and introducing the variable y = q ξ v the system can be written as a first order systeṁ 4. Application to the numerical integration of chains of rigid bodies. The advantages of the quaternion formulation is that: 1) it reduces the storage compared to using rotation matrices directly, 2) the unitarity of the quaternion is a quadratic conservation law, therefore it can be preserved by symplectic methods [21,8] (although this holds also for rotation matrices). Methods that do not preserve quadratic conservation laws would require a projection step or a very small stepsize to reduce the local error hence the unitarity error. Alternatively, one could use methods for orthogonal flows [11,2,13] or with Lie-group methods [3,12,1] but this is beside the scope of the present paper. As a proof of concept, we will use Gauss-Legendre Runge-Kutta methods, which are implicit. To justify the choice, Table 1 shows a comparison between the 2-stages GLRK (order 4) and RK45 for a four-body pendulum in §4.3 only under the effect of gravity (no springs/dampers). The RK45 method is implemented with relative error tolerance rtol = 1e-13 and absolute tolerance atol = 1e-13. The GLRK2 is implemented with a stepsize h = 0.01 and a tolerance rtol = 1e-9 on the solution of the implicit iterations (by fixed point). The solutions of the two methods are compared at time T = 10.
The cost of the algorithm will depend on the number N of links. Table 2 gives the timings of a N -body pendulum displaced by θ 0 = 0.1 about the x-axis in the first body. The system is integrated up to T = 10 with a 3-stages GLRK (order 6) with stepsize h = 0.01. The computational cost grows quadratically with the number of links N . This is reasonable, as the matrix L R increases by a block-row at each time. Although we only add a rotation, we need also the relative rotations with respect to each of the previous N − 1 bodies.
In what follows, the numerical simulations are performed both in Matlab and Python. In Matlab, the reference solutions are computed using ode45, setting the relative tolerance to RelTol = 2.22045e-14 and absolute tolerance to AbsTol =1e-14. In Python, the reference solutions are computed using the built-in package scipy.integrate and the method solve ivp, using an explicit RK45 method with relative error tolerance rtol = 1e-13 and absolute tolerance atol = 1e-13.

Solid pendulum.
A pendulum shaped as a long narrow rod is fixed at point O (origin of the inertial system) and free to rotate. At rest position, the center of mass is located at l/2e 3 in the inertial frame (l cm = l/2). If the pendulum is displaced from the rest position by an angle θ 0 and released (with zero initial velocity), the motion is planar and consists of rotation by an angle θ in the yz-plane (vertical plane, rotation about the x axis). Using a classical paramterization by an angle θ from the vertical position, the equations of the motion of the pendulum are given as where m is the mass of the pendulum, g the gravity constant and J O is the moment of inertia about the origin, J O = 1 3 ml 2 .  In the MFM, the B,Ḃ, D and M matrices are trivial to formulate for this example, where now J is the matrix of the principal moments of inertia in the body frame, where ε is a small number (we assume that the width of the prism is very small compared to its length l). In this set up, the force is as there is no external torque. The vector y in (30) is simply where q is the rotation quaternion and ω 1 = ω 1,0 is the angular velocity of the body relative to the inertial system. Writing down explicitly the differential equation for ω 1 using (30) we see that these are equivalent to . We recognize the parallel axis theorem on the left hand side and the torque exerted by gravity on the center of mass on the right hand side, thus obtaining a vector version of (31).
In Figure 3 we compare the solution of the MFM with the exact solution θ from (31). For the MFM we use the Implicit Midpoint Rule (IMR, Gauss-Legendre method of order 2) implemented with stepsize h = 0.01. The angle θ is reconstructed as θ = 2 arccos(q 0 ), see (27). The numerical error is as expected from theory. The numerical solution recovers well the planar motion although the simulation is in full 3D. Clearly, the method is an overkill for this simple planar example, however, it shows how straightforward it is to set up full 3D computations with much more complicated forces and torques by merely changing f 1 and τ 1 in (33).
For the simulations, we have used l = 2 m, and m = 50 kg and initial condition θ 0 = 0.1.

Spinning top.
To illustrate the ease of the implementation of the method, we consider the case of a symmetric heavy top. We consider a top idealized as a solid rod (radius r rod = 0.05m, h rod = 0.4m) with a solid disk on top (radius r disk = 0.5m, h disk = 0.05m) both of constant density ρ = 1000kg/m 3 . The total mass of the body is m ≈ 28.274kg, the center of mass is situated at 0.4m along the main symmetry axis of the body and the inertia matrix in body coordinates centered at the center of mass is The inertia matrix is obtained by considering the individual inertia matrices of the rod and the disk, which are then translated to the center of mass using the parallel axis theorem. The form of the matrices B,Ḃ, M, D for the system are precisely those of the pendulum (32), and so are the forces (33). The only difference is in the initial conditions and the parameter values. We use q 0 = [cos(θ 0 /2), sin(θ 0 /2)n] , θ 0 = π 6 , n = [1, 0, 0] and ξ v,0 = [0, 0, 25] (corresponding toθ 0 = 25). A simulation with these parameter (T = 30, h = 0.01) can be found in Figure 4, an animation, with slightly different parameters, can be found here: https://youtu.be/cOL68IbniPE. Let u = [θ j , φ j , ψ j ] be the rotational displacement away from the neutral position of body j (in body coordinates). These displacements are computed by converting the rotation matrix R j−1,j into angle displacements using the relation R = exp( u). Then u = log R = sin −1 y y y, where y = 1 2 (R − R ), see [12]. Each displacement will result in a torsional force applied to body j and a corresponging where k j is the torsional spring coefficient. The spring coefficients are k 1 = 4000 kg·m s 2 , k 2 = 3000 kg·m s 2 , k 3 = 2000 kg·m s 2 and k 4 = 1000 kg·m s 2 . Initially, the bodies are rotated first about the first axis and then the second axis by the same angle θ 0 , q 1 (t 0 ) = q 2 (t 0 ) = q 3 (t 0 ) = q 4 (t 0 ) = cos( θ0 2 ) sin( θ0 2 )e 1 * cos( θ0 2 ) sin( θ0 2 )e 2 The initial angular velocities are zero, ξ j,v (t 0 ) = 0, j = 1, . . . , 4.
The system is integrated with a 3-stages GLRK for t 0 = 0 < t 1 < · · · < t N = T with [t 0 , T ] = [0, 15] and time step h = 0.01. An animation of the system solution is available at https://youtu.be/EA6zu04TB7I 4.4. A 16-body pendulum. This system consists of 16 linked bodies where the first link is hinged to the origin O of the inertial system. The hinges are all spherical, allowing the links to rotate in any direction.
The dynamic system results in a a 122-term vector ODE system evolving on SO(3) 16 . The initial condition for the first body is q 1 (t 0 ) is expressed by (27) with θ 0 = π 8 and the remaining coordinates without any displacement. The results from integration with the 2-stages GLRK method are displayed in Figure 7. As we expected, the quaternion unit length is preserved to the level of machine accuracy (GLRK method preserve quadratic integrals). Similarly also the relative error of the system is very well preserved in the numerical simulation. Several types of external forces may be applied to the model. Let us consider a generic number of links N . We consider a spring-damper with neutral length l. The spring is attached at a point P situated at the end of body N , with coordinates x p = x N + R N s N,2 in the inertial frame, x N being the coordinates of the N th center of mass in the inertial frame. Furthermore, let x cp be the coordinates of the attachement of the other end of the spring to the inertial system. Let s = x cp − x p , so that σ = ||s|| − l is the stretch of the spring and n = s/||s|| is the unit vector in the direction of the displacement. The spring force is then f s = −k s σ n. Similarly, the damping is given as f d = −k d (v P n)n. The constants k s and k d are the spring and the damper coefficients, respectively. P Figure 8. Spring loaded/damped chain. The red vector represents n, the unit vector in the direction of the displacement of the spring.
This force f s and the damping f d generate a moment (torque) on the N th body. In local coordinates, this torque is given as An animation of this system can be found at https://youtu.be/a_ibo_unOog.

4.5.
A 64-body pendulum. As an example of a significantly longer system is a 64-body system. The first body is hinged to the origin of the inertial system, Figure 9. 64-body pendulum and every subsequent body is hinged at the end of the previous body and free to rotate in any direction. The dynamic system is a 672-term vector ODE system generating a solution on SO(3) 16 . The pendulum is coiled in the xy-plane and released under the effect of gravity. Torsional dampers are attached at the joints to reduce oscillations due to the stiffness of the system. We use the same body parameters and intertia moments as for the previous experiment, the joint-damper coefficients are 70 kg s . An animation of the 64-body pendulum falling from rest is available at https://youtu.be/efhnb3jQkas 5. Conclusions. In this paper we have generalized the MFM of [18] to arbitrary systems of linked rigid bodies, allowing the bodies to rotate in all three dimensions. Further, we have systematized the way to construct the B and theḂ matrices as block matrices for numerical simulations. The formulation does not require any form of symbolic manipulation as the matrices B andḂ can be computed on the go and therefore it allows the simulation of larger systems, compared to earlier approaches [20].
The generalized coordinates of the systems are the local frame rotations (of frame j with respect to frame j − 1). We further use a unit-quaternion representation of the rotations that has several advantages. First of all it avoids complications due to multiple elementary rotations and singularities, secondly has the advantage that the unity of quaternions is a quadratic invariant, therefore easily preserved by existing quadratic-preserving methods.
There are many possible extension of the present work. An extension to prismatic joints as well as rotations with angle restrictions is quite straightforward, but was left out for simplicity sake. Further, we have observed that the complexity of the present approach is quadratic with the number of joints, but a thorough investigation of the complexity and optimization of the method has not be considered. In particular, it would be interesting to investigate whether it is possible to apply recursive methods to obtain O(N ) computational complexity, like in the current Newton-Euler formulation of mechanics [5]. In addition, the Lagrangian approach can be generalized to a discrete variational approach by doing an appropriate discretization of the Lagrangian and doing variation with respect to the discrete variables in the spirit of [14]. In this way, the discrete system inherits the underlying geometrical properties of the original system. This approach might be particularly interesting for long time simulations where it is important to conserve energy and/or momentum or other conserved quantities of the system, or in the context of flexible bodies and nonliner beam theory.