{'fraction_non_alphanumeric': 0.06023411751335379, 'fraction_numerical': 0.0147175815433572, 'mean_word_length': 3.918529904974846, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 30, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Variational integrators for underactuated mechanical control systems with symmetries

Optimal control problems for underactuated mechanical systems can be seen as a higher-order variational problem subject to higher-order constraints (that is, when the Lagrangian function and the constraints depend on higher-order derivatives such as the acceleration, jerk or jounces). In this paper we discuss the variational formalism for the class of underactuated mechanical control systems when the configuration space is a trivial principal bundle and the construction of variational integrators for such mechanical control systems. An interesting family of geometric integrators can be defined using discretizations of the Hamilton's principle of critical action. This family of geometric integrators is called variational integrators, being one of their main properties the preservation of geometric features as the symplecticity, momentum preservation and good behavior of the energy. We construct variational integrators for higher-order mechanical systems on trivial principal bundles and their extension for higher-order constrained systems and we devote special attention to the particular case of underactuated mechanical systems


Introduction
The construction of variational integrators have deserved a lot of interest in the recent years (from both theoretical and applied points of view). The goal of this paper is to develop variational integrators for optimal control problems of mechanical systems defined on a trivial principal bundle. Our motivation is the optimal control of the class of underactuated mechanical systems. This type of mechanical systems, i.e. the underactuated, is characterized by the fact that there are more degrees of freedom than actuators.
The presence of underactuated mechanical systems is ubiquitous in engineering applications as a result, for instance, of design choices motivated by the search of less cost devices or as a result of a failure regime in fully actuated mechanical systems. Optimal control problems of an underactuated mechanical systems can be seen as an optimization problem involving Lagrangians defined on higher-order tangent bundles subject to higher-order constraints. The purpose is to find an optimal curve which solves the controlled equations and minimizes a cost function depending on higher-order derivatives subject to initial and final boundary conditions.
We extend the theory of discrete mechanics and construction of variational integrators, which is based on discrete calculus of variations, to systems which depend on higher-order derivatives and are subject to constraints (also depending of higher-order derivatives). Our discrete setting follows the lines given by Marsden and Wendlandt [31] and Marsden and West [32] for first order systems without constraints.
We shall develop the discrete analogue of higher-order Lagrange-Poincaré equations (introduced by Gay-Balmaz, Holm and Ratiu in [13]) for trivial bundles and its extension to the case of systems subject to higher-order constraints. For the construction of higher-order discrete Lagrange-Poincaré equations with higher-order constraints we use the discrete Hamilton's principle and the Lagrange multiplier theorem in order to obtain discrete paths that approximately satisfy the dynamics and the constraints. Such formulation gives us the preservation of important geometric properties of the mechanical system, such as momentum, symplecticity, group structure, good behavior of the energy, etc [16].
To be self-contained, we introduce a brief background in discrete mechanics.

General Background.
A discrete Lagrangian is a map L d : Q × Q → R, which may be considered as an approximation of the integral action defined by a continuous Lagrangian L : T Q → R, where q(t) is the unique solution of the Euler-Lagrange equations for L; q(0) = q 0 , q(h) = q 1 and the time step h > 0 is small enough. We construct the grid {t k = kh | k = 0, . . . , N}, Nh = T and define the discrete path space C d (Q) := {q d : {t k } N k=0 → Q}. We will identify a discrete trajectory q d ∈ C d (Q) with its image q d = {q k } N k=0 where q k := q d (t k ). Define the action sum A d : C d (Q) → R, corresponding to the Lagrangian L d by summing the discrete Lagrangian on each adjacent pair and defined by where q k ∈ Q for 0 ≤ k ≤ N. We would like to point out that the discrete path space is isomorphic to the smooth product manifold which consists on N + 1 copies of Q and the discrete action inherits the smoothness of the discrete Lagrangian.
The discrete variational principle then requires that δA d = 0 where the variations are taken with respect to each point q k , 1 ≤ k ≤ N − 1 along the path, and the resulting equations of motion, (a system of difference equations) given fixed endpoints q 0 and q N , are where D 1 and D 2 denote the derivative to the Lagrangian respect to the first and second arguments, respectively. These equations are usually called discrete Euler-Lagrange equations.
If the matrix (D 12 L d (q k , q k+1 )) is regular, it is possible to define a (local) discrete flow Υ L d : Q × Q → Q × Q, by Υ L d (q k−1 , q k ) = (q k , q k+1 ) from (1). The discrete algorithm determined by Υ L d preserves the (pre-)symplectic form on Q × Q, ω d , i.e., Υ * L d ω d = ω d (see [31], [32] and references therein). Given an action of a Lie group G on Q, we can consider the G−action on Q × Q by g · (q k , q k+1 ) := (g · q k , g · q k+1 ). This action is symplectic. Denoting by g the Lie algebra of G we can define two discrete momentum maps denotes the fundamental vector field and Θ + Organization of the paper. The paper is structured as follows. In §2 we derive second-order Euler-Lagrange (Lagrange-Poincaré) equations for trivial principal bundles from Hamilton's principle (theorem 2.1) and we study its extension to higher-order Lagrangian systems. The proposed method appears in §3 where we construct, from a discretization of the Lagrangian and through discrete variational calculus, higher-order discrete Euler-Lagrange (Lagrange-Poincaré) equations on trivial principal bundles (theorem 3.1). In §4, we study continuous (proposition 4.1) and discrete higher-order mechanics for systems subject to higher-order constraints. We apply these techniques in §5 to optimal control problems of underactuated mechanical systems with Lie group symmetries and explore two examples: the optimal control of a vehicle whose configuration space is the Lie group SE(2), and the associated optimal control problem of a homogeneous ball rotating on a plate.

2.
Higher-order Euler-Lagrange equations on trivial principal bundles 2.1. Higher-order tangent bundles. In this subsection we recall some basic facts of the higher-order tangent bundle theory. At some point, we will particularize this construction to the case when the configuration space is a Lie group G. For more details see [26].
Let M be a differentiable manifold of dimension n. It is possible to introduce an equivalence relation in the set C k (R, M) of k-differentiable curves from R to M. By definition, two given curves in M, γ 1 (t) and γ 2 (t), where t ∈ (−a, a) with a ∈ R have contact of order k at q 0 = γ 1 (0) = γ 2 (0) if there is a local chart (ϕ, U) of M such that q 0 ∈ U and , for all s = 0, ..., k. This is a well-defined equivalence relation in C k (R, M) and the equivalence class of a curve γ will be denoted by [γ] (k) 0 . The set of equivalence classes will be denoted by T (k) M and it is not hard to show that it has a natural structure of differentiable manifold. Moreover, τ k M : Define the left-and right-translation of G on itself Obviously ℓ g and r g are diffeomorphisms.
The left-translation allows us to trivialize the tangent bundle T G and the cotangent bundle T * G as follows (g, α g ) −→ (g, T * e ℓ g (α g )) = (g, α) , where g = T e G is the Lie algebra of G and e is the neutral element of G. In the same way, we have the identification T T G ≡ G × 3g where 3g stands for g × g × g. Throughout this paper, the notation n V , where V is a given space, denotes the cartesian product of n copies of V . Therefore, in the case when the manifold M has a Lie group structure, i.e. M = G, we can also use the left trivialization to identify the higher-order tangent bundle T (k) G with G × kg. That is, if g : I → G is a curve in C (k) (R, G): It is clear that Υ (k) is a diffeomorphism. We will denote ξ(t) := g −1 (t)ġ(t), therefore and g(0) = g, ξ (l) (0) = ξ (l) , 0 ≤ l ≤ k − 1. We will use the following notations without distinction ξ (0) = ξ, ξ (1) =ξ, when referring to the derivatives.
Let L : T Q → R be a Lagrangian function. Since T Q is isomorphic to T M ×T G and T G can be identify with G × g after a left-trivialization, we can consider our Lagrangian function as L : The motion of the mechanical system is described by applying the following principle, where is an arbitrary curve on the Lie algebra with η(0) = η(T ) = 0 and η = g −1 δg (see [17]). This principle gives rise to the Euler-Lagrange equations on trivial principal bundles d dt where ad * : g * → g * is the coadjoint representation of the Lie algebra g. If the Lagrangian L is left-invariant, that is, L does not depend of the variable on G, the above equations are rewritten as which shall be considered as the Lagrange-Poincaré equations.
Higher-order Euler-Lagrange equations on trivial principal bundles: The previous ideas can be extended to Lagrangians defined on a higher-order trivial principal bundle. We identify the higher-order tangent bundle for l = 1, . . . , k; j = 1, . . . , k − 1 and where the variation δξ is induced by δg as δξ =η+[ξ, η] where η is a curve on the Lie algebra with fixed endpoints. Therefore, from Hamilton's principle, integrating k times by parts and using the boundary As in the previous cases, if the Lagrangian is left-invariant the right-hand side of the second equation vanishes and one obtains the higher-order Lagrange-Poincaré equations, which coincide with the equations given in [13] for G-invariant Lagrangians.

Discrete higher-order Lagrange-Poincaré equations
In this section we will derive, using discrete calculus of variations, discrete Euler-Lagrange equations for Lagrangians defined on a left-trivialized higher-order tangent bundle to Q = M ×G, that is, T (k) M ×G×kg, where G is a finite dimensional Lie group and g its associated Lie algebra.
3.1. Discrete second-order Euler-Lagrange equations on trivial principal bundles. It is well known [2] that a natural discretization of the second order tangent bundle to a manifold M is given by three copies of it. We take 3(M × G) ≡ 3M × 3G as a natural discretization of T (2) Q since we are considering a discretization of the Lie algebra its associated Lie group.
where W k := g −1 k g k+1 ∈ G. Hamilton's principle establishes that the sequence (q (0,N ) , g (0,N ) ) ∈ C 2(N +1) is a solution of the discrete Lagrangian system determined by We now proceed, as in the continuous case, to derive the discrete equations of motion applying Hamiltons principle of critical action. For it, we consider variations of the discrete action sum, that is, where we use the notation D l L d | k := D l L d (q k , q k+1 , q k+2 , g k , W k , W k+1 ) and D l denotes the partial derivative with respect to the l−th variable. Taking variations of W k and considering the Lie algebra element Σ k := g −1 k δg k , we obtain where g k , W k ∈ G and Σ k ∈ g.
Rearranging the sum indexes, (8) can be decomposed in the following way: where the notation L d q implies that the M variables are frozen while, L d g implies that the G variables are frozen and where we have used the relations Σ k = g −1 k δg k and (9). From these equalities we obtain the following theorem: and consider the path (q (0,N ) , g (0,N ) ) ∈ C 2(N +1) . Then, the path (q (0,N ) , g (0,N ) ) satisfies δA d (q (0,N ) , g (0,N ) ) = 0 for A d : N ) ) satisfies the discrete second-order Euler-Lagrange equations depend of the first entry on G, the equations in theorem 3.1 are rewritten as These should be consider as the discrete second-order Lagrange-Poincaré equations.

Discrete higher-order Lagrange-Poincaré equations.
It is easy to extend our techniques for higher-order discrete mechanical systems. Consider a mechanical system determined by a Lagrangian L : As before, we consider the left-trivialized higher-order tangent bundle as T (k) M × G × kg. The associated discrete problem is founded by replacing the left-trivialized higher-order tangent bundle by (k + 1) copies of M × G. For simplicity, we use the following notation: Let L d : (k + 1)(M × G) → R be a discrete Lagrangian. Fixed initial and final conditions (q (0,k−1) , g (0,k−1) , q (N −k+1,N ) , g (N −k+1,N ) ) ∈ (M × G) 2k with N > 2k, we define the set of admissible sequences with boundary conditions (q (0,k−1) , g (0,k−1) ) and (q (N −k+1,N ) , g (N −k+1,N ) ) Let define the discrete action over an admissible path as A d : Hamilton's principle state that the sequence (q (0,N ) , g (0,N ) ) ∈C 2(N +1) is a solution of the discrete Lagrangian system determined by L d : Taking variations over the discrete action sum we obtain where we have used the relation (9) and the endpoints condition

Mechanical systems with constraints on higher-order trivial principal bundles
In this section we derive, from a discretization of Hamilton's principle, using Lagrange's multiplier theorem, an integrator for higher-order Lagrangian systems with higher-order constraints.
Here, η(t) is a curve in the Lie algebra g with fixed endpoints induced by the variations δξ =η + [ξ, η].

be called a solution of the higher-order variational problem with constraints if c is a critical point of A M .
By using the Lagrange multipliers theorem we may characterize the regular critical points of the higher-order problem with constraints as an unconstrained problem for an extended Lagrangian system (See [27] for a detailed proof). where λ = (λ 1 , . . . , λ m ) ∈ R m and L : The equations of motion for L are These equations are the higher-order Euler-Lagrange equations with higher-order constraints on If the Lagrangian is left-invariant (that is, L does not depend of the variables on G) these equations are rewritten as the higher-order Lagrange-Poincaré equations with higher-order constraints.

4.2.
Discrete variational problem with constraints on higher-order trivial principal bundles: In this subsection we will get the discretization of the last variational principle to obtain the discrete higher-order Euler-Lagrangian equations for systems subject to higher-order constraints. Let L d : (k + 1)(M × G) → R and Φ α d : (k + 1)(M × G) → R be the discrete Lagrangian and discrete constraints, respectively, for 1 ≤ α ≤ m and denote by M d the constraint submanifold locally determined by the vanishing of these m discrete constraint functions. As before, let define the discrete action sum by Therefore, we can consider the following problem as higher-order discrete variational problem with constraints: It is well-know that this optimization problem with higher-order constraints is equivalent to the unconstrained higher-order variational problem for where λ (0,N −k) = (λ 0 , ..., λ N −k ), and λ i is a vector with components λ i α , 1 ≤ α ≤ m. The unconstrained higher-order variational problem is defined as the minimization of A d where q (0,k−1) , g (0,k−1) , q (N −k+1,N ) , g (N −k+1,N ) are fixed and i = 0, ..., N − k. The critical points of the unconstrained problem, will be those annihilating ∂ A d /∂q i , and ∂ A d /∂λ i α . Thus, the higher-order discrete Euler-Lagrange equations with constraints on (k + 1)(M × G) are ,N ) , g (0,k−1) , g (N −k+1,N ) fixed points on kM and kG respectively.

Application to optimal control of underactuated mechanical systems
The purpose of this section is to study optimal control problems in the case of underactuated mechanical systems, that is, a Lagrangian control system such that the number of the control inputs is less than the dimension of the configuration space ("superarticulated mechanical system" following the nomenclature given in [1]).
We shall consider the configuration space Q as a trivial principal bundle. In what follows we assume that all the control systems are controllable, that is, for any two points x 0 and x f in the configuration space Q, there exists an admissible control u(t) defined on some interval [0, T ] such that the system with initial condition x 0 reaches the point x f at time T (see [3,7] for more details).
Define the control manifold U ⊂ R r where u(t) ∈ U is the control parameter. Consider the left-trivialized Lagrangian L : T M × g → R. The equations of motion of the system shall be considered the controlled Euler-Lagrange equations where we denote by B a := {(µ a , η a )}, µ a (q) ∈ T * q M, η a (q) ∈ g * , a = 1, . . . , r; and A = 1, . . . , n. Here, we are assuming that {(µ a , η a )} are independent elements of Γ(T * M × g * ) and u a are admissible controls where Γ(Q) denotes the set of sections of Q. Taking this into account, the optimal control problem can be formulated as: finding trajectories (q(t), ξ(t), u(t)) of the state variables and control inputs satisfying (11), subject to initial conditions (q(0),q(0), ξ(0)) and final conditions (q(T ),q(T ), ξ(T )), and, extremizing the functional J(q,q, ξ, u) = T 0 C(q(t),q(t), ξ(t), u(t)) dt.
where C : T M × g × U → R is the cost functional. Now, complete B a to a basis {B a , B α } of the vector space T * M ×g * . Take its dual basis {B a , B α } on Γ(T M × g) = X(M) × C ∞ (M, g). This basis induces coordinates and χ a (q); χ α (q) ∈ g, q ∈ M then equations (11) are rewritten as d dt The proposed optimal control problem is equivalent to a second-order variational problem with second-order constraints (see [3] and reference therein), determined by the second-order Lagrangian L : subjected to the second-order constraints Remark 5.1. It is possible to extend our analysis to systems with external forces f : T M × g → T * M × g * given by the following diagram

t t t t t t t t
M just by adding the corresponding terms in the right hand side of (11). These equations are therefore rewritten as such that f (q,q, ξ) = f A (q,q, ξ)dq A andf (q,q, ξ) ∈ g * .

5.1.
Optimal control of an underactuated vehicle. Consider a rigid body moving in SE(2) with a thruster to adjust its pose. The configuration of this system is determined by a tuple (x, y, θ, γ), where (x, y) is the position of the center of mass, θ is the orientation of the blimp with respect to a fixed basis, and γ the orientation of the thrust with respect to a body basis. Therefore, the configuration manifold is Q = SE(2) × S 1 (see [7] and references therein), where (x, y, θ) are the local coordinates of SE(2) and γ is the local coordinate of S 1 . The Lagrangian of the system is given by its kinetic energy and the input forces are where the control forces that we consider are applied to a point on the body with distance p > 0 from the center of mass (m is the mass of the rigid body), along the body x-axis. Note this system is an example of underactuated mechanical system when the configuration space is a trivial principal bundle.
A basis of the Lie algebra se(2) ∼ = R 3 of SE(2) is given by Thus, we can write down the structure constants as and all others zero. An element ξ ∈ se(2) is of the form ξ = ξ 1 e 1 + ξ 2 e 2 + ξ 3 e 3 ; therefore the reduced Lagrangian ℓ : T S 1 × se(2) → R is given by Then the reduced Euler-Lagrange equations with controls (11) in this case are given by On the other hand, choosing the adapted basis {B a , B α } the modified equations of motion (13) read in this case as Now, we can study the optimal control problem that consists, as mentioned before, on finding a trajectory of state variables and control inputs satisfying the previous equations from given initial and final conditions (γ(0),γ(0), ξ(0)), (γ(T ),γ(T ), ξ(T )) respectively, and extremizing the cost functional where ρ 1 and ρ 2 are constants. The related optimal control problem is equivalent to a second-order Lagrangian problem with second-order constraints defined as follows (see [3] for more details). Extremize subject to the second-order constraints Here, L : which basically is the cost function C = ρ 1 u 2 1 + ρ 2 u 2 2 in terms of the new variables. • Discrete setting: Following the prescription in theorem 3.1 and the further conclusion in corollary 3.2, we shall consider a discrete Lagrangian when approaching the discrete associated problem. Moreover, since we are dealing with a constrained problem, we must include the constraints in the variational procedure as shown in §4.2. Therefore, the discrete Lagrangian and constraints read: The discrete Lagrangian L d is chosen as a suitable approximation of the action, where τ is a general retraction map (see appendix 6.1), L is defined in (16) and Φ α are defined in (15). Here, γ k , γ k+1 , γ k+2 ∈ S 1 while W k , W k+1 ∈ SE (2). Note that we are taking a symmetric approximation to γ k , that is γ k +γ k+1 +γ k+2

3
. In addition, we are taking the usual discretizations for the first and second derivatives, that iṡ where se(2) ∋ ξ k = τ −1 (W k )/h. Taking advantage of the retraction map, we can define the discrete Lagrangian and the discrete constraints on the Lie algebra, that is, L d : 3(S 1 ) × 2se(2) → R, Φ α d : 3(S 1 ) × 2se(2) → R, α = 1, 2 (with some abuse of notation, we employ the same notation, that is L d and Φ α d , for the Lagrangian and constraints in both spaces). To consider the Lie algebra instead of the Lie group shall be useful since the Lie algebra is a vector space and, moreover, we stay in the space where the original system is defined. Namely As before, the notation L d | ξ , Φ α d | ξ denotes that the variables in se(2) are frozen while, correspondingly, L d | γ , Φ α d | γ denotes that the S 1 variables are frozen. To derive (18b) the properties of the right-trivialized derivative of the retraction map and its inverse, (see appendix, proposition 6.1) have been used (see [5,18,20]). In order to obtain the complete set of unknowns, that is γ (0,N ) , ξ (0,N ) , λ (0,N −2) α , we also have to take into account the reconstruction equation, which in this case has the form where g k ∈ SE(2). Finally, the range of validity of equations (18a) and (18b) is k = 2, ..., N − 2.
As was established in §4.2, due to the variational procedure (γ 0 , γ 1 ) and (γ N −1 , γ N ) are fixed, which leaves γ (2:N −2) as unknowns (i.e. N − 3 unknowns). By the same variational procedure (g 0 , g 1 ) and (g N −1 , g N ) are also fixed, which by means of (19) imply that ξ 0 and ξ N −1 are fixed. Nevertheless, due to the reconstruction discretization g k+1 = g k τ (hξ k ), is clear that fixing ξ k implies constraints in the neighboring points, in this case g k+1 and g k . If we allow ξ N , that means constraints at the points g N +1 and g N . Since we only consider time points up to T = Nh, having a constraint in the beyond-terminal configuration g N +1 makes no sense. Hence, to ensure that the effect of the terminal constraint on ξ is correctly accounted for, the set of algebra points must be reduced to ξ (0,N −1) . Furthermore, since ξ 0 and ξ N −1 are also fixed, the final set of algebra unknowns reduces to ξ (1,N −2) (i.e. 3(N − 2) unknowns, since dim se(2) = 3).
On the other hand, the boundary condition g(T ) is enforced by the relation τ −1 (g −1 N g(T )) = 0, which basically means that g N = g(T ). It is possible to translate this condition in terms of algebra elements as We have 2(N − 1) extra unknowns when adding the Lagrange multipliers λ (0,N −2) α (recall that, in this case α = 1, 2). Summing up, we have equations (corresponding to (18a)+ (18b)+(20)+(18c)). Consequently, our discrete variational problem (which comes from the original optimal control problem) has become a nonlinear root finding problem. From the set ξ (0,N −1) we can reconstruct the configuration trajectory by means of the reconstruction equation (19). For computational reasons is useful to consider the retraction map τ as the Cayley map for SE(2) instead of a truncation of the exponential map (see the Appendix for further details).
We also would like to stress that derivation of these discrete equations have a pure variational formulation and as a consequence, the integrators defined in this way are symplectic and Poisson-momentum preserving (see [28]). By using backward error analysis, it is well known that these integrators have a good energy behavior (see [32]). Similar techniques have been employed in [18] for the discrete optimal control of an underwater vehicle on SE(3).

Optimal Control of a Homogeneous Ball on a Rotating Plate.
We consider the following well-known problem (see [3,23,27]), namely the model of a homogeneous ball on a rotating plate. A (homogeneous) ball of radius r > 0, mass m and inertia mk 2 about any axis rolls without slipping on a horizontal table which rotates with angular velocity Ω about a vertical axis x 3 through one of its points. Apart from the constant gravitational force, no other external forces are assumed to act on the sphere. Let (x, y) be denote the position of the point of contact of the sphere with the table. The configuration space of the sphere is Q = R 2 × SO (3) where may be parametrized Q by (x, y, g), g ∈ SO(3), all measured with respect to the inertial frame. Let ω = (ω 1 , ω 2 , ω 3 ) be the angular velocity vector of the sphere measured also with respect to the inertial frame. The potential energy is constant, so we may put V = 0.
The nonholonomic constraints are given by the non-slipping condition bẏ The matrixġg T is skew-symmetric therefore we may writė where (ω 1 , ω 2 , ω 3 ) represents the angular velocity vector of the sphere measured with respect to the inertial frame. Then, we may rewrite the constraints in the usual form:ẋ + rω 2 = −Ωy,ẏ − rω 1 = Ωx. In addition, since we do not consider external forces the Lagrangian of the system corresponds with the kinetic energy K(x, y, g,ẋ,ẏ,ġ) = 1 2 (mẋ 2 + mẏ 2 + mk 2 (ω 2 1 + ω 2 2 + ω 2 3 )). Observe that the Lagrangian is metric on Q which is bi-invariant on SO(3) as the ball is homogeneous. Now, it is clear that Q = R 2 × SO(3) is the total space of a trivial principal SO(3)-bundle over R 2 with respect the right SO(3)−action given by (x, y, R) → (x, y, RS) for all S ∈ SO(3) and (x, y, R) ∈ R 2 × SO(3). The action is in the right side since the symmetries are material symmetries.
The bundle projection φ : Q → M = R 2 is just the canonical projection on the first factor. Therefore, we may consider the corresponding quotient bundle E = T Q/SO(3) over M = R 2 . We will identify the tangent bundle to SO(3) with so(3) × SO(3) by using right translation. Note that throughout the previous exposition we have employed the left trivialization. However, we would like to point out that the right trivialization just implies minor changes in the derivation of the equations of motion (see [17]).
Under this identification between T (SO(3)) and so (3) Thus, the quotient bundle T Q/SO(3) is isomorphic to the product manifold T R 2 × so (3), and the vector bundle projection is τ R 2 •pr 1 , where pr 1 : T R 2 ×so(3) → T R 2 and τ R 2 : T R 2 → R 2 are the canonical projections.
where X is a vector field on R 2 and f : R 2 → so(3) is a smooth map. Therefore, a global basis of sections of T R 2 × so(3) → R 2 is There exists a one-to-one correspondence between the space Γ(E = T Q/SO (3)) and the G-invariant vector fields on Q. If We have a constrained system on E = T Q/SO(3) ≃ T R 2 × so(3) and note that in this case the constraints are nonholonomic and affine in the velocities. This kind of systems was analyzed by J. Cortés et al [12] (in particular, this example was carefully studied). The constraints define an affine subbundle of the vector bundle E ≃ T R 2 × so(3) → R 2 which is modeled over the vector subbundle D generated by the sections D = span{e 5 ; re 1 + e 4 ; re 2 − e 3 }.
Moreover, the angular momentum of the ball about the axis x 3 is a conserved quantity since the Lagrangian is invariant under rotations about the axis x 3 and the infinitesimal generator for these rotations lies in the distribution D. The conservation law is written as ω z = c, where c is a constant or asω z = 0. Then by the conservation of the angular momentum the second-order constraints appear.
After some computations the equations of motion for this constrained system are precisely   ẋ − rω 2 = −Ωy, y + rω 1 = Ωx, together withẍ + k 2 Ω r 2 + k 2ẏ = 0,ÿ − k 2 Ω r 2 + k 2ẋ = 0. Now, we pass to an optimization problem. Assume full controls over the motion of the center of the ball (the shape variables). The controlled system can be written as, differential equations (ODEs).
In addition, the configurations R ∈ SO(3) are given by the continuous reconstruction equationṘ = Rω.
In the particular case when the angular velocity Ω depends on the time (see [3,22]), the equations of motion are rewritten as • Discrete setting: As in the previous example, we discretize this problem by choosing a discrete Lagrangian L d and discrete constraints Φ α d . Employing equivalent arguments than in the previous example, we set L d : 3(R 2 )×2so(3) → R and Φ α d : 3(R 2 ) × 2so(3) → R, α = 1, 2, 3, as We employ the same unknowns-equations counting process than in the previous example to find out that the number of unknowns matches the number of equations. Therefore, our discrete variational problem (which comes from the original optimal control problem) has become again a nonlinear root finding problem. For computational reasons is useful to consider the retraction map τ as the Cayley map for SO(3) instead of a truncation of the exponential map (see the Appendix for further details).

Conclusions
In this paper, we have designed a new class of variational integrators for optimal control problems of underactuated mechanical systems, showing how developments in the theory of discrete mechanics and discrete calculus of variations with constraints can be used to construct numerical optimal control algorithms with certain geometric desirable features.
We construct variational principles for higher-order mechanical systems with higher-order constraints to solve an optimal control problem for systems where its configuration space is a trivial principal bundle. From a discretization of Hamilton's principle of critical action we derive the discrete version of the problem. We show two concrete applications of our ideas: an underactuated vehicle and a (homogeneous) ball rotating on a plate. It is also possible to use our techniques and the obtained numeric integrators for other interesting problems, for instance, the theory of k-splines on SO(3) [15].
We would like to point out that a slight modification of the techniques presented in this work would allow to approach the Clebsh-Pontryagin optimal control problem (see [14,15]).
In a future work, we will also extend our construction to the case of non-trivial fiber bundles using a connection to split the reduced space [8].
6. Appendix 6.1. Discrete Mechanics on Lie groups. In this appendix we will recall the basics results on discrete mechanics on Lie groups and Hamilton's principle for the derivation of the discrete Euler-Poincaré equations.
If the configuration space is a finite dimensional Lie group G, then the discrete trajectory is represented numerically using a set of N + 1 points (g 0 , g 1 , . . . , g N ) with g i ∈ G, 0 ≤ i ≤ N.
The following theorem give us the relation between the discrete Euler-Lagrange equations and the discrete Euler-Poincaré equations. Theorem 6.1. [30] Let G be a Lie group and L d : G × G → R be a discrete Lagrangian function. We suppose that L d is left-invariant over the diagonal action; that is, L d (gg k , gg k+1 ) = L d (g k , g k+1 ) with g ∈ G. Let L d : G → R be the restriction to the identity, that is, L d : Fixed g 0 and g N , for a sequence of points {(g k , g k+1 )} N −1 k=0 ∈ G × G, with N > 2, we consider W k = g −1 k g k+1 . Then the following statements are equivalent: where for each k, ǫ → g ǫ k is a smooth curve on G such that g 0 k = g k with fixed end points.
k=0 satisfy the Euler-Lagrange equations for L d .
L d (W k ) for all variations δW k = −η k W k +W k η k+1 with η 0 = η N = 0; where η k ∈ g is given by η k = g −1 k δg k . A way to discretize a continuous problem is using a retraction map τ : g → G which is an analytic local diffeomorphism. This application maps a neighborhood of 0 ∈ g to a neighborhood of the identity e ∈ G. As a consequence, it is possible to deduce that τ (ξ)τ (−ξ) = e for all ξ ∈ g.
The retraction map is used to express small discrete changes in the group configuration through unique Lie algebra elements (see [22]), namely ξ k = τ −1 (g −1 k g k+1 )/h, where ξ k ∈ g. That is, if ξ k were regarded as an average velocity between g k and g k+1 , then τ is an approximation to the integral flow of the dynamics. The difference g −1 k g k+1 ∈ G, which is an element of a nonlinear space, can now be represented by the vector ξ k , in order to enable unconstrained optimization in the linear space g for optimal control purposes.
It will be useful in the sequel, mainly in the derivation of the discrete equations of motion, to define the right trivialized tangent retraction map as a function dτ : g × g → g by T τ (ξ) · η = T r τ (ξ) dτ ξ (η), where η ∈ g. Here we use the following notation, dτ ξ := dτ (ξ) : g → g. The function dτ is linear in its second argument. From this definition the following identities hold (see [5] for further details) Proposition 6.1. Given a map τ : g → G, its right trivialized tangent dτ ξ : g → g and its inverse dτ −1 ξ : g → g, are such that for g = τ (ξ) ∈ G and η ∈ g, the following identities hold: ∂ ξ τ (ξ) η = dτ ξ η τ (ξ) and ∂ ξ τ −1 (g) η = dτ −1 ξ (η τ (−ξ)). The most natural example of retraction map is the exponential map at the identity e of the group G, exp e : g → G. We recall that, for a finite-dimensional Lie group, exp e is locally a diffeomorphism and gives rise a natural chart [30].
Then, there exists a neighborhood U of e ∈ G such that exp −1 e : U → exp −1 e (U) is a local C ∞ −diffeomorphism. A chart at g ∈ G is given by Ψ g = exp −1 e • ℓ g −1 . In general, it is not easy to work with the exponential map. In consequence it will be useful to use a different retraction map. More concretely, the Cayley map (see [5,16] for further details) will provide us a proper framework in the examples shown along the paper. and is valid for a class of quadratic groups (see [16] for example) that include the groups of interest in this paper (e.g. SO(3), SE(2) and SE (3)). Its right trivialized derivative and inverse are defined by dcay x y = (e − Using the isomorphic map· : where v = (v 1 , v 2 , v 2 ) T ∈ R 3 , the set {ê 1 ,ê 2 ,ê 3 } can be used as a basis for se (3), where {e 1 , e 2 , e 3 } is the standard basis of R 3 . The map cay : se(3) → SE(2) is given by while the map dτ −1 ξ becomes the 3 × 3 matrix and I 3 denotes the 3 × 3 identity matrix.