On variational integrators for optimal control of mechanical control systems. Leonardo Colombo, David Martín de Diego and Marcela Zuccalli

Transcription

1 RACSAM Rev. R. Acad. Cien. Serie A. Mat. VOL. Falta Falta), Falta, pp Falta Comunicación Preliminar / Preliminary Communication On variational integrators for optimal control of mechanical control systems Leonardo Colombo, David Martín de Diego and Marcela Zuccalli Abstract. In this paper we study discrete second-order vakonomic mechanics, that is, constrained variational problems for second-order lagrangian systems. One of the main applications of the presented theory will be optimal control of underactuated mechanical control systems. We derive geometric integrators which are symplectic and preserve the momentum map. Additional, we show the applicability of the proposed theory in an example, the planar rigid body. 1. Introduction One of the most important problems in engineering are related with planning and control of robotic devices. The applications include the optimal design of autonomous vehicles and of minimalist manipulators. In this sense, for fully actuated mechanical systems a broad range of powerful techniques were developed in the last decades. These techniques are possible because fully actuated systems possess a number of strong properties that facilitate control design, such as feedback linearizability, passivity, matching conditions, and linear parametrizability. For underactuated systems one or more of the above structural properties are usually lost. For these reasons, control design becomes much more difficult and there are correspondingly less results available. The class of underactuated mechanical systems include spacecraft, underwater vehicles, satellites, mobile robots, helicopters, wheeled vehicles, mobile robots, underactuated manipulators, etc. With this motivation in mind, we will develop in this paper the study of discrete second-order vakonomic mechanics showing its applicability for optimal control of underactuated mechanical systems. This study is motivated by the importance of the design of structure-preserving algorithms for numerical simulation of controlled systems. A main idea for this paper will be the design of variational integrator using discrete variational calculus adapted to second-order theories. For this type of systems it is possible to construct discretization schemes which preserve an appropriate symplectic form and the momentum map associated with a Lie group action. The variational integrators have shown, in explicit examples, an exceptionally good long-time behaviour and this research is of great interest from numerical and geometrical considerations see [9, 4]). The paper is organized as follows. In Section 2, we consider an optimal control problem for an underactuated mechanical system and we reduce it to an equivalent second-order variational problem using standard Presentado por Falta. Recibido: Falta. Aceptado: Falta. Palabras clave / Keywords: Underactuated mechanical system, constrained variational calculus, Levi-Civita connection, optimal control, vakonomic mechanics, Lagrangian mechanics, Higher-order mechanics, Discrete mechanics, Variational integrators Mathematics Subject Classifications: Falta c Falta Real Academia de Ciencias, España. 1

2 Leonardo Colombo, David Martín de Diego and Marcela Zuccalli tools from riemannian geometry. In Section 3, we recall some basic elements of discrete vakonomic mechanics [2]. In Section 4, we develop discrete variational calculus for second-order dynamics. Using this formulation we derive variational integrators for optimal control problems for underactuated mechanical systems. As an example, we use this geometric integrator to the optimal control of the planar rigid body. 2. Optimal control of underactuated mechanical systems In this section we consider a particular class of mechanical control systems, underactuated system; that is, systems in which the number of control inputs is less than the dimension of the configuration space. We assume that these systems are controllable, that is, for any two points x 0 and x f in the configuration space, there exists an admissible control ut) defined on some time interval [0, T ] such that the system with initial condition x 0 reaches the point x f in time T see for more details [5, 4]). Let Q be a differentiable manifold of dimension n with local coordinates q A ) and G a riemannian metric specifying the kinetic energy of the mechanical system. The metric is locally written as G = G AB dq A dq B. Denote also by G : T Q T Q the induced vector bundle isomorphism and by # G : T Q T Q the inverse isomorphism. We can construct the Levi-Civita connection G on Q as the unique affine connection which is torsion-less and metric with respect to G. It is determined by the standard formula for all X, Y, Z XQ). 2G G XY, Z) = XGY, Z)) + Y GX, Z)) ZGX, Y )) +GX, [Z, Y ]) + GY, [Z, X]) GZ, [Y, X]) Fixed a potential function V : Q R, the mechanical system is defined by the mechanical Lagrangian L : T Q R, and the corresponding equations of motion are Here, grad G V is the vector field on Q characterized by Lv q ) = 1 2 Gv q, v q ) V q), where v q T q Q 1) Ġ ct)ċt) + grad GV ct)) = 0. 2) Ggrad G V, X) = XV ), for every X XQ). In local coordinates the Equations 2) are d 2 q C dt 2 = Γ C ABqt)) dqa dq B AB V G dt dt q C. 3) where G AB ) are the entries of the inverse matrix of G AB ) and where Γ C AB are the Christoffel components of the Levi-Civita connection. Adding control forces in our picture, given by {θ m+1,..., θ n }, where θ a are independent 1-forms on Q, m + 1 a n; then if we denote by Y a = G θ a the corresponding input sections, then equations 2) are modified as follows: Ġ ct)ċt) + grad G V ct)) = u at)y a ct)). 4) where u m+1,..., u n ) U R n m. Denote by D = span{y a }, m + 1 a n, and by {Z α }, 1 α m a basis of D. An alternative way of writing Equations 4) is the following one C ab G Ġ ct)ċt) + grad G V ct)), Y b) = u a, m + 1 a n 5) G Ġ ct)ċt) + grad G V ct)), Zα) = 0, 1 α m 6) 2

3 On Variational Integrators for optimal control of mechanical control systems where C ab = G Y a, Y b). Given a cost function C : T Q U R, a solution of the optimal control problem consists on a trajectory q A t), u a t)) of the state variables and control inputs satisfying equations 5) and 6) with given initial and final conditions q A t 0 ), q A t 0 )) and q A t f ), q A t f )) respectively, extremizing the cost functional A = tf t 0 Cq A, q A, u a ) dt. This optimal control problem is equivalent to the following second order constrained problem see [6] for an intrinsic point of view): Extremize the functional: Ã = tf t 0 Lq A t), q A t), q A t)) dt subjected to the second-order constraints given by Φ α q A, q A, q A ) = G Ġ ct)ċt) + grad G V ct)), Zα) = 0, with α = 1,..., m, where L : T 2) Q R is defined as Lq A, q A, q A ) = C q A, q A, C ab G Ġ ct)ċt) + grad G V ct)), Y b)). Notice that T 2) Q denotes the second order tangent bundle of Q see [10]). 3. Dicrete vakonomic mechanics In this section we recall some basic elements for discrete vakonomic mechanics using discrete variational calculus with constraints see [2]). The solutions of the vakonomic problem are the critical sequences of a discrete action sum subjected to some constraint functions. Consider a continuous vakonomic system defined by a Lagrangian L : T Q R and a constraint submanifold M of T Q locally defined by the vanishing of m-independents constraints Φ α : T Q R with 1 α m. For a discretization of this systems, we substitutes the velocity space T Q by the cartesian product Q Q, and then the lagrangian L by a discrete lagrangian function L d : Q Q R this discrete Lagrangian may be considered as an approximation of the continuous Lagrangian L). In the same way, the discrete constraint submanifold M d Q Q defined, locally, by the vanishing of m-independent constraints functions Φ α d : Q Q R, 1 α m. Fixed q 0, q N Q for some integer N > 2) consider the sequences q 0, q 1,...,, q N ) Q N+1. We also assume that the sequences verify the discrete constraints if Φ α d q k, q k+1 ) = 0, k = 0,..., N 1. Define the discrete action sum by A d q 0, q 1,..., q N ) = N 1 k=0 L d q k, q k+1 ). The discrete constrained variational problem is defined by { ext Ad q 0, q 1,..., q N ) with q 0 and q N fixed suject to Φ α d q k, q k+1 ) = 0, 1 α m and 0 k N 1. 7) 3

4 Leonardo Colombo, David Martín de Diego and Marcela Zuccalli We construct the augmented Lagrangian L d : Q Q R m R: L d x, y, λ) = L d x, y) + λ α Φ α d x, y). From the classical Lagrange multiplier Lemma [1], we deduce that the solutions of the problem 7) coincides with the solutions of the discrete variational problem { ext Ad q 0, q 1,..., q N, λ 0, λ 1,..., λ N 1 ) with q 0 and q N fixed, q k Q, λ k R m 8) k = 0,..., N 1, q N Q, where A d q 0, q 1,..., q N, λ 0, λ 1,..., λ N 1 ) = and λ k is a m-vector with components λ k α with 1 α m. N 1 k=0 L d q k, q k+1, λ k ) Therefore, applying standard discrete variational calculus we deduce that the solutions of problem 7) verify the following set of difference equations { D1 L d q k, q k+1 ) + D 2 L d q k 1, q k ) + λ k αd 1 Φ α d q k, q k+1 ) + λ k 1 α D 2 Φ α d q k 1, q k ) = 0, 1 k N 1, Φ α d q k, q k+1 ) = 0, 1 α m and 0 k N 1 where D 1 L d and D 2 L d denote the derivatives of the discrete lagrangian L d respect to the first and the second arguments, respectively. This equations are called discrete vakonomic equations. For all function F C 2 ) F Q Q) we denote by D 12 F the n n-matrix x A y B. Then, if the matrix D 12 L d x, y) + λ α D 12 Φ α Φ α d d x, y) x, y) ) x Φ α T d x, y) 0 m m y M n+m) n+m)r) is regular, then, by a direct application of the implicit function theorem, we deduce that, in a neighborhood of the point x, y, λ), exists a unique local application Υ d : M d R m M d R m x, y, λ) y, z, Λ), such that for all solutions q 0, q 1,..., q N, λ 0, λ 1,..., λ N 1 ) of the discrete vakonomic equations, we have that locally Υ d q k 1, q k, λ k 1 ) = q k, q k+1, λ k ). The application Υ d is called the discrete flow of the discrete vakonomic problem. Remark 1 In [2], the authors have shown that, under the regularity condition, the discrete flow Υ d preserves a symplectic form naturally defined on M d R m given locally by [ 2 L d ω d x, y, λ) = x A x, y) + λ 2 Φ α ] d yb x A x, y) dx A dy B + Φα d yb x A x, y)dλ α dx A and if the discrete Lagrangian L d and the constraint Φ α d are invariant under the action of a Lie group G on Q, the flow preserves the momentum map, that is where J d : Q Q R m g is given by x, y, λ) J d x, y, λ) : J d Υd q k 1, q k, λ k 1 ) ) = J d q k 1, q k, λ k 1 ). g R ξ D 2 L d x, y) + λ α D 2 Φ α d x, y), ξ Q y), where ξ Q is the infinitesimal generator of ξ and g is the Lie algebra of G 4

5 On Variational Integrators for optimal control of mechanical control systems 4. A variational integrator for optimal control of underactuated mechanical systems In this section we extend the results of the previous section to the case of constrained second-order lagrangian dynamics. As a consequence of the previous sections, the derived numerical methods are directly applied to our initial objective: geometric integrators for optimal control problems for underactuated mechanical systems. In particular, we design numerical methods preserving an appropriate symplectic form and they are momentum preserving Discrete second-order vakonomic mechanics Given a second order vakonomic mechanical system with Lagrangian L : T 2) Q R and a second order constraint submanifold M of T 2) Q, locally defined by the vanishing of m independent constraints Φ α : T 2) Q R. A natural discretization of this system consists on a discrete Lagrangian function L d : Q Q Q R and a constraint submanifold M d of Q Q Q which is locally defined by the vanishing of m-constraint functions Φ α d : Q Q Q R. Fixed q 0, q 1 and q N 1, q N for some integer N > 4), we consider the discrete sequences on Q, q 0, q 1,...,, q N ) Q N+1 verifying the discrete constraints Φ α d q k 1, q k, q k+1 ) = 0 k = 1,..., N 1 by Define the discrete action sum by A d q 0,..., q N ) = N 2 k=0 L d q k, q k+1, q k+2 ). We are looking for solutions of the following discrete variational problem with constraints ext A d q 0, q 1,..., q N ), with q 0, q 1 and q N 1, q N fixed subject to Φ α q k, q k+1, q k+2 ) = 0 with 1 α m and 0 k N 2. As, in the previous section, consider the augmented Lagrangian L d : Q Q Q R m R defined L d x, y, z, λ) = L d x, y, z) + λ α Φ α d x, y, z). The solutions of the problem 9) coincide with the solutions of the following problem { extremize Ad q 0, q 1,..., q N, λ 0, λ 1,..., λ N 2 ) with q 0, q 1 and q N 1, q N fixed where q k Q, λ k R m, k = 0,..., N, A d q 0, q 1,..., q N, λ 0, λ 1,..., λ N 2 ) = and λ k is a m-vector with components λ k α with 1 α m. Hence, the extremality conditions are N 2 k=0 L d q k, q k+1, q k+2, λ k ) 0 = D 3 Ld q k 2, q k 1, q k ) + D 2 Ld q k 1, q k, q k+1 ) +D 1 Ld q k, q k+1, q k+2 ) + λ k 2 α D 3 Φ α d q k 2, q k 1, q k ) +λ k 1 α D 2 Φ α d q k 1, q k, q k+1 ) + λ k αd 1 Φ α d q k, q k+1, q k+2 ), 2 k N 2 0 = Φ α d q k 2, q k 1, q k ) 0 = Φ α d q k 1, q k, q k+1 ) 0 = Φ α d q k, q k+1, q k+2 ). 9) xx 10) 5

6 Leonardo Colombo, David Martín de Diego and Marcela Zuccalli If the matrix D13 Ld x, y, z) + λ α D 13 Φ α d x, y, z) D 3 Φ α d x, y, z) D 1 Φ α d x, y, z)) T 0 ) regularity condition) 11) is regular for all x, y, z) M d and λ α R, 1 α m, then by a direct application of the implicit function theorem, we deduce that there exists a local) unique application Υ d : M d R 2m M d R 2m q 0, q 1, q 2, q 3, λ 0 α, λ 1 α) q 1, q 2, q 3, q 4, λ 1 α, λ 2 α) which univocally determines q 4 and λ 2 α, 1 α m from the initial conditions q 0, q 1, q 2, q 3, λ 0 α, λ 1 α). Here, M d denotes the submanifold of Q 4 = Q Q Q Q M d = {q 0, q 1, q 2, q 3 ) Q 4 Φ α d q 0, q 1, q 2 ) = 0, Φ α d q 1, q 2, q 3 ) = 0, 1 α m}. The mapping Υ d will be called the discrete second-order vakonomic flow. Remark 2 See [7]). Using similar techniques than in [2] and [3] it is possible to show that, under the regularity assumption, the discrete second-order vakonomic flow is symplectic and preserves momentum when we have an action preserving the discrete lagrangian L d and the constraint submanifold M d. Let Θ d, Θ+ d be the discrete canonicals one forms on Q4 R 2m given by Θ + d q 0, q 1, q 2, q 3, λ 0, λ 1 ) = 1 i+1 D j L d q i j+1, q i j+2, q i j+3 ) + λ i j+1 α D j Φ α d q i j+1, q i j+2, q i j+3 ) dq i i=0 j=1 Θ + d q N 3, q N 2, q N 1, q N, λ N 3, λ N 2 ) = N 3 D j L d q i j+1, q i j+2, q i j+3 ) + λ i j+1 α D j Φ α d q i j+1, q i j+2, q i j+3 ) dq i i=n 1 j=i N+3 We define the discrete Poincaré-Cartan 2-form, Ω d on Q 4 R 2m, Ω d = dθ + d = dθ d Ω Md Ω d q 0, q 1, q 2, q 3, λ 0, λ 1 ) = [ D 21 L d q 0, q 1, q 2 ) + λ 0 αd 21 Φ α d q 0, q 1, q 2 ) ] dq 1 dq 0 + [ D 31 L d q 0, q 1, q 2 ) + λ 0 αd 31 Φ α d q 0, q 1, q 2 ) ] dq 2 dq 0 + D 1 Φ α d q 0, q 1, q 2 )dλ 0 α dq 0 + [ D 11 L d q 1, q 2, q 3 ) + λ 1 αd 11 Φ α d q 1, q 2, q 3 ) + D 12 L d q 0, q 1, q 2 ) + λ 0 αd 12 Φ α d q 0, q 1, q 2 ) ] dq 0 dq 1 + [ D 31 L d q 1, q 2, q 3 ) + λ 1 αd 31 Φ α d q 1, q 2, q 3 ) + D 32 L d q 0, q 1, q 2 ) + λ 0 αd 32 Φ α d q 0, q 1, q 2 ) ] dq 2 dq 1 +D 1 Φ α d q 1, q 2, q 3 )dλ 1 dq 1 + D 2 Φ α d q 0, q 1, q 2 )dλ 0 dq 1 If we define j : M d R 2m Q 4 R 2m, the canonical inclusion, this give rise the two form = j Ω d. From the definition of Υ d it is obvious that it applies M d R 2m onto itself. Moreover, Υ Md R 2m ) Ω Md = Ω Md Assume that additionally we have an action of a Lie group G on the space Q. It naturally induces and action on the Q Q Q by g, q 0, q 1, q 2 )) gq 0, gq 1, gq 2 ) where g G. Construct now the discrete momentum map : Q4 R 2m g J ± d 6

7 On Variational Integrators for optimal control of mechanical control systems by J ± d q 0, q 1, q 2, q 3, λ 0, λ 1 ) : g R ξ Θ ± d q 0, q 1, q 2, q 3, λ 0, λ 1 ), ξ Q 4q 0, q 1, q 2, q 3 ) where ξ Q 4 is the infinitesimal generator corresponding to an element of the Lie algebra ξ g, that is, ξ Q 4q 0, q 1, q 2, q 3, λ 0, λ 1 ) = ξ Q q 0 ), ξ Q q 1 ), ξ Q q 2 ), ξ Q q 3 )). If the discrete lagrangian L d and the discrete constraints Φ d are invariant under this action then L d = L d + λ α Φ α d is also invariant, and, as a consequence, J + d = J d = J d. Under these conditions we can show that JΥd q 0, q 1, q 2, q 3, λ 0, λ 1 ), ξ = Jq 0, q 1, q 2, q 3, λ 0, λ 1 ), ξ. Therefore, from the symmetry invariance we deduce the preservation of the momentum map, that is, we have deduced a discrete version of the classical Noether theorem for this particular kind of systems Application to optimal control for underactuated mechanical systems In this section, we show that the discrete vakonomic approach of a second-order problem is and appropriate framework for the treatment of the discrete versions of optimal control problems for underactuated mechanical system considered in Section 3. The main application is the construction of geometric numerical integrators for this type of systems. A reasonable discretization of the Euler-Lagrange equations with controls is D 2 L d q k 1, q k ) + D 1 L d q k, q k+1 ) + hu a ) k G Y a ) q k= 0, 1 k N 1. 12) where h is the fixed time step. Using the same idea that in Section 2, we can rewrite Equation 12) as C ab q k ) D 2 L d q k 1, q k ) + D 1 L d q k, q k+1 ), Y a qk = hu a ) k, m + 1 a n 13) D 2 L d q k 1, q k ) + D 1 L d q k, q k+1 ), Z α qk = 0, 1 α m. 14) The optimal control problem is determined prescribing the discrete cost functional A d = N 1 k=1 hcq k, q k+1, u a ) k ) with initial and final conditions q 0, q 1 and q N 1, q N, respectively. Since the control variables appear explicitly in 12), the previous discrete optimal control problem is equivalent to the second-order discrete vakonomic problem determined by L d q k 1, q k, q k+1 ) = C q k, q k+1, 1 ) h C abq k ) D 2 L d q k 1, q k ) + D 1 L d q k, q k+1 ), Y a qk Φ α d q k 1, q k+1, q k+2 ) = D 2 L d q k 1, q k ) + D 1 L d q k, q k+1 ), Z α qk. As in the previous subsection, we define L d : Q Q Q R m R as L d q k, q k+1, q k+2, λ α,k ) = L d q k, q k+1, q k+2 ) + λ α,k Φ α d q k, q k+1, q k+2 ), obtaining a second-order vakonomic system and from the results deduced on the previous section we can derive the discrete equations of motion which give us a geometric integrator preserving symplecticity and momentum map if it is the case) for optimal control problems for underactuated mechanical systems. 7

8 Leonardo Colombo, David Martín de Diego and Marcela Zuccalli Example 1 Planar Rigid Body: The configuration space for this system in Q = R 2 S 1 and it can be considered as the simplest example in the category of rigid body dynamics, but adequate to test our method. The three degrees of freedom describe the translations in R 2 and the rotation about its center of mass. The configuration is given by the followings variables: θ describes the relative orientation the body reference frame with respect to the inertial reference frame. The vector x, y) denotes the position of the center of mass measured with respect to the inertial reference frame. The lagrangian is of kinetical type L = 1 2 qt M q, where M = m m J and where m is the mass of the body and J is its moment of inertia about the center of mass. For simplicity we assume that the body moves in a plane perpendicular to the direction of the gravitational force, and then the potential energy is zero. Additionally, we assume that there are not acting frictional forces. For the planar body, the control forces that we consider are applied to a point on the body with distance l > 0 from the center of mass, along the body x axis see [5] for more details about this example). The equations of motion are The control vector fields are mẍ = u 1 cos θ u 2 sin θ mÿ = u 1 sin θ + u 2 cos θ J θ = lu 1 Y 2 = cos θ m Y 3 = sin θ m x + sin θ m x + cos θ m y l J y and element Z 1 generating the orthogonal D of D = span {Y 2, Y 3 }, θ Z 1 = l cos θ x + l sin θ y + θ. The equations of motion are now modified as follows, mj ) J + ml 2 cos θẍ + sin θÿ l θ = u 1 m sin θẍ + m cos θÿ = u 2 ml cos θẍ + l sin θÿ + J θ = 0. Consider the following cost function C = u2 1 + u 2 2, then this optimal control problem is equivalent to 2 the following second-order constrained problem with second-order constraints: Extremize à = tf t 0 Lq A t), q A t), q A t)) dt subject to the second-order constraints given by Φ α q A, q A, q A ) = ml cos θẍ + l sin θÿ + J θ = 0. 8

9 On Variational Integrators for optimal control of mechanical control systems Here L : T 2) Q R is defined by Lq A, q A, q A ) = m2 J 2 ) 2 2 J + ml 2 ) 2 cos θẍ + sin θÿ l θ + sin θẍ + cos θÿ) 2) Now, we develop a discrete version of this higher-order constrained problem. Consider the discretization L d : Q Q R, with Q = R 2 S 1, of the lagrangian L : T Q R: and L d x k, y k, θ k, x k+1, y k+1, θ k+1 ) = 1 mxk+1 x k ) 2 + my k+1 y k ) 2 + Jθ k+1 θ k ) 2) 2h G Y 2 ) q k = cos θ k dx + sin θ k dy l dθ G Y 3 ) q k = sin θ k dx + cos θ k dy G Z 1 ) q k = ml cos θ k dx + ml sin θ k dy + J dθ where the control forces are applied to a point on the body with distance l > 0 from the center of mass, along the body x axis. Denoting by 2 [q k ] = q k+2 2q k+1 +q k h, then the discrete control equations are 2 m 2 [x k ] = u 1 ) k cos θ k u 2 ) k sin θ k m 2 [y k ] = u 1 ) k sin θ k + u 2 ) k cos θ k J 2 [θ k ] = lu 1 ) k Therefore we obtain the following constrained second-order discrete variational problem: L d q k, q k+1, q k+2 ) = m2 h J 2 cos θk 2 J + ml 2 ) 2 2 [x k ] + sin θ k 2 [y k ] l 2 [θ k ] ) 2 + sin θ k 2 [x k ] + cos θ k 2 [y k ] ) ) 2 Φ α d q k, q k+1, q k+2 ) = ml cos θ k 2 [x k ] + l sin θ k 2 [y k ] + J 2 [θ k ] = Conclusions and future work In this paper, we have introduced a discrete geometric framework for optimal control of mechanical systems. Our procedure is strongly based on the formulation of second-order discrete vakonomic systems. Moreover, the construction allows us to design variational integrators sharing some qualitative properties with the continuous case, as for instance, symplectic and momentum preserving, and, as a consequence a good energy behavior associated with these systems. In a future paper, we will study the performance of the proposed numerical methods, preferably, for long-time integration and the extension of backward error analysis to these methods. Acknowledgement. This work has been supported by MICINN Spain) Grant MTM C02-01, MTM E, project Ingenio Mathematica i-math) No. CSD Consolider- Ingenio 2010) and IRSES-project Geomech L.Colombo also wants to thank CSIC for a JAE- Pre grant. The authors wish to thank the referee for his helpful comments. References [1] Abraham R, Marsden JE. 1978). Foundations of Mechanics. Second Edition, Benjamin/Cummings, New York. 9

10 Leonardo Colombo, David Martín de Diego and Marcela Zuccalli [2] Benito R, Martín de Diego D. 2005). Discrete Vakonomic Mechanics. Journal of Mathematical Physics, 46, [3] Benito R, de León M, Martín de Diego D. 2006). Higher order discrete Lagrangian mechanics. International Journal of Geometric Methods in Modern Physics, 3, [4] Bloch A.M. 2003). Nonholonomic Mechanics and Control. Interdisciplinary Applied Mathematics Series, 24, Springer-Verlag, New York. [5] Bullo F, Lewis A. 2005). Geometric Control of Mechanical Systems: Modeling, Analysis, and Design for Simple Mechanical Control Systems. Texts in Applied Mathematics, Springer Verlag, New York. [6] L. Colombo, D. Martin de Diego and M. Zuccalli. 2010) Optimal Control of Underactuated Mechanical Systems: A Geometrical Approach. Journal Mathematical Physics 51, [7] L. Colombo, D. Martin de Diego and M. Zuccalli. Higher-order Discrete vakonomic mechanics. Preprint [8] Cortés J. 2002). Geometric, Control and Numerical Aspects of Nonholonomic Systems. Lec. Notes in Math., 1793, Springer-Verlag, Berlin [9] Hairer E, Lubich C, Wanner G. 2002). Geometric Numerical Integration, Structure-Preserving Algorithms for Ordinary Differential Equations Springer Series in Computational Mathematics, 31, Springer-Verlag Berlin [10] de León M and Rodrigues PR. 1985). Generalized Classical Mechanics and Field Theory, North-Holland Mathematical Studies 112, North-Holland, Amsterdam. Colombo Leonardo: leo.colombo@icmat.es; Instituto de Ciencias Matemáticas ICMAT-CSIC- UAM-UC3M-UCM) C/ Serrano 123, Madrid, Spain Martín de Diego David: david.martin@icmat.es; Instituto de Ciencias Matemáticas ICMAT- CSIC-UAM-UC3M-UCM) C/ Serrano 123, Madrid, Spain Zuccalli Marcela: marce@mate.unlp.edu.ar; Departamento de Matemáticas, Facultad de Ciencias Exactas, Universidad Nacional de La Plata. Calle 50 y 115, 1900 La Plata, Buenos Aires, Argentina 10