A Lie Group Variational Integrator for Rigid Body Motion in SE(3) with Applications to Underwater Vehicle Dynamics Nikolaj Nordkvist and Amit K. Sanyal Abstract— The topic of variational integrators for mechanical systems whose dynamics evolve on nonlinear spaces has seen strong growth recently. Within this class of variational integrators is the subclass of Lie group variational integrators that can be used for mechanical systems whose dynamics evolve on Lie groups. This class of mechanical systems includes all systems that can be modeled as rigid bodies or connections of rigid bodies. In this paper, we present a Lie group variational integrator for the full (translation and orientation) motion of a rigid body under the possible influence of nonconservative forces and torques. We use a discretization scheme for such systems which is based on the discrete Lagrange-d’Alembert principle to obtain the Lie group variational integrator. We apply the composition of the Lie group variational integrator with its adjoint and a Crouch-Grossman method to the example of a conservative underwater system. We show numerically that with respect to energy these manifold methods, as expected, behave as a symplectic integrator and a nonsymplectic integrator, respectively.
I. I NTRODUCTION The dynamics of a rigid body in the presence of conservative and nonconservative forces and torques is an important problem modeling the time evolution of, controlled or uncontrolled, spacecraft, aerospace and underwater vehicles, and mobile robots, for example. In this paper we develop a so-called Lie group variational integrator as a discretization of the equations of motion for the rigid body in SE(3) (translation and rotation). The idea behind variational integrators is to discretize the variational principles of mechanics: Hamilton’s principle for a conservative system or the Lagrange-d’Alembert principle for a system with nonconservative forcing [1], [2]. Since we wish to include nonconservative forces in our analysis we apply the latter approach. Discrete variational mechanics was studied in [3] and the discrete Euler-Poincar´e equation for reduced dynamics of a mechanical system on a Lie group was given in [4]. Lie group variational integrators for a rigid body in a conservative force field were developed in [5], [6], [7]. The theory of Hamilton-Pontryagin integrators, presented in [8], provides a general variational approach to mechanical systems. Lie group variational integrators for optimal control of rigid bodies have been dealt with in recent articles like [9], [10], [11]. Newmark-type algorithms on the Lie algebra of the Lie group of rigid body rotations for free rigid bodies were obtained in [12], [13]. Crouch-Grossmann methods, Nikolaj Nordkvist, Mechanical Engineering, University of Hawai‘i at M¯anoa, Honolulu, HI 96822
[email protected] Amit K. Sanyal, Mechanical Engineering, University of Hawai‘i at M¯anoa, Honolulu, HI 96822
[email protected]
introduced in [14], provide a generalization of Runge-Kutta methods to manifolds which can be directly applied to rigid body systems using the exponential map from the Lie algebra to the Lie group to generate flow updates. The exponential map was also used in [15], [16] to numerically obtain rigid body rotations. Advantages of Lie group variational integrators are twofold. They discretize the motion on the Lie group, without the need of local maps or projection, and they are variational in nature which imply conservation of a discrete momentum map when the system exhibits symmetry. Variational methods are also know to be symplectic, see [2], and symplectic integrators are known to show good energy behavior over exponentially long time intervals for conservative systems; see [17]. The Lie group variational integrator we derive here is a generalization of the integrator from [6]. This previous work applies to conservative rigid body systems with mass matrix M = mI, where I is the 3 × 3 identity matrix. Here we allow the mass matrix to have non identical elements and thus bodies submerged in a fluid, like underwater vehicles, where added masses need to be included can be analyzed. Furthermore we add the effects of nonconservative forces and torques, making it useful in the numerical analysis of rigid body control schemes. The Lie group variational integrator derived in [7] applies to the system consisting of three rigid bodies connected by ball joints and submerged in a fluid. A brief outline of this paper is given here. In Section II we give the (continuous) equations of motion of a rigid body in SE(3) discussing how they are derived using the (continuous) Lagrange-d’Alembert principle. Using the discrete Lagranged’Alembert principle we derive the Lie group variational integrator in Section III and we discuss its order and how to increase this by using its adjoint. In Section IV we apply the composition of the Lie group variational integrator with its adjoint to the example of a conservative underwater system and show that it performs better, with respect to long time conservation of energy, than a higher order CrouchGrossman method. We conclude the paper, in Section V, by summarizing the results. II. C ONTINUOUS E QUATIONS OF M OTION Here we describe the rigid body orientation and translation kinematics and dynamics model using rotation matrices for attitude representation. The configuration space for rigid body dynamics is the special Euclidean group SE(3). Any h R b element of SE(3) is given by 0 1 , where R ∈ SO(3) describes the orientation as a rotation matrix and b ∈ R3
is the position of the center of mass of the rigid body described in inertial frame coordinates. The Lie group SE(3) is the semidirect product of SO(3) and R3 and for the sake of simplicity we write elements as (b, R) ∈ SE(3). Therefore the state evolves in the 12 dimensional tangent bundle TSE(3). The space of skew symmetric 3×3 matrices is denoted by so(3), this is the Lie algebra of SO(3). The vector space isomorphism (·)× : R3 7→ so, defined by the the cross-product as x× y = x × y, identifies so(3) and R3 . We denote the Lie algebra of SE(3) which has h by se(3) i × elements Ω0 ν1 , where Ω, ν ∈ R3 , so we can identify this space with R6 ∋ (ν, Ω). The angular velocity corresponds to Ω and ν gives the translational velocity in body coordinates. We obtain the diffeomorphism T SE(3) ≃ SE(3) × se(3) by left trivialization and by identifying se(3) with R6 we get T SE(3) ≃ SE(3) × R6 . For the remainder of the paper, we represent the attitude state as an element of SE(3) × R6 . The kinematic equations describing the relation between b, R, ν, and Ω are given by R˙ = RΩ× ,
b˙ = Rν.
(1)
Let U (b, R) : SE(3) 7→ R denote the potential energy map. Defining the inner product as the trace pairing h·, ·i : Rn×m × Rn×m given by hA, Bi := tr(AT B), the Lagrangian for the motion of a rigid body in SE(3) is given by L(b, ν, R, Ω) =
1 1 hM ν, νi + hJΩ, Ωi − U (b, R), 2 2
(2)
where J is the inertia matrix and M is the mass matrix for the body. For many rigid body applications, e.g. spacecraft and asteroids, the mass matrix will be of the form M = mI, where m is the mass of the object, but for underwater vehicles the elements may be different due to the fact that added masses have to be taken into account. The dynamics of the system satisfies the (continuous) Lagrange-d’Alembert principle Z T Z T (hτ, Σi + hϕ, γi) dt = 0, L(b, ν, R, Ω)dt + δ 0
0
where τ and ϕ denote the nonconservative moments and forces, respectively, in the body frame. According to [18] the infinitesimal variations δb, δν, δR, and δΩ satisfy δb = Rγ, ×
δR = RΣ ,
δν = γ˙ + Ω× γ + ν × Σ, δΩ = Σ˙ + Ω× Σ,
where U (b, R), M(b, R) ∈ R3 are defined by ∂U (b, R), ∂b ∂U T ∂U . M(b, R)× := R − RT ∂R ∂R U (b, R) := −RT
III. D ISCRETE E QUATIONS OF M OTION In this section, we develop a discrete time model for the dynamics of the rigid body described in the previous section, using a discrete variational approach. The discrete model we obtain is a Lie group variational integrator similar to the ones obtained in [5], [6], [11]. However, unlike these previous works, non-conservative feedback control torques are included in the rigid-body dynamics model. Also the mass matrix is not assumed to be a scalar multiple of the identity matrix, and the theory can therefore be applied to underwater vehicle dynamics—unlike these previous works. The Lie group variational integrator for the rigid body dynamics is derived using the discrete Lagrange-d’Alembert principle. A. The Discrete Lagrange-d’Alembert Principle A variational integrator discretizes the Lagranged’Alembert principle (or Hamilton’s principle for a conservative systems) rather than the continuous equations of motion—which is the case for a generic Runge-Kutta method. For a thorough exposition of the discretization of Lagrange-d’Alembert principle, we direct our readers to [2]. The discrete version of the Lagrange-d’Alembert principle using generalized coordinates is also given in [1]. Let fk denote the discrete approximation to a continuous time-varying quantity f at time tk = t(k). We denote by h 6= 0 the fixed step size, i.e., tk+1 − tk = h. A discrete Lagrangian L approximates a segment of the action integral Lk ≈
tk+1
L(b, ν, R, Ω)dt. tk
Similarly we construct F to approximate a segment of the virtual work integral Fk ≈
Z
tk+1
(hτ, Σi + hRϕ, δbi) dt Z tk+1 1 × × τ , Σ + hRϕ, δbi dt, = 2 tk tk
where we have used ha, bi = 21 ha× , b× i for a, b ∈ R3 . The discrete dynamics is then prescribed by the discrete Lagrange-d’Alembert principle
where Σ and γ vanish at the end points but are otherwise arbitrary. This gives the (continuous) dynamics equations as δ M ν˙ =M ν × Ω + U (b, R) + ϕ, J Ω˙ =JΩ × Ω + M ν × ν + M(b, R) + τ,
Z
N −1 X k=0
Lk +
N −1 X
Fk = 0,
k=0
where Σ0 = ΣN = 0 and δb0 = δbN = 0.
(3)
B. Discretizing the Kinematic Equations Integrating the kinematic equation R˙ = RΩ× with the assumption of constant angular velocity in the time interval [tk , tk+1 ], we get Rk+1 = Rk Fk , where Fk = exp(fk× ) ∈ SO(3) is a first order approximation to exp(hΩ× k ), i.e., × Fk ≈ exp(hΩ× k ) ≈ I + hΩk .
(4)
By ensuring that Fk ∈ SO(3) we guarantee that Rk evolves on SO(3). Defining the modified inertia matrix J := 21 tr(J)I − J, where I is the 3 × 3 identity matrix, it can be verified that for any Ω ∈ R3 we have the identities (JΩ)× = Ω× J + J Ω× ,
hJΩ, Ωi = J Ω× , Ω× .
(5) (6)
We use (5) to impose a defining relation between Fk and Ωk by inserting the approximation (4) in (5) × T (JΩk )× = Ω× k J − J (Ωk ) 1 (Fk − I)J − J (FkT − I) ≈ h 1 = (Fk J − J FkT ). h
From the kinematic equation b˙ = Rν we get that the velocity b˙ k can be approximated by b˙ k ≈ Rk νk . Since b belongs to a linear space we thereby get the approximation bk+1 = hRk νk + bk . Hence a discrete counterpart of the kinematic equations becomes h(JΩk )× = Fk J − J FkT , Rk+1 = Rk Fk , bk+1 = hRk νk + bk .
(7) (8)
C. Discretizing the Dynamic Equations As the discrete Lagrangian we choose the approximation h 1 h 1 Lk = hM νk , νk i + J (Fk − I), (Fk − I) 2 2 h h h U (bk , Rk ) + U (bk+1 , Rk+1 ) , − 2
where we have used (6) in the definition of the (continuous) 1 Lagrangian (2) and Ω× k ≈ h (Fk − I). For F we choose an approximation of the form E D h × × T δb τk , Σk+1 + h ϕk , Rk+1 Fk = k+1 . 2
For this choice of L and F the discrete Lagrange-d’Alembert principle (3) gives N −1 X
1 hδFk , J (Fk − I)i + h hδνk , M νk i h k=0 + * ∂U h ∂U − , δRk + , δRk+1 2 ∂R k ∂R k+1 * + ∂U ∂U + , δbk + , δbk+1 ∂b k ∂b k+1 ! D E h × × T = 0. (9) + τk , Σk+1 + h ϕk , Rk+1 δbk+1 2
The variation of Fk can be computed from the definition Fk = RkT Rk+1 , using δRk = Rk Σ× k , to give × δFk = −Σ× k Fk + Fk Σk+1 .
Since the symmetric matrices are orthogonal to the skewsymmetric matrices we get E E D D T − Σ× , F T J . hδFk , J (Fk − I)i = Σ× k k+1 k , J Fk
Using (7) these elements can be rewritten as D D D E E E T = 1 Σ× , J F T − 1 Σ× , F T J Σ× , J F k k k k k k 2 2 h × = − Σk , (JΩk )× . 2
The variation of νk is calculated using (8) as 1 T R (bk+1 − bk ) δνk = δ h k 1 = RkT (δbk+1 − δbk ) − Σ× k νk . h
Using these identities and the end point conditions equation (9) becomes N −1 X 1 1 T × × × Σk , − (JΩk ) + Fk−1 (JΩk−1 ) Fk−1 2 2 k=1 × h − Σ× , ν × (M ν ) k k k 2 h × × + Σk , M(bk , Rk )× + τk−1 2 ∂U (bk , Rk ) + δbk , Rk−1 M νk−1 − Rk M νk − h ∂b ! h + hδbk , Rk ϕk−1 i = 0. 2 Using the identity F η × F T = (F η)× , for all η ∈ R3 and F ∈ SO(3), we thus obtain the discrete dynamical equations M νk+1 =FkT M νk + hUk+1 + hϕk , JΩk+1 =FkT JΩk + h(M νk+1 ) × νk+1 + hMk+1 + hτk , where Mk := Mk (bk , Rk ) and Uk = U (bk , Rk ).
D. Resulting Integrator The one-step method Φh giving (bk , νk , Rk , Ωk ) 7→ (bk+1 , νk+1 , Rk+1 , Ωk+1 ) is thus determined by the equations h(JΩk )× = Fk J − J FkT ,
(10)
Rk+1 = Rk Fk , bk+1 = hRk νk + bk ,
(11) (12)
M νk+1 = FkT M νk + hUk+1 + hϕk , JΩk+1
(13)
= FkT JΩk + hM νk+1 × νk+1 + hMk+1 + hτk . (14)
If we choose ϕk = αϕ(bk , νk , Rk , Ωk )+(1 − α)ϕ(bk+1 , νk , Rk+1 , Ωk ) , (15) τk = βτ (bk , νk , Rk , Ωk )+(1 − β)τ (bk+1 , νk , Rk+1 , Ωk ) , (16) where α, β ∈ [0, 1], equations (10)-(14) can be solved in the order they appear with (10) being the only implicit equation. For conservative systems with M = mI the integrator (10)-(14) is equivalent to the integrator described in [6]. This is most easily seen from the fact that the discrete Lagrangians, giving the two integrators, are equivalent when M = mI.
where ϕ∗k (τk∗ ) is ϕk (τk ) where k and k + 1 have been interchanged. Inserting (18) in (17) and using F η × F T = (F η)× we can rewrite these equations as T J −JF Fk+1 k+1 × = h JΩk + hM νk × νk + hMk + hτk∗ , (19) (20) M νk+1 = Fk+1 M νk + hUk + hϕ∗k , JΩk+1 = Fk+1 JΩk + hM νk × νk + hMk + hτk∗ , (21) T , Rk+1 = Rk Fk+1 bk+1 = hRk+1 νk+1 + bk .
(22) (23)
With the choice for ϕk and τk given in (15) and (16), respectively, the adjoint equations (19)-(23) becomes highly coupled implicit equations. When there are no nonconservative torques or forces, i.e. τ = 0 and ϕ = 0, we get that when solving these equation in the order the appear the only implicit equation is equation (19). The order of the adjoint method is the same as the original integrator, i.e., it is a first order method. Consider the composition method Ψh = Φ∗h/2 ◦ Φh/2 , see [2]. This integrator is by definition symmetric, i.e. Ψ∗h = Ψh , and therefore it must be of even order. Since Φh , defined by (10)-(14), is a first order method the method Ψh is therefore a second order integrator. G. Solving the Implicit Equation
E. Order of the Integrator exp(hfk× ),
3
Using Fk = fk ∈ R , and Taylor expanding fk gives when inserted in the implicit equation (10) that fk = Ωk + h2 J −1 (JΩk × Ωk ) + O(h2 ). Inserting this in the integrator (10)-(14) with (15) and (16) shows that this integrator is at least of first order. When the orientation is constant R = R0 and no nonconservative force ϕ is acting on the system the translational dynamics is given by bk+1 = bk + hR0 νk along with M νk+1 = M νk + hU (bk+1 , R0 ) which Euler method, see [2], for the system h i is the h symplectic i R0 ν b˙ = U (b,R0 ) . This confirms that the method indeed M ν˙ is a first order method F. Corresponding Adjoint Integrator The adjoint of an integrator Φh is the integrator defined by Φ∗h := Φ−1 −h , see [2]. The adjoint is obtained from the method by replacing h with −h and interchanging k and k + 1. The adjoint of the method (10)-(14) is therefore T , −h(JΩk+1 )× =Fk+1 J − J Fk+1 T , =Rk Fk+1
(17)
Rk+1 bk+1 =hRk+1 νk+1 + bk , M νk+1 =Fk+1 M νk + hUk + hϕ∗k , JΩk+1 =Fk+1 JΩk + hM νk × νk + hMk + hτk∗ , (18)
In this section we look into solution methods of the implicit equation in F ∈ SO(3) F J − J F T = a× ,
(24)
3
given a ∈ R . This equation appears in (10) and (19). In the paper by Moser and Veselov [3] an explicit solution to this problem is given. Define the polynomial P (λ) = det(I − λa× − λ2 J 2 ) which comes out to a third order polynomial in β = λ2 . We then define λ+ i as the square root of the ith root βi with positive real part we construct the + + diagonal matrix Λ = diag(λ+ 1 , λ2 , λ3 ) and define vectors + × 2 vi ∈ ker(I −λi a −βi J )\{0}, i = 1, 2, 3. The solution is then given by F = (JψΛψ −1 )T where ψ = [v1 , v2 , v3 ]. This approach only applies when all the roots βi are distinct and not purely real and negative (in which case λ+ i is not clearly defined). When a is small, which is the case our application for small h, and the elements of J (which is diagonal) are distinct these conditions are satisfied. In [19] explicit solutions are given in terms of solutions to an algebraic Riccati equation. A different approach is to solve the equation numerically using the Newton-Raphson method, as described in detail in, e.g., [6]. For this define F = exp(f × ) for some f ∈ R3 to convert it into a nonlinear equation on R3 . Using Rodrigues’ formula gives F = exp(f × ) = I +
sin kf k × 1 − cos kf k × 2 f + (f ) . kf k kf k2
−4
4
x 10
−4
5
x 10
3 0
2
∆E
∆E
−5
1
−10
0
−15
−1
−2
0
0.5
1
1.5
2
2.5
3
3.5
4
4.5
5
t
−20
0
50
100
150
t
Fig. 1. Time plots of the change in energy for the second order composition of the Lie group variational integrator (10)-(14) with its adjoint (solid) and a third order Crouch-Grossman method (dashed).
Inserting this into the implicit equation gives sin kf k × a× = (f J + J f × ) kf k 1 − cos kf k × 2 × 2 . (f ) J − J (f ) + kf k2
Using the identity (5) and (η × Jη)× = (η × )2 J − J (η × )2 , η ∈ R3 , equation (24) is seen to translate into the following implicit equation in f ∈ R3 1 − cos kf k sin kf k Jf + f × Jf. a= kf k kf k2 The differential, with respect to f , of the right hand side of this expression is given by sin kf k cos kf k sin kf k Jf f T J+ − kf k kf k2 kf k3 1 − cos kf k × + f J − (Jf )× 2 kf k 1 − cos kf k sin kf k (f × Jf )f T − 2 + kf k3 kf k4 Since this can be calculated numerically with minimal computational effort the Newton-Raphson method will serve as an efficient way of numerically solving the implicit equation (24). IV. N UMERICAL S IMULATIONS This section presents some numerical simulation results for a conservative underwater vehicle. In particular we apply the composition of the Lie group variational integrator with its adjoint and a third order Crouch-Grossman method to the example and compare results. We make the (crude) assumption that dissipative forces are negligible. The potential energy of an underwater vehicle is given by E D U (b, R) = −(m − ρV)gb3 + ρVg r, RT e3 ,
where m is the mass, g is the gravitational acceleration, ρ is the density of water, V is the volume of the body, and r ∈ R3 is the vector from the center of gravity to the center of buoyancy, expressed in the body fixed frame. As is standard in the underwater vehicle literature the positive b3 axis is taken to point downwards in the same direction as the force from gravity. This potential gives U (b, R) = RT (m − ρV)ge3 , M(b, R) = −ρVgr × (RT e3). In this simulation we take the mass to be m = 123.8kg and mass (including added masses) and inertia matrices to be M = mI + diag(65, 70, 75)kg, J = diag(5.46, 5.29, 5.72)kg · m2 . This corresponds to an ellipsoidal shaped underwater vehicle which is close to being spherical. The weight of the water displaced by the vehicle is taken as ρVg = 1215.8N and for r we use r = (0, 0, −0.007)T m. As initial conditions for the simulations we choose R(0) = × exp (1, 2, 3)T , Ω(0) = (0.3, 0.2, 0.1)T s−1 , b(0) =
(0, 0, 10)T m, and ν(0) = R(0)T (0.1, 0.2, 1)T m · s−1 . The final time tend = 150s is chosen such that b3 (tend ) ≈ b3 (0). The theory of Crouch-Grossman methods, see [14], is a generalization of Runge-Kutta methods for a class of systems on manifolds. Crouch-Grossman methods discretize the rigid body dynamics on SE(3) × R6 globally without the need of local coordinates (like Runge-Kutta methods) or projection. However, Crouch-Grossman methods (like Runge-Kutta methods) do not take into account any special structure of the system, contrary to Lie group variational integrators that are variational in nature as the systems they discretize.
With the above choice of parameters and initial conditions we have simulated the discrete dynamics of the underwater vehicle with a step size of h = 0.01s using the second order composition method (described in section III-F) constructed from the Lie group variational integrator (10)-(14) with its adjoint and a third order Crouch-Grossman method. In Fig. 1 we have plotted the energy behavior of the two simulations. In Fig. 1 (left) we see the rate of change in energy is higher for the composition method based on the Lie group variational integrator than for the Crouch-Grossman method; this is due to the higher order of the Crouch-Grossman method. For the longer time interval, in Fig. 1 (right), the composition method based on the Lie group variational integrator actually conserves the energy better, in the sense that the fluctuations appears to be bounded. Variational integrators turns out to be symplectic, see [2], and symplectic integrators are known to exhibit such long term energy boundedness. This is due to the fact the a symplectic integrator exactly, up to machine accuracy, solves the equations of motion for a mechanical system which is close, in some sense, to the system under consideration; a fact proved in [17]. Since the adjoint of a symplectic method is symplectic and the composition of symplectic methods is symplected the energy behavior of the composition method, based on the Lie group variational integrator (10)-(14), is expected. To solve the implicit equation (24) we implemented the methods described in Section III-G and compared them using Matlab. It turns out that the Moser-Veselov method appears to be faster than the Newton-Raphson based method (at least twice as fast when the error tolerance for the Newton-Raphson based method is taken to be of the order of that produced by the Moser-Veselov method.) Although the Moser-Veselov method appears to be faster one must bear in mind that it requires certain assumptions to be applicable. Also the solution from the Moser Veselov method may differ from SO(3) by a small quantity greater than the machine accuracy (in our implementation of this method the error F T F − I was of the order 10−14 .) V. C ONCLUSION In this paper we derived a first order Lie group variational integrator discretizing the dynamics of rigid body motion in SE(3) using the discrete Lagrange-d’Alembert principle. Thereby we obtained an integrator that is globally defined on the nonlinear state space SE(3) × R6 and that share the desirable properties of variational integrators. The method is defined for a general mass matrix M and included the effects of possible nonconservative forces and torques thus providing an extension of the Lie group variational integrator from [6] to nonconservative systems and systems where added masses needs to be included in the mass matrix, like underwater systems. We applied the second order composition of the Lie group variational integrator with its adjoint to the example of an uncontrolled underwater vehicle assuming that dissipative forces and torques were negligible thus making it a conservative system. We compared the result with a third order
Crouch-Grossman method to show the superior long term energy performance of the Lie group variational integrator. R EFERENCES [1] J. Marsden and M. West, “Discrete mechanics and variational integrators,” Acta Numerica, vol. 10, pp. 357–514, 2001. [2] E. Hairer, C. Lubich, and G. Wanner, Geometric Numerical Integration. New York: Springer Verlag, 2002. [3] J. Moser and A. P. Veselov, “Discrete versions of some classical integrable systems and factorization of matrix polynomials,” Communications in Mathematical Physics, vol. 139, pp. 217–243, 1991. [4] J. E. Marsden, S. Pekarsky, and S. Shkoller, “Discrete Euler-Poincare and Lie-Poisson equations,” Nonlinearity, vol. 12, pp. 1647–1662, 1999. [5] T. Lee, M. Leok, and N. H. McClamroch, “A Lie group variational integrator for the attitude dynamics of a rigid body with applications to the 3D pendulum,” in IEEE Conf. on Control Applications, 2005, pp. 962–967. [6] ——, “Lie group variational integrators for the full body problem,” Comput. Methods Appl. Mech. Engrg., vol. 196, no. 29-30, pp. 2907– 2924, 2007. [7] ——, “Dynamics of connected rigid bodies in a perfect fluid,” in American Control Conference, St. Louis, MO, 2009, pp. 408–413. [8] N. Bou-Rabee and J. E. Marsden, “Hamilton-pontryagin integrators on Lie groups part i: Introduction and structure-preserving properties,” Foundations of Computational Mathematics, vol. 9, pp. 197–219, 2008. [9] I. I. Hussein, M. Leok, A. K. Sanyal, and A. M. Bloch, “A discrete variational integrator for optimal control problems on SO(3),” in IEEE Conf. on Decision and Control, San Diego, CA, Dec. 2006, pp. 6636–6641. [10] A. M. Bloch, I. I. Hussein, M. Leok, and A. K. Sanyal, “Geometric structure-preserving optimal control of the rigid body,” Journal of Dynamical & Control Systems, vol. 15, no. 3, pp. 307–330, 2009. [11] T. Lee, M. Leok, and N. H. McClamroch, “Optimal control of a rigid body using geometrically exact computations on SE(3),” in IEEE Conf. on Decision and Control, San Diego, CA, Dec. 2006, pp. 2710– 2715. [12] J. C. Simo and L. Vu-Quoc, “On the dynamics in space of rods undergoing large motions—a geometrically exact approach,” Computer Methods in Applied Mechanics and Engineering, vol. 66, pp. 125–161, 1988. [13] J. C. Simo and T. S. Wong, “Unconditionally stable algorithms for rigid body dynamics that exactly preserve energy and momentum,” International Journal for Numerical Methods in Engineering, vol. 31, pp. 19–52, 1991. [14] P. E. Crouch and R. Grossman, “Numerical integration of ordinary differential equations on manifolds,” Journal of Nonlinear Science, vol. 3, pp. 1–33, 1993. [15] J. C. Simo, N. Tarnow, and K. K. Wong, “Exact energy-momentum conserving algorithms and symplectic schemes for nonlinear dynamics,” Computer Methods in Applied Mechanics and Engineering, vol. 100, pp. 63–116, 1992. [16] P. Krysl, “Explicit momentum-conserving integrator for dynamics of rigid bodies approximating the midpoint Lie algorithm,” International Journal for Numerical Methods in Engineering, vol. 63, pp. 2171– 2193, 2005. [17] G. Benettin and A. Giorgilli, “On the Hamiltonian interpolation of near to the identity symplectic mappings with application to symplectic integration algorithms,” Journal of Statistical Physics, vol. 74, pp. 1117–1143, 1994. [18] J. E. Marsden and T. S. Ratiu, Introduction to Mechanics and Symmetry, 2nd ed. New York: Springer Verlag, 1999. [19] J. R. Cardoso and F. Silva Leite, “The Moser-Veselov equation,” Linear Algebra and its Applications, vol. 360, pp. 237–248, 2003.
