{'fraction_non_alphanumeric': 0.04547145940314058, 'fraction_numerical': 0.010667063580660366, 'mean_word_length': 4.289594541398767, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 1, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 25, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Discrete Variational Optimal Control

This paper develops numerical methods for optimal control of mechanical systems in the Lagrangian setting. It extends the theory of discrete mechanics to enable the solutions of optimal control problems through the discretization of variational principles. The key point is to solve the optimal control problem as a variational integrator of a specially constructed higher-dimensional system. The developed framework applies to systems on tangent bundles, Lie groups, underactuated and nonholonomic systems with symmetries, and can approximate either smooth or discontinuous control inputs. The resulting methods inherit the preservation properties of variational integrators and result in numerically robust and easily implementable algorithms. Several theoretical and a practical examples, e.g. the control of an underwater vehicle, will illustrate the application of the proposed approach.


Introduction
The goal of this paper is to develop, from a geometric point of view, numerical methods for optimal control of Lagrangian mechanical systems. Our approach employs the theory of discrete mechanics and variational integrators [31] to derive both an integrator for the dynamics and an optimal control algorithm in a unified manner. This is accomplished through the discretization of the Lagrange-d'Alembert variational principle on manifolds. An integrator for the mechanics is derived using a standard Lagrangian function and virtual work done by control forces, while control optimality conditions are derived using a special Lagrangian defined on a higher-dimensional space which encodes the dynamics and a desired cost function. The resulting integration and optimization schemes are symplectic, respect the state space structure, and momentum preserving. These qualities are associated with numerical stability which motivate the development of practical algorithms that can be applied to robotic or aerospace vehicles.
The proposed framework is general and applies to unconstrained systems, as well systems with symmetries, underactuation, and nonholonomic constraints. In particular, our construction is appropriate for controlled Lagrangian systems that evolve on a general tangent bundle T Q with associated discrete state space Q × Q, where Q is a differentiable manifold ( [31,35]). In addition we focus on underactuated systems evolving on a Lie group G ( [4,6,18,21]) that are applicable for systems consisting of rigid bodies. Finally, the theory extends to the more general principle bundle setting with discrete analog Q × Q × G (or more generally (Q × Q)/G) assuming that the action of a Lie group G of symmetry leaves the control system invariant ( [9,11,19]).
The main idea is the following: we take an approximation of the Lagranged'Alembert principle for forced Lagrangian systems, which models control inputs and external forces such as gravity or drag. In principle, we admit the possibility of piecewise continuous control forces, as happens in real applications. We observe that the discrete equations of motion for this type of systems are interpreted as the discrete Euler-Lagrange equations of a new Lagrangian defined in an augmented discrete phase space. Next, we apply discrete variational calculus techniques to derive the discrete optimality conditions. After this, we recover two sequences of discrete controls modeling a piecewise control trajectory.
Additionally we show how to derive the equations for different reduced systems. We specifically develop numerical methods for systems on Lie groups that lead to practical algorithm implementation. One such example system-an underactuated underwater vehicle-is used to illustrate the developed methodology. The resulting algorithm is simple to implement and has the ability to quickly converge to a solution which is close to the optimal solution and to the true system dynamics. We also extend our techniques to more general reduced systems like optimal control problems in trivial principal bundles and we show how to introduce nonholonomic constraints in our framework.
Moreover, since we are reducing the optimality conditions to discrete Euler-Lagrange equations, the geometric preservation properties like symplectic-momentum preservation in the standard case or Poisson bracket and momentum preservation for reduced systems are automatically guaranteed using the results in [25,31].
The paper is structured as follows: §2 introduces variational integrators. §3 formulates optimal control problems for Lagrangian systems defined on tangent bundles, in the continuous and discrete setting, and for both fully and underactuated systems. A simple control problem for a mechanical Lagrangian on R n illustrates these developments. In §4, discrete mechanics on Lie groups is introduced. Specifically, discrete Euler-Poincaré equations and their Hamiltonian version, the discrete Lie-Poisson equations, are obtained. Sections §5 and §8 develop the discretization procedure and the numerical aspects of the proposed approach. The developed algorithm is illustrated with an application to an unmanned underwater vehicle evolving on SE(3). Finally, §7 deals with reduced systems on a trivial principal bundle and with nonholonomic mechanics.

Discrete Mechanics and Variational Integrators
Let Q be a n-dimensional differentiable manifold with local coordinates (q i ), 1 ≤ i ≤ n. Denote by T Q its tangent bundle with induced coordinates (q i ,q i ). Given a Lagrangian function L : T Q → R the Euler-Lagrange equations are d dt These equations are a system of implicit second order differential equations.
In the sequel, we will assume that the Lagrangian is regular, that is, the matrix ∂ 2 L ∂q i ∂q j is non-singular. It is well known that the origin of these equations is variational (see [1,30]).
Variational integrators retain this variational character and also some of main geometric properties of the continuous system, such as symplecticity and momentum conservation (see [12] and references therein).
In the following we will summarize the main features of this type of numerical integrators [31]. 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 : is a solution of the Euler-Lagrange equations for L, where q(0) = q 0 and q(h) = q 1 and h > 0 is enough small.
Remark 2.1. The Cartesian product Q × Q is equipped with an interesting differential structure, the Lie groupoid structure which allows us to extend the construction of variational calculus to another interesting situations (Lie groupoids). See [25] for more details.
Define the action sum S d : and N is the number of steps. The discrete variational principle states that the solutions of the discrete system determined by L d must extremize the action sum given fixed endpoints q 0 and q N . By extremizing S d over q k , 1 ≤ k ≤ N − 1, we obtain the system of difference equations or, in coordinates, ∂L d ∂x i (q k , q k+1 ) + ∂L d ∂y i (q k−1 , q k ) = 0, where 1 ≤ i ≤ n, 1 ≤ k ≤ N − 1 and x, y denote the n-first and n-second variables of the function L respectively.
These equations are usually called the discrete Euler-Lagrange equations. Under some regularity hypotheses (the matrix (D 12 (2). Define the discrete Legendre transformations associated to L d as and the discrete Poincaré-Cartan 2-form where ω Q is the canonical symplectic form on T * Q. The discrete algorithm determined by Υ L d preserves the symplectic form ω d , i.e., Υ * Moreover, if the discrete Lagrangian is invariant under the diagonal action of a Lie group G, then the discrete momentum map J d : is preserved by the discrete flow. Therefore, these integrators are symplecticmomentum preserving. Here, ξ Q denotes the fundamental vector field determined by ξ ∈ g, where g is the Lie algebra of G. (See [31] for more details.) 3. Discrete optimal control on tangent bundles Consider a mechanical system which configuration space is a n-dimensional differentiable manifold Q and which dynamics is determined by a Lagrangian L : T Q → R. The control forces are modeled as a mapping f : Observe that this last definition also covers configuration and velocity dependent forces such as dissipation or friction (see [35]). For greater generality we consider control variables that are only piecewise continuous to account for impulsive controls.
The motion of the mechanical system is described by applying the principle of Lagrange-D'Alembert, which requires that the solutions q(t) ∈ Q must satisfy where (q ,q) are the local coordinates of T Q and where we consider arbitrary variations δq ∈ T q(t) Q with δq(0) = 0 and δq(T ) = 0 (since we are prescribing fixed initial and final conditions (q(0),q(0)) and (q(T ),q(T ))). Given that we are considering an optimal control problem, the forces f must be chosen, if they exist, as the ones that extremize the cost functional: where C : The optimal equations of motion can now be derived using Pontryagin maximum principle. Generally, it is not possible to explicitly integrate these equations and, consequently, it is necessary to apply a numerical method. In this work, using discrete variational techniques, we will first discretize the Lagrange-d'Alembert principle and then the cost functional. We obtain a numerical method that preserves some geometric features of the original continuous system as we will see in the sequel.
To discretize this problem we replace the tangent space T Q by the Cartesian product Q × Q and the continuous curves by sequences q 0 , q 1 , . . . q N (we are using N steps, with time step h fixed, in such a way t k = kh and N h = T ). The discrete Lagrangian L d : Q × Q → R is constructed as an approximation of the action integral in a single time step (see [31]), that is We choose the following discretization for the external forces: Observe that, as mentioned above, we have introduced the discrete controls as two different sequences u − k and u + k . In the notation followed through this paper, the time interval between [k, k + 1] is denoted as the k-th interval, while the controls in k + and (k + 1) − are denoted by u − k and u + k+1 respectively. This choice allows us to model piecewise continuous controls, admitting discrete jumps at the time steps t k = hk. Our notation is completely depicted in the following figure: Moreover, we have that [31]). Therefore, we derive a discrete version of the Lagrange-D'Alembert principle given in (3): From this principle is easy to derive the system of difference equations: where k = 1, . . . , N − 1. Equations (5) are called the forced discrete Euler-Lagrange equations (see [35]). We can also approximate the cost functional (4) in a single time step h by yielding the discrete cost functional: .
3.1. Fully-actuated Systems. In this section we assume the following condition Definition 3.1. (Fully actuated discrete system) We say that the discrete mechanical control system is fully actuated if the mappings Define the momenta (see [31,35]) Since both f ± k (q k ,q k+1 ) are diffeomorphisms we can express u ± k in terms of (q k , p k , q k+1 , p k+1 ) using (6) and (7). Next, we define a new Lagrangian The system is fully-actuated, consequently the Lagrangian L d is well defined on the entire discrete space T * Q × T * Q. Now the discrete phase space is the Cartesian product T * Q × T * Q of two copies of the cotangent bundle. The definition (6), (7) gives us a matching of momenta (see [31]) which automatically implies , k = 1, . . . , N − 1, which are the forced discrete Euler-Lagrange equations (5). In other words, the matching condition enforces that the momentum at time k should be the same when evaluated from the lower interval [k − 1, k] or the upper interval [k, k + 1]. Consequently, along a solution curve there is a unique momentum at each time t k , which can be called p k .
The discrete Euler-Lagrange equations of motion for the Lagrangian L d : In summary, we have obtained the discrete equations of motion for a fully-actuated mechanical optimal control problem as the discrete Euler-Lagrange equations for a Lagrangian defined on the product of two copies of the cotangent bundle. Therefore, all the preservation properties of the discrete equations (9) and (10) are now a direct consequence of the theory of variational integrators [31].
3.2. Example: optimal control problem for a mechanical Lagrangian with configuration space R n . Consider the case Q = R n and assume that M is an n × n constant and symmetric matrix. The mechanical Lagrangian is the potential function and x ∈ R. The system is fully actuated and there exist no velocity constraints. The optimal control problem is typically in terms of boundary conditions (x(0),ẋ(0)) and (x(T ),ẋ(T )) for a given final time T . Note that in the continuous setting we can define the momentum by the continuous Legendre transformation FL : In consequence, we can define boundary constraints also in the phase space: ( We set the Trapezoidal discretization for the Lagrangian (see [12]), that is, where, as above, h is the fixed time step and x 1 , x 2 , . . . , x N is a sequence of elements on R n . Our concrete discrete Lagrangian is The control forces are For sake of clarity, we are going to fix the control forces in the following manner f ± (x k , x k+1 , u ± k ) = u ± k . Looking at equations (6) and (7) is easy to obtain the associated momenta p k and p k+1 , namely where V x represents the derivative of V with respect to the variable x. Applying equations (9) and (10) to L d we obtain the following equations: where both set of equations are defined for k = 1, ..., N − 1. It is quite clear that we could remove the p k dependence in equation (12). However, we prefer to keep it in order to stress that the discrete variational Euler-Lagrange equations (9) and (10) are defined in T * Q × T * Q (T * R n × T * R n in the particular case we are considering in this example).

Underactuated Systems.
In this section, we examine the case of underactuated systems defined as follows: Definition 3.2. (Underactuated discrete system) We say that the discrete mechanical control system is underactuated if the mappings , are both embeddings, that is, they are one-to-one immersions that are homeomorphisms of U to its image.

Under this hypothesis we deduce that
As a consequence, the set of admissible momenta defined in (6) and (7) satisfy (8) is restricted to these points only. Thus, it is necessary to apply constrained variational calculus to derive the corresponding equations (see [2]). This is typically performed by means

Thus, the Lagrangian function defined in
Therefore the solutions of the optimal control problem are now viewed as the solutions of the discrete constrained problem determined by an extended Lagrangian L d and the constraints Φ ± α . Since f ± (q k ,q k+1 ) are embeddings, as established in definition (3.2), the number of constraints is determined by n minus the dimension of U . Note that the total number of constraints, Φ ± α , is therefore 2(n − dim U ).
To solve this problem we introduce Lagrange multipliers (λ − k ) α ,(λ + k ) α and consider discrete variational calculus using the augmented Lagrangian . Observe that, in spite the constraints are functions of the Cartesian product of two copies of the cotangent bundle i.e. Φ ± α : The discrete Euler-Lagrange equations gives us the solutions of the underactuated problem.
Typically, the underactuated systems appear in an affine way that is are linear maps (we assume that U is a vector space and Lin(E 1 , E 2 ) is the set of all linear maps between E 1 and E 2 ). In consequence Then the constraints are deduced using the compatibility conditions: , which imply constraints in (q k , q k+1 , p k ) and (q k , q k+1 , p k+1 ), respectively. The fact that f ± k (q k ,q k+1 ) are both embeddings implies furthermore that rank

Discrete optimal control on Lie groups
An indispensable tool in the study of mechanical systems is reduction theory. Therefore, in this work we consider its discrete analogue. This is precisely the motivating idea of the work by Moser and Veselov [34], i.e. to give a discrete analogue of Euler-Poincaré reduction. The approach is to reduces the standard second order Euler-Lagrange equations when the configuration space is a Lie group G to first order equations on the Lie algebra g.
Following the developments in § 2 assume that the Lagrangian defined by for any elementḡ ∈ G and (g k , g k+1 ) ∈ G × G. According to this, we can define a reduced Lagrangian l d : G → R by and e is the identity of the Lie group G. The reduced action sum is given by Taking variations of S d and noting that k δg k , we arrive to the discrete Euler-Poincaré equations: where l : G × G → G and r : G × G → G are respectively the left and the right translations of the group (see also [6]).
If we denote by where Ad : G × g → g is the adjoint action of G on g. Typically this equations are known as the discrete Lie-Poisson equations (see [6,28,29]). Consider a mechanical system determined by a Lagrangian l : g → R, where g is the Lie algebra of a Lie group G, which also is a n-dimensional vector space. The continuous external forces are defined as follows f : g × U → g * . The motion of the mechanical system is described applying the following principle for all variations δξ(t) of the form δξ(t) =η(t) + [ξ(t), η(t)], where η(t) is an arbitrary curve on the Lie algebra with η(0) = 0 and η(T ) = 0 (see [30]). In addition ·, · is the natural pairing between g and g * . These equations give us the controlled Euler-Poincaré equations: The optimal control problem consists of minimizing a given cost functional: where C : g × U −→ R. Now, we consider the associated discrete problem. First we replace the Lie algebra g by the Lie group G and the continuous curves by sequences W 0 , W 1 , . . . W N (since the Lie algebra is the infinitesimal version of a Lie group, its proper discretization is consequently that Lie group [29,31]).
The discrete Lagrangian l d : G → R is constructed as an approximation of the action integral, that is Let define the discrete external forces in the following way: In addition η 0 = η N = 0 and ·, · is the natural pairing between g and g * .
For sake of simplicity we are sometimes going to omit the dependence on G × U of both f + k and f − k . Taking all the previous into account, we derive a discrete version of the Lagrange-D'Alembert principle for Lie groups: ..N −1 an arbitrary sequence of elements of g which satisfies η 0 , η N = 0 (see [18,21]). From this principle is easy to derive the system of difference equations: for k = 1, . . . , N − 1, which are called the controlled discrete Euler-Poincaré equations.
The cost functional (15) is approximated by yielding the discrete cost functional: Observe that now C d : U × G × U → R.

Fully Actuated Systems.
In the fully actuated case the mappings are diffeomorphisms for all W ∈ G, therefore, we can construct the Lagrangian L d : where the variables ν k , ν k+1 ∈ g * are defined by The discrete phase space g * × G × g * is now a mixture of two copies of the Lie algebra g * and a Lie group G. This is also an example of a Lie groupoid ( [25]). The discrete optimal control problem defined in (16) and (18) has been reduced to a Lagrangian one, with Lagrangian function L d : g * ×G×g * → R. In consequence, we are able to apply discrete variational calculus to obtain the discrete equations of motion in the phase space g * × G × g * .
The critical points of the discrete action sum subjected to the previous boundary conditions are characterized by Taking derivatives we obtain , ν ), where W ∈ G and ν, ν ∈ g * . Since δh k (which is defined as d h k d | =0 ) and δν k (which is defined as d ν k d | =0 ), k = 1, . . . , N − 1 are arbitrary, we deduce the following discrete equations of motion: for all s. Additionally, the embedding condition implies that rank b − k = rank b + k = dim U . Then, taking the dual basis {e s , e σ }, we induce the following constraints: Observe in (23) that, even though the constraints are functions Φ ± σ : g * × G × g * → R, neither Φ − σ depends on ν k+1 nor Φ + σ on ν k . Once we have defined the constraints we can implement the Lagrangian multiplier rule in order to solve the underactuated problem. Namely, we define de extended Lagrangian as: Defining the discrete action sum we obtain the underactuated discrete equations of motion where the subscripts (W k−1 ), (W k ), (ν k−1 , ν k ), (ν k , ν k+1 ) denoted variables that are fixed.

Numerical Methods for Systems on Lie Groups
We now put the discrete optimal control equations (22) and (25) into a form suitable for algorithmic implementation. The numerical methods are constructed using the following guidelines: (1) good approximation of the dynamics and optimality, (2) avoid issues with local coordinates (3) guarantee for numerical robustness and convergence, (4) numerical efficiency.
The discrete mechanics approach provides an accurate approximation of the dynamics (requirement 1) through momentum and symplectic form preservation and good energy behavior. In addition, we will satisfy requirement 2 for systems on Lie groups by lifting the optimization to the Lie algebra through a retraction map that will be defined in this section. The resulting algorithms are numerically robust in the sense that there are no issues with coordinate singularities and the dynamics and optimality conditions remain close to their continuous counterparts even at big time steps. Yet, as with any other nonlinear optimization scheme it is difficult to formally claim that the algorithm will always converge (requirement 3). Nevertheless, in practice there are only isolated cases for underactuated systems that fail to converge. A remedy for such cases has been suggested in [18]. In general, the resulting algorithms require a small number of iterations, e.g. between 10 and 20 to converge (requirement 4).
The optimization variables W k are regarded as small displacements on the Lie group. Thus, it is possible to express each term through a Lie algebra element that can be regarded as the averaged velocity of this displacement. This is accomplished using a retraction map τ : g → G which is an analytic local diffeomorphism around the identity such that τ (ξ)τ (−ξ) = e, where ξ ∈ g. Two standard choices for τ are employed in this work: the exponential map, and the Cayley map.
Regarding ξ as a velocity we set the discrete Lagrangian l d : , 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.
The variational principle will now be expressed in terms of the chosen map τ . The resulting discrete mechanics will thus involve the derivatives of the map which we define next (see also [7,15,18]): Definition 5.1. Given a map τ : g → G, its right trivialized tangent dτ ξ : g → g and is inverse dτ −1 ξ : g → g, are such that for g = τ (ξ) ∈ G and η ∈ g, the following holds Using these definitions, variations δξ and δg are constrained by where η k = g −1 k δg k , which is obtained by straightforward differentiation of ξ k = τ −1 (g −1 k g k+1 )/h. The retraction map τ choices are: a) The exponential map exp : g → G, defined by exp(ξ) = γ(1), with γ : R → G in the integral curve through the identity of the vector field associated with ξ ∈ g (hence, withγ(0) = ξ). The right trivialized derivative and its inverse are defined by where B j are the Bernoulli numbers (see [12]). Typically, these expressions are truncated in order to achieve a desired order of accuracy.
b) The Cayley map cay : g → G is defined by cay(ξ) = (e − ξ 2 ) −1 (e + ξ 2 ) and is valid for a general class of quadratic groups (see [12]) 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 ).
Next, the discrete forces and cost function are defined through a trapezoidal approximation, i.e.
respectively. With the choice of a retraction map and the trapezoidal rule the equations of motion (13) become while the momenta defined in (21) take the form Finally, define the Lagrangian d : g * × g × g * → R such that d (ν, ξ, ν ) = L d (ν, τ (hξ), ν ). Note that the Lagrangian is well-defined only on g * ×U×g * , where U ⊂ g is an open neighborhood around the identity for which τ is a diffeomorphism. To make the notation as simple as possible we retain the Lagrangian definition to the full space g * × g × g * .
Boundary Conditions.: Establishing the exact relationship between the discrete and continuous momenta, µ k and µ(t) = ∂ ξ l(ξ(t)), respectively, is particularly important for properly enforcing boundary conditions that are given in terms of continuous quantities. The following equations relate the momenta at the initial and final times t = 0 and t = T and are used to transform between the continuous and discrete representations: which also corresponds to the relations ν 0 = ∂ ξ l(ξ(0)) and ν N = ∂ ξ l(ξ(T )). These equations can also be regarded as structure-preserving velocity boundary conditions, i.e., for given fixed velocities ξ(0) and ξ(T ).
The solution is computed using root-finding procedure such as Newton's method. If the initial guess does not satisfy the dynamics we recommend to use a Levenberg-Marquardt algorithm which has slower but more robust convergence.

5.1.
Example: optimal control effort. Consider a Lagrangian consisting of the kinetic energy only full unconstrained actuation, no potential or external forces and no velocity constraint. The map I : g → g * is called the inertia tensor and is assumed full rank.
In the fully actuated case we have f (ξ k , u ± k ) ≡ u ± k . We consider a minimum effort control problem, i.e.
The optimal control problem for fixed initial and final states (g(0) , ξ(0)) and (g(T ) , ξ(T )) can now be summarized as: subject to: The optimality conditions for this problem are derived as follows. The Lagrangian becomes where the momentum has been computed according to Thus the optimality conditions become It is important to note that these last two equations define N · n equations in the Nṅ unknowns ξ 0:N −1 . A solution can be found using nonlinear root finding. Once ξ 0:N have been computed, is possible to obtain the final configuration g N by reconstructing the curve by these velocities. Beside, the boundary condition g(T ) is enforced through the relation τ −1 (g −1 N g(T )) = 0 without the need to optimize over any of the configurations g k .

5.2.
Extension: the configuration-dependent case. The developed framework can be extended to a configuration-dependent Lagrangian L : G × g → R, for instance defined in terms of a kinetic energy K : g → R and potential energy V : G → R according to where g ∈ G and ξ ∈ g. The controlled Euler-Poincaré equations are in this caseμ where the external forces are defined as f : while the G-dependent discrete forces now become This leads to the discrete equations The momenta become In consequence, we can define a discrete Lagrangian depending on the variables (ν k , g k , ξ k , ν k+1 ) which discrete equations of motion will be a mixture between (22) and (28), (29), namely 6. Applications 6.1. Underwater Vehicle. We illustrate the developed algorithm with an application to a simulated unmanned underwater vehicle. Figure (1) shows the model equipped with five thrusters which produce forces and torques in all directions but the body-fixed "y"-axis. Since the input directions span only a five-dimensional subspace the problem is solved through the underactuated framework. The vehicle configuration space is G = SE(3). We make the identification where g ∈ SE(3). Elements of the Lie algebra ξ ∈ se(3) are identified with body-fixed angular and linear velocities denoted ω ∈ R 3 and v ∈ R 3 , respectively, through where the map· : The algorithm is thus implemented in terms of vectors in R 6 rather than matrices in se (3).
The map τ = cay : se(3) → SE(3) is chosen, instead of the exponential, since it results in more computationally efficient implementation. It is defined by where cay : so(3) → SO(3) is given 1 by where I n is the n × n identity matrix and dcay : R 3 → R 3 is defined by 1 note that cay denotes a map to either SO(3) or SE(3) which should be clear from its argument.
The matrix representation of the right-trivialized tangent inverse dτ −1 (ω,v) : The vehicle inertia tensor I is computed assuming cylindrical mass distribution with mass m = 3kg. The control basis vectors are {e s } 5 s=1 = {e 1 , e 2 , e 3 , e 4 , e 5 }, while the non-actuated direction is e σ = e 6 , where e i is the i-th standard basis vector of R 6 . The control functions take the form here H is a negative definite viscous drag matrix and the constants c, d are the lengths of the thrusting torque moment arms (see Figure 1). We are interested in computing a minimum control effort trajectory between two given boundary states, i.e. conditions on both the configurations and velocities. Such a cost function is defined in §5.1. The optimal control problem is solved using equations (31). The computation is performed using Algorithm 5.2. Figure 2 shows the computed velocities and controls for the "reconfiguration" trajectory shown in Figure 1. The algorithms requires between 10-20 iterations depending on the boundary conditions and when applied to N = 32 segments.  Figure 3. An optimal trajectory of an underactuated rigid body on SO(3) (a). The body is controlled using two force inputs around the body-fixed x and y axes. An L1-control cost function results in a discontinuous optimal trajectory (b) which our algorithm can handle.
6.2. Discontinuous Control. One of the advantages of employing the discrete variational framework is the treatment of discontinuous control inputs as illustrated in §3. The nature of the control curve depends on the cost function. In the standard squared control effort case (i.e. L 2 control curve norm employed in §6.1) the resulting control is smooth. Another cost function of interest is T 0 u(t) dt (i.e. the L 1 control curve norm) which is typically imposed along with the constraints u min ≤ u(t) ≤ u max . This case results in a discontinuous optimal control curve. Our formulation can handle such problems easily since the terms u − k and u + k are regarded as the forces before and after time t k , respectively. A computed scenario of a rigid body actuated with two control torques around its principles axes of inertia (Fig. 3) illustrates the discontinuous case.

Extensions
The methods developed in the previous sections are easily adapted to other cases which are of great interest in real applications. In particular, this section will be devoted to the discussion of two important extensions: the case of optimal control problems for Lagrangians of the type l : T M ×g → R (that is, reduction by symmetries on a trivial principal fiber bundle) and the case of nonholonomic systems. Here, M denotes a smooth manifold. Observe that the phase space T M × g unifies the previously studied cases of a tangent bundle and a Lie algebra.
The notion of principal fiber bundle is present in many locomotion and robotic systems [5,8,27]. When the configuration manifold is Q = M × G, there exists a canonical splitting between variables describing the position and variables describing the orientation of the mechanical system. Then, we distinguish the pose coordinates g ∈ G (the elements in the Lie algebra will be denoted by ξ ∈ g), and the variables describing the internal shape of the system, that is x ∈ M (in consequence (x,ẋ) ∈ T M ). Observe that the Lagrangians of the type l : T M × g → R mainly appears as reduction of Lagrangians of the type L : T (M × G) → R, which are invariant under the action of the Lie group G. Under the identification T (M × G)/G ≡ T M × g we obtain the reduced Lagrangian l. We first develop the discrete optimal control problem for systems in an unconstrained principle bundle setting in §7.1. Nonholonomic constraints are then added to treat the more general case of locomotion systems in §7.2.
where (x k , x k+1 ) ∈ M × M and W k ∈ G. Again, we make an election for the discrete control forces f ± k : Similarly to the developments in § 3 and § 4.1 we can formulate the discrete Lagrange-D'Alembert principle: which can be rewritten as k=0 a sequence of independent elements of g such that η 0 = η N = 0. Applying variations in the last expression and rearranging the sum, we finally obtain the complete set of forced discrete Euler-Lagrange equations: with k = 1, . . . , N −1. Since we are dealing with an optimal control problem, we introduce a discrete cost function C d : M × G × M × U × U → R. As in previous cases, our objective is to extremize the following sum subjected to equations (37) and (38). Let us initially restrict our attention to the case of fully actuated systems.
Definition 7.1. (Fully actuated discrete system) We say that the discrete mechanical control system is fully actuated if the mappings are both diffeomorphisms.
According to equations (37) and (38), we can introduce the momenta by means of the following discrete Legendre transforms: In the fully actuated case, is possible to find the value of all control forces in terms of x k , x k+1 , W k , p k , p k+1 , µ k , µ k+1 , that is: Replacing (39) and (40) into C d , we finally obtain the discrete Lagrangian that completely describes our system: The associated discrete cost functional is As usual, we take now variations in (41) in order to obtain the discrete Euler-Lagrange equations for our optimal control problem (with some abuse of notation we denoteQ k = (x k , p k , µ k , W k , µ k+1 , x k+1 , p k+1 ) the whole set of coordinates in the new phase space): , together with the forced discrete Euler-Lagrange equations (37) and (38).
Typically, actuation is achieved by controlling only a subset of the shape variables. In our setting this is can be regarded as underactuationthe mappings in definition 7.1 become embeddings. If this is the case, it is necessary to introduce constraints and apply constrained variational calculus as in § 3.2 and § 4.1.

Discrete Optimal Control of Nonholonomic
Systems. This subsection is devoted to add nonholonomic constraints to the picture. Holonomic constraints might be considered as a pacticular case of the nonholonomic ones (see [23] for further details). With this extension it would be possible consider examples of optimal control of robotic vehicles. In the following we will expose the theoretical framework, leaving for future research the application to concrete examples.
A controlled discrete nonholonomic system on M × M × G is given by the following quadruple (see [14,20] Typically, there is a relation between the constraint distribution and the discrete constraint, since from M c we induce for every x ∈ M , the subspace D c (x) of T x M × g given by with e being the identity element of the Lie group G. iv) The discrete control forces f ± k : M c ×U → T * M ×g * where U ⊂ R m (again, forces f ± k split intof ± k andf ± k as in the previous section). We have the following discrete version of the Lagrange-D'Alembert principle for controlled nonholonomic systems: [14]).
Take a basis of sections {(X a ,η a )} of the vector bundle τ Dc : D c −→ M , where X a ∈ X(M ) andη a : M → g for a = 1, ..., rank(D c ). Hence, the equations of motion derived from the discrete Lagrange-D'Alembert principle for controlled nonholonomic systems are: where Ψ α (x k , x k+1 , W k ) = 0 are the constraints which locally determine M d .
In a more geometric way, we can write equations (42) and (43) as follows Given a discrete cost function C d : U × M c × U −→ R and the optimal control problem is to minimize the action sum subject to equations (42) and (43) and to some given boundary conditions. We next distinguish between the fully and under-actuated case using the following definition: Definition 7.2. (Fully actuated nonholonomic discrete system) We say that the discrete nonholonomic mechanical control system is fully actuated if the mappings u)), are both diffeomorphisms for all (x 0 , x 1 , W 1 ) ∈ M c .
Regarding equation (42) and its geometric redefinition just below, let introduce the following momenta: where both π k and π k+1 belong to D * c . In the fully actuated case, the value of all control forces can be completely determined in terms of x k , x k+1 , W k , π k , π k+1 , where the coordinates (x k , x k+1 , W k ) always belong to M c . Therefore we can re-express the cost function in terms of these variables and, in consequence, derive the discrete Lagrangian Therefore, denotingQ k = (x k , π k , W k , x k+1 , π k+1 ) as the whole set of coordinates of the new phase space D * c ×G×D * c , we deduce that the equations of motion are where λ k α are the Lagrange multipliers of the new constrained problem. The underactuated case can be handled by adding new constraints and applying discrete constrained variational calculus similarly to §4.
A natural framework that simplifies the previous construction is based on discrete mechanics on Lie groupoids [25]. The Lie groupoid structure generalizes the case of Q × Q, the Lie group G and also many intermediate situations. In particular, many of the examples studied in this paper can be modeled using Lie groupoid techniques adapted to our formalism (see [16]).

Conclusions
This paper develops numerical methods for optimal control of Lagrangian mechanical systems defined on tangent bundles, Lie groups, trivial principal bundles, and nonholonomic systems. The proposed approach preserves the geometry and variational structure of mechanics through the discretization of the variational principles on manifolds. The key point is to solve the optimal control through discrete mechanics, i.e. by formulating the optimization as the solution of an action principle of a higher-dimensional system in a new Lagrangian phase space, i.e. T * Q × T * Q in the general case and g * × G × g * in the Lie group case. The optimal control algorithm is then derived as a variational integrator subject to boundary conditions. We thus expect that both the dynamics and optimal control solutions will have accurate and stable numerical behavior (due to symplectic-momentum preservation) even at large time-steps (which allows for improved run-time efficiency).
Simulations of an underactuated underwater vehicle illustrate an application of the method. Yet, further numerical studies and comparisons would be necessary to exactly quantify the advantages and the limitations of the proposed algorithm. An important future direction is thus to study the convergence properties of the optimal control system. Convergence for general nonlinear systems is a complex issue. In this respect, it is interesting to note that the discrete mechanics and optimal control on Lie groups such as the example in using the Cayley map results in polynomial form without further approximation or Taylor series truncation. A useful future direction is then to study the regions of attraction of the numerical continuation using tools from algebraic geometry.
More generally, the theoretical framework introduced in §7 can serve as a basis for deriving algorithms for control systems such as multi-body locomotion systems or robotic vehicles with nonholonomic constraints. Furthermore, the developed classes of systems can be unified through the recently developed groupoid framework [14,37]. Each of the considered product spaces (e.g. Q×Q) can be regarded as a single groupoid space with equations of motion resulting from a single generalized discrete variational principle. This will enable the automatic solution of optimal control problems for various complex systems and a convenient unified framework for implementing practical optimization schemes such as [4,18,21,35]. More importantly, this viewpoint can be used to apply standard discrete Lagrangian regularity conditions (e.g. [31]) to optimal control problems evolving on the groupoid space. This would provide a deeper insight into the solvability of the resulting optimization schemes.