- Email: [email protected]
Geometric Numerical Integration of Nonholonomic Systems and Optimal Control Problems
IFAC
Copyright i:- IFAC Lagrangian and Hamiltonian Methods for Nonlinear Control, Seville, Spain, 2003
C:OC> Publications www.elsevier.com ·locate:ifac
GEOMETRIC NUMERICAL INTEGRATION OF NONHOLONOMIC SYSTEMS AND OPTIMAL CONTROL PROBLEMS M. de Le6n, * D. Martin de Diego, * A. Santamaria-Merino *.1
*
Instituto de Matematicas y Fisica Fundamental. CSIC. Serrano 123. 28006 Madrid, Spain
Abstract: A geometric derivation of numerical integrators for nonholonomic systems and optimal control problems is obtained. It is based in the classical technique of generating functions adapted to the special features of nonholonomic systems and optimal control problems. Copyright © 20031FAC Keywords: Geometric integrators, nonholonomic systems, optimal control
1. INTRODUCTION
theory by using generating functions of the second kind .
Standard methods for simulating the motion of a dynamical system usually ignore many of the geometric features of this system (simplecticity. conservation laws. symmetries .. . ). However. new methods have been recently developed . called geometric integrators. which are concerned with some of the extra features of geometric nature of the dynamical system (see [HaLuWa:02]).
2.
~O~HOLONOr..IIC
SYSTEr..lS
2.1 Geometrical formulation of nonholonomic systems Let Q be an n-dimensional differentiable manifold. with local coordinates (qi) and tangent bundle TQ. with induced coordinates (qi. (y). Con::;ider a Lagrangian system. with Lagrangian L : TQ -+ R subject to nonholonomic constraints. defined by a submanifold D of the velocity phase space TQ. \Ye will assume that dim D = 2n - m and that D is locally described by the \'anishing of m independent functions oQ (the "constraint functions").
In the first part of the paper. we propose a class of geometric integrators for nonholonomic systems [Leomar:96D.NeiFuf:72] based on a discretization of the Lagrangian function (in a more precise sense. we discretize the action function) and a coherent discretization of the constraint forces (see [Le:\laSa:02]) . These equations will be conceptually equivalent to the proposed for systems with external forces (see [I\lar\Yes:Ol]). Finally. the second part is corcerned with the construction of s~'mplectic integrators for optimal control
satisfying the rank condition rank
(~~~)
= m.
In what follows. we will follow a Hamiltonian point of view. The canonical coordinates on T*Q (the cotangent bundle of Q) are denoted by (q' . Pi). Assume. for simplicity. that the Lagrangian L is h~' perregular. that is. the Legendre transformation Leg: TQ -; T*Q.(qi.i/) ....... (qi ,Pi = aL / ai/). is a global diffeomorphism. The constraint functions on T*Q become IJIIl = Oil 0 Leg-I. i.e.
I This work has been supported by grant BF\12001-2272. :\.. Santamarfa t'llerino wishes to thank the Programa de formaci6n de Im'estigadores of the Departamento de Educaci6n . llniversidades e Investigaci6n of the Basque Government (Spain) for financial support.
1JI"(qi. Pi ) = O"(qi.
141
~H). UPi
where the Hamiltonian
.,
H : T*Q ----+ lR is defined by H = EL 0 Leg-I. Here. EL denotes the energ~: of the system. locally defined by EL = qi L. Since locally . . . aH Leg- I (q' · Pi) = (q"-a ). then H = p;Q' Pi L(qi . qi) . where q; is expressed in terms of qi and Pi by using Leg-I.
q
Zf, -
(1) •
then LXHAI()Q = d(ixHAI()Q - H) - A . or. equivalently. LXHA/()Q = d(L Therefore. integrating we have
F; ()Q
Let 111 denote the image of the constraint submanifold D under the Legendre transformation. and let F be the distribution on T*Q along 111. whose annihilator is given by FO = Leg*(FO)) . Here. FO represents the constraint forces subbundle, 10-
= span{J.1Q
=
ao"
oqi
X 1Al ET1I1.
.
matrix (cab) =
'( ow 0Pi
)
Leg-I) - A
d(1" LoFt dt) - 1" Ft A.
(4)
and nonholonomic
In what follows , we will follow similar arguments for the construction of generating functions for symplectic or canonical maps [Arn:78] . However. because of equation (4). we have that the nonholonomic flow is not a canonical transformation: i.e.,
(2)
This description will allow us to construct a new family of nonholonomic integrators for equations (19). Denote by 7ri : rQ x T*Q ----+ rQ. i = 1. 2. the canonical projections. Consider the following forms
b
Hij OW OPj
=
2.2 "Generating functions" mechanics
dq'} . The
where wQ = -d()Q = dqi Adp, (with ()Q = Pi dqi) is the canonical symplectic form on T*Q. Suppose in addition that the following compatibility condition FJ. n T 111 = {O} holds, where" ..l .. denotes the symplectic orthogonal with respect to wQ . Observe that. locally. this condition means that the a
- ()Q
0
where Ft is the flow of the vector field ~L.D.
Hamiltonian equations of motion of the nonholonomic system can be then rewritten in intrinsic form as (iX'-
(3)
A, .
)l'ow. consider the flow Ft : 111 ~ M. t E I ~ lR generated by the vector field XlI.M. solution of the nonholonomic problem. Since (3) is geometricall~' rewritten as
aw a aoa (-a H)i)(q·P) = (a'; o Leg-I)(q , p). Pj q
_
op, .
for initial conditions (qo. Po) E 111 and A = A, dq'. We also denote by A = Leg*(A) the I-form on TQ ",-ich represents the constraint force once the Lagrange multipliers have been determined.
together with the constraint equations Wa(q.p) = O. Here Hij are the components of the inverse of the matrix (Hil) = (a2H/ apiapj). Note that
cally defined by FO
oH
{ P, - - ~: -
The equations of motion for the nonholonomic s~-stem on T*Q can now be written as follows (see [CaLel\la:99.:'.larl:95] and references therein) ., aH q =ap, aH aw a { p, - - aq' - Aa ap) H ji
=
is regular. The
compatibility condition is not too restrictive. since it is trivially verified by the usual systems of mechanical type (i .e. with a Lagrangian of the form kinetic minus potential energy). where the Hij represent the components of a positive definite Riemannian metric. The compatibility condition guarantees. in particular. the existence of a unique solution of the constrained equations of motion (2) which. henceforth. will be denoted by X H .:H on the Hamiltonian side. and Leg;I(XH ..H) = ~L.D on the Lagrangian side.
e= n=
7r;()Q - 7r;()Q .
7I2""':Q - 7ri""':Q = -de.
Denote by iFh : Graph(F,,) '----t rQ x rQ the inclusion map and observe that Graph(Fh) C ,\1 x 111. Then. from (4). iF" e is equal to
l\Ioreover. if we denote by X H the Hamiltonian function of H. i.e .. iXHWQ = dH then. using the constraint functions. we may explicitely determine the Lagrange multipliers An as An = -CabXH(Wb) . :'-J'ext. writing the I-form A = b DIJI" . -CabXH(W )~H)idq' . then. the nonholonomic PJ equations are equi\"alently rewritten as
Let (qo.Po.ql.pd be coordinates in T*Q x rQ in a neighborhood of some point in Graph(F,,). If (qo.Po.ql.pd E Graph(F,,) then wQ(qo.po) = 0 and w"(ql.pd = O. :\Ioreover. along Graph(Fh). ql = ql(qo·Po). PI = pdqo·Po) and
142
(1" -1"
PI dql - Podqo = d
integral (resp .. second integral) of the right-hand !:iide.
L(q(t). q(t)) dt)
Proof: It suffices to prove the result for 1'.; that is.
(6)
A(q(t).q(t)).
where (q(t). q(t)) = Ft (qo. qo) with Leg(qo· qo) = (qo. Po)· Equation (6) is satisfied along Graph(F,,),
S2h(qo.q2) = Sh(qo.qd
= 2.
+ Sh(ql.q2).
where ql Yerifies the condition (9).
Assume that. in a neighborhood of some point :1' E Graph(F,,). we can change this system of coordinates to a new coordinates (qo. qd. Denote by
Since
dql - Po dqu = dS"(qo. qIl _1h A(q(t). q(t)) .
PI
h
S"(qo. qd
= r L(q(t). q(t)) dt.
Jo
P2dq2-Pldql=dSh(ql.q2)-12hA(q(t).q(t)) .
where q(t) is a solution curve of the nonholonomic problem with q(O) = qo and q(h) = ql and an adequate extension of S". It is easy to show that this solution always exists for adequate values of qo and ql'
then
pz dq2 - Po dqo = d (S" (qo. qd
_1h A(q(t). q(t)) - 12h A(q(t) . q(t)) .
Thus, we deduce that
Po
1" lh - .
as" + = -~ uqo
os" { PI = ~ uql
oq . A(q(t). q(t))!:} uqo
0
oq
Since the variables ql do not appear on the left-hand side term, we obtain expression (9) . ~loreover. for this choice of ql . S2h (qo. q2) = Sh (qo. qd + Sh (ql , q2) is a "generating function of the first kind" of F 2 h . I
(7)
A(q(t). q(t))~ . uql
0
where (qo. ql) verifies the constraint functions <.po (qo. ql . h) = O. explici tely defined by -.pO (qo. ql . h)
llJa(qo.
=
OSh
-~(qo.
u%
qd
+
1" -
Equations (9) determine an implicit system of difference equations which permit us to obtain q2 from the initial data qo and ql'
oq (8) A(q(t). q(t))!:}).
2.3 Nonholonomic integrators
u%
0
where q(t) is a solution of the nonholonomic problem with q(O) = qo and q(h) = q".
In what follows . assume that Q is a vector space. Since Sh(qo. qd = Ioh L(q(t). q(t)) dt , where q(t) is a nonholonomic solution with q(O) = qo and q(h) = ql. we can obtain nonholonomic integrators by taking adequate approximations of the "generating function" Sit and the extra-term
Next. we will show how the group composite law of the flow F". F l\'" = F" 0 . . . 0 F". is expressed in "-v--"
s
terms of the corresponding "generating functions" Sh . ~loreover. the following Theorem "'ill result in a new construction of numerical integrators for nonholonomic mechanics when we change the "generating function" and the constraint forces by appropriate approximations.
Ioh A.(q(t). q(t)) . Consider. for instance. the approximation h
Sa(qo·qd
L
S"(q".qk+d·
q rh A(q(t). q(t)) aa Jo qo
where qk . 1 :S k :S 1\' - 1. are points verifying
" . 1 o
+ DIS"(qk.qk+l)
~ (1 -
=
oq 12h .\(q(t) . oq A(q(t).q(t))!:} + . q(t))!:}. uq\
h
uqo
(10)
A natural approximation of the constraint forces adapted to our choice of approximation for Sh are
.\; - 1
S ·'lih(qO.q.v) =
ql - qo
= hL((l - o)qo + oq\. - h - ) '
for some parameter 0 E [0. 1]. (In general. we will write S~(qo.qd ~ Sh(qo.qd.)
Theorem 2.1. The function SNh. the "generating function" for F.";}I' is giYen b!'
D 2S h (qk _ l.qk)
+ Sh (ql . q2 ) )
l
(9)
h -
o
and q(t) is a solution cun'e of the nonholonomic problem with q(O) = qk-I and q(h) = qk (respectively. q(h) = q" and q(2h) = qk+d for the first
-
o)hA((l - o)qo
oq
A(q(t) . q(t))-a ql
~
ql - qo
+ oql· - h - )
-
oh/\((l - o)qo
. ql - qo
+ oql· - , 1
Consequently. we obtain the following numerical method for nonholonomic systems
143
D2S~(qk-l·qd + DIS~(ql;·qk+d = ql; - ql; - l ohA((1 - O)qk-J + Oqk" h )
3. OPTL\lAL CONTROL THEORY 3. 1 Geometric formulation of optimal control problems
qk+! - ql; +(1 - o)hA((1 - o)ql; + Oqk+l· h ).
A general optimal control problem consists of a set of differential equations with 1 ::::; k ::::; N -1 and initial condition satisfying
(y
!Cl Q( qo.qIl uqo
ql - qo ) +(1 - Q)hA((1 - o)qo + Oql· - h - ) = 0 .
Example 2.2. Nonholonomic particle.
Consider the Lagrangian L : T~3
--t
.1(c) =
~
;::.:=gy
(12)
The ordinary differential equations (11) on Q depending on the parameters u can be seen as a vector field r along the projection map Tt , that is. r is a smooth map r : U -----+ TQ such that the diagram
r
U ---------------. TQ
I
is commutative. This vector field is locally written ' .'
as
':
.
··so······ iiiO···· ;·5.:.'····· 100
L(q(t). u(t))dt.
In a global description. one assumes a fiber bundle structure Tt : U -----+ Q. where Q is the configuration manifold with local coordinates qi and U is the bundle of controls, with local coordinates (qi.uo) , 1:::; i:::; n , 1 :::; a::::; m.
\ I: '.
:gc
i
t!
to
subject to the constraint (j) = z - yx = O. Taking o = 1/ 2 in (10) we obtain a geometric integrator for the continuous nonholonomic problem. The first figure compares the method introduced here to the traditional Runge-Kutta method of fourth order. showing an improvement in several orders of magnitude. Observe that, in this scale, the value of the energy in each step of our algorithm is practically undistinguishable from the initial value of the energy. - ".-...:""
(11)
where q'i denote the states and u the control variables. and a cost function L(q. u) . Given some boundary conditions (usually qo = q(to)) the aim is to find a C 2 -piecewise smooth curve c(t) = (q(t). u(t)), satisfying the control equations (11) and minimizing the functional
aSh
';;0 (qo.ql.h) = WO(qo . -
= ri(q(t) . u(t)). 1 :::; i :::; n.
2Sc
30c
'
r
.
a
= rl(q .u)"".
uq l
The solutions of such problem are provided by Pontryaguin's maximum principle. If we construct the Hamiltonian function · 3sc··········~··
.. ···· 4ii····
5(IC
H(q.p. u) = L(q. '11) The second figure is a comparison between our method and the one proposed in [Coddar:O 1]. A similar behaviour is obserwd. ='l'evertheless. a slightly better behaviour can also be appreciated. where the proposed algorithm shows on average a better preservation of the original energy.
+ Pir i(q. '11)
(13)
where PI' 1 ::::; i ::::; n. are now considered as Lagrange's multipliers. then a curve I : ~ --t U. -: (t) = (q( t). u( t)) is an optimal trajectory if there exist functions Pi (t). 1 ::::; i ::::; n such that they are solutions of the Hamilton equations: . oH q'(t ) = -;=}(q(t ).p(t).u(t )) UPi (14) { Pi( t) = oH oqi (q(t). p(t).u(t)) and
H(q(t).p(t). '11 (t)) = min H(q(t) . p(t) . i'J.
(15)
I'
with t E [to . t f]. This last condition is usually replaced by oH ou o = O. 1 ::::; (J :::; m (16)
144
when we are looking for extremal trajectories.
of (Po. wo) is equivalent to the regularity of the
It is well known that the Pontryaguin's necessary conditions for extremality have a geometric interpretation in terms of presymplectic hamiltonian system. The total space of the system will be T*Q xQ U . Let '-'-"Q be the canonical symplectic form on T*Q and consider the canonical projection prl : T*Q xQ U ---. TOQ. Denote b~' ,-,-,' = pr~..vQ the induced closed 2-form on TOQ xQ U. The 2-form ..v is degenerate and its characteristic distribution is locally spanned by fJ /au u . 1 :::; a :::; m. Define the Pontryaguin's hamiltonian function H : T*Q xQ U ----. lR as follows H(Q q. u q ) = L(uq)+Qq(r(u q )) where Q q E T;Q and u q E pr-l(q). Obviously. the coordinate expression of H is (13).
matrix (
i/(t) = { Pi(f) =
( 17)
P2
'-t
PI
'-t
Po
'-t
a:l~o (q(t).p(t)) a~A dqi (q(f).p(t))
(20)
0
3.2 Generating functions of the second kind Let (M. 0) be an exact symplectic manifold (0 is symplectic and exact. 0 = -de) and suppose that F : M --> M is a transformation from M to itself and Graph(F) the graph of F , Graph(F) C M x M. Denote by 7ri : M x M --> M , i = 1.2 the canonical projections and the forms :
g:;.
'-t ... '-t
The dynamical equa-
where we have substituted in (14) the control variables UU by its value UU = f"(q.p) applying the implicit function theorem to the primary constraints (/)Q = O. In such case. there exists a unique solution X Po of Equation (19) and its flow preserves the symplectic 2-form wo. i.e. it is a canonical transformation.
the Dirac-Bergmann-Gotay-Nester algorithm to the presymplectic system (T*Q xQ U. ..v. H) we obtain that equations (16) correspond to the primary constraints for the presymplectic system:
.
I
ixpo..vo = dH IPo (19) Taking coordinates (qi. p;) on Po. then the dynamical equations are:
Appl~'ing
. . . '-t
au au
tions for the optimal control problem will be
Equations (1-1) (15) and (16) are intrinsically written as
ix..v = dH
a2H ) u ' b
8 = 7r;e - 7r;e f.l =
7r;0 - 7r;0 = -de
Denote by iF : Graph(F) ' - t M x M the inclusion map. Then. F is a canonical transformation if and only if iFf.l = O. that is, if Graph(F) is a lagrangian submanifold of (M x M , f.l). In such a case, iFf.l = -di F8 = 0 and. at least locally, there exists a function 5 : Graph F --+ lR such that
T*Q x Q U
If this algorithm stabilizes. i.e. there exists a positive integer kEN such that P k = PHI and dim PA- f. O. then we will obtain an stable submanifold PI = P/.:. on which a vector field exists such that
(21) Taking (qi. Pi) as natural coordinates in Graph(F) and (qi. P" q'. Pi) the coordinates in M x ."'1. then. along Graph(F). q' = q'(q.p) and P, = Pi(q.P) and Pi dqi - Pi dqi = dS(q.p) . Suppose that (qi. Pi) are independent local coordinates on Graph(F) (see [Arn:78]): i.e. 5 = S(q. p). Since
(18) The constraints determining PI are known in the control literature as higher order conditions for optimality. Therefore, a necessary condition for optimality of the curve '") : lR ---. u. ,( t) = (q( t). u (t)) \\'ill be the existence of a lift 1 of , to PI such that ~ will be an integral curve of a solution of Equations (18).
Pi dqi - Pi dqi
= _qi dp; + d(qip;)
- Pi dqi = dS.
if we define S2(q. p) = qipi - S(q. p). where P is expressed in terms of p and q. then qi dPi + Pidq' = dS 2(q. p) Definition 3.1. The function S2(q. p) will be called a generating function of the second kind of the canonical transformation F.
In the regular case. the final constraint algorithm is Po (that is. Po = PI) and all the constraints are second class following the classical classification of Dirac. In such case (Po . ..vo) is a symplectic manifold. where ..vO denotes the restriction of the pres~'mplectic 2-form to the constraint submanifold Po . Locall~·. the symplecticity
:'-Jow. suppose that (M. O. H) is a hamiltonian system and X Hits hamiltonian "ector field. sa~' iXHO = dH . Denote by Fh : M --> A1 its flow.
145
where id and f d are adequate approximations to L lPo and r lPo. respectively.
Theorem 3.2. Let a function S2'Vh be defined by N-J
Sfh(qO'PcY) =
L (S~(qk , pk+d -
qk+JPk+d k=O where qk. 1 ::; k ::; N. and Pk. 0::; k ::; N - 1. are stationary points of the right-hand side. that is
Denote by j (qk" Pk+ Il the function hf d(qk.Pk+Il + qk· Then.
-,
S2'(qk,Pk+d
j (qk. Pk+d
-
=
-
= Ld(qk,Pk+d + pk+lf(qk·pk+d
and hence the equations
aS~ qk+l = ap (qk,Pk+d·
Pk
=
as;'
a; (qk·Pk+I).
0 ::; k ::; .1\' - 1
{
0::; k ::; .1\' - 1
N-I -
Proposition 3.3. A generating function of the second kind for Fh is given by Plql
-lh
REFERENCES [Arn:78] Arnold V I 1978 Mathematical Methods of Classical Mechanics (Graduate Text in Mathematics 60, Springer-Verlag New York) [CaLeMa:99] F. Cantrijn, t\I. de Leon, D. l\lartin de Diego: On almost-Poisson structures in nonholonomic mechanics. Nonlinearity 12 (1999),721-737. [CorMar:01] Cortes J and Martinez S 2001 Nonholonomic integrators Nonlinearity 14. 13651392 [HaLu\Va:02] Hairer E , Lubich C and Wanner G 2002 Geometric Numerical Integration , Structure-Preserving Algorithms for Ordinary Differential Equations (Springer Series in Computational ~Iathematics 31. SpringerVerlag Berlin Heidelberg) [Leot\Iar:96] de Leon t\I and t\Iartin de Diego D 1996 On the geometry of non-holonomic Lagrangian systems J. Math. Phys. 37 (7). 3389-3414 [Le:"IaSa:02] de Leon t\J. t\Iartfn de Diego D and Santamarfa A 2002 Geometric integrators and nonholonomic mechanics Preprint IMAFFCSIC [Le\\':86] Lewis F .L 1986 Optimal Control (John Wiley&: Sons. Kew York) [t\lar\\'es:01 ] ~larsden J E and \Vest ~1 2001 Discrete mechanics and variational integrators Acta Numerica • 357-514 [t\larl:95] ~Iarle Ch t\1 1995 Reduction of constrained mechanical s~'stems and stabilit~: of relative equilibria CommuTl . Math. Phys. 174. 295-318 [:'JeiFuf:72] Keimark J and Fufaev ;\ 1972 Dynamics of Nonholonomic Systems (Translations of ~Iathematical t\Ionographs Vo!. 33 Providence: Am. ~lath. Soc.)
(pdq - H dt)
where t ----t (q(t).p(t)) is an int.egral curve of the Hamilton equations such that q(O) = qo and p(h) = PI .
3.3 Generating functions of the second kind and discrete optimal control problems From Proposition 3.3, a generating function of the second kind for the Hamiltonian system (Po. HlPo) ' which determines the dynamics of t.he optimal control problem given by (11) and (12). is
no.
S~(qo. pIl
-1"
= Plql
(p(t)q(t) - HlPo(q(t ).p(t) )) dt
(22)
where t -+ (q(t).p(t)) is an integral curve of the vector field XPo with q(O) = qo and p(h) = PI' \\'e turn now to the construct.ion of a numerical int.egrator for the Hamilt.onian system (Po. wo· H lPo ) by using an approximation of t.he generating function. The proposed methods also realize the integration steps by canonical transformations: therefore. they are s ~!mplectic integrators.
Example 3.4. Consider. for instance. the following approximation to S~ :
-" (qk·Pk+d = Pk'+lqk+J - hpk+l (qk+l-qk) S2 h +hid(qk.Pk+J)
(23)
associate perfomance index: J = Lk=O Ld(qk. ud Observe that this discrete optimal control problem is symplectic in the sense explained in the subsection above.
I
Finally, we have the following
S~(qo.pIl =
=
are exactly the discrete equations corresponding to the classical discrete optimal control problem (see [Lew:86]). determined by the control equations: qk+l = P(qk. Uk). ((qo) given) and with
then S2'" h is a generating function of the second kind for FNh : M -+ M .
Proof: It is similar to that of Theorem 2.1.
aS~ --(qk·Pk+l) aq aS~ qk+l = ap (qk.Pk+I)
Pk
+ hpk+l f d(qk·Pk+l) 146
Copyright i:- IFAC Lagrangian and Hamiltonian Methods for Nonlinear Control, Seville, Spain, 2003
C:OC> Publications www.elsevier.com ·locate:ifac
GEOMETRIC NUMERICAL INTEGRATION OF NONHOLONOMIC SYSTEMS AND OPTIMAL CONTROL PROBLEMS M. de Le6n, * D. Martin de Diego, * A. Santamaria-Merino *.1
*
Instituto de Matematicas y Fisica Fundamental. CSIC. Serrano 123. 28006 Madrid, Spain
Abstract: A geometric derivation of numerical integrators for nonholonomic systems and optimal control problems is obtained. It is based in the classical technique of generating functions adapted to the special features of nonholonomic systems and optimal control problems. Copyright © 20031FAC Keywords: Geometric integrators, nonholonomic systems, optimal control
1. INTRODUCTION
theory by using generating functions of the second kind .
Standard methods for simulating the motion of a dynamical system usually ignore many of the geometric features of this system (simplecticity. conservation laws. symmetries .. . ). However. new methods have been recently developed . called geometric integrators. which are concerned with some of the extra features of geometric nature of the dynamical system (see [HaLuWa:02]).
2.
~O~HOLONOr..IIC
SYSTEr..lS
2.1 Geometrical formulation of nonholonomic systems Let Q be an n-dimensional differentiable manifold. with local coordinates (qi) and tangent bundle TQ. with induced coordinates (qi. (y). Con::;ider a Lagrangian system. with Lagrangian L : TQ -+ R subject to nonholonomic constraints. defined by a submanifold D of the velocity phase space TQ. \Ye will assume that dim D = 2n - m and that D is locally described by the \'anishing of m independent functions oQ (the "constraint functions").
In the first part of the paper. we propose a class of geometric integrators for nonholonomic systems [Leomar:96D.NeiFuf:72] based on a discretization of the Lagrangian function (in a more precise sense. we discretize the action function) and a coherent discretization of the constraint forces (see [Le:\laSa:02]) . These equations will be conceptually equivalent to the proposed for systems with external forces (see [I\lar\Yes:Ol]). Finally. the second part is corcerned with the construction of s~'mplectic integrators for optimal control
satisfying the rank condition rank
(~~~)
= m.
In what follows. we will follow a Hamiltonian point of view. The canonical coordinates on T*Q (the cotangent bundle of Q) are denoted by (q' . Pi). Assume. for simplicity. that the Lagrangian L is h~' perregular. that is. the Legendre transformation Leg: TQ -; T*Q.(qi.i/) ....... (qi ,Pi = aL / ai/). is a global diffeomorphism. The constraint functions on T*Q become IJIIl = Oil 0 Leg-I. i.e.
I This work has been supported by grant BF\12001-2272. :\.. Santamarfa t'llerino wishes to thank the Programa de formaci6n de Im'estigadores of the Departamento de Educaci6n . llniversidades e Investigaci6n of the Basque Government (Spain) for financial support.
1JI"(qi. Pi ) = O"(qi.
141
~H). UPi
where the Hamiltonian
.,
H : T*Q ----+ lR is defined by H = EL 0 Leg-I. Here. EL denotes the energ~: of the system. locally defined by EL = qi L. Since locally . . . aH Leg- I (q' · Pi) = (q"-a ). then H = p;Q' Pi L(qi . qi) . where q; is expressed in terms of qi and Pi by using Leg-I.
q
Zf, -
(1) •
then LXHAI()Q = d(ixHAI()Q - H) - A . or. equivalently. LXHA/()Q = d(L Therefore. integrating we have
F; ()Q
Let 111 denote the image of the constraint submanifold D under the Legendre transformation. and let F be the distribution on T*Q along 111. whose annihilator is given by FO = Leg*(FO)) . Here. FO represents the constraint forces subbundle, 10-
= span{J.1Q
=
ao"
oqi
X 1Al ET1I1.
.
matrix (cab) =
'( ow 0Pi
)
Leg-I) - A
d(1" LoFt dt) - 1" Ft A.
(4)
and nonholonomic
In what follows , we will follow similar arguments for the construction of generating functions for symplectic or canonical maps [Arn:78] . However. because of equation (4). we have that the nonholonomic flow is not a canonical transformation: i.e.,
(2)
This description will allow us to construct a new family of nonholonomic integrators for equations (19). Denote by 7ri : rQ x T*Q ----+ rQ. i = 1. 2. the canonical projections. Consider the following forms
b
Hij OW OPj
=
2.2 "Generating functions" mechanics
dq'} . The
where wQ = -d()Q = dqi Adp, (with ()Q = Pi dqi) is the canonical symplectic form on T*Q. Suppose in addition that the following compatibility condition FJ. n T 111 = {O} holds, where" ..l .. denotes the symplectic orthogonal with respect to wQ . Observe that. locally. this condition means that the a
- ()Q
0
where Ft is the flow of the vector field ~L.D.
Hamiltonian equations of motion of the nonholonomic system can be then rewritten in intrinsic form as (iX'-
(3)
A, .
)l'ow. consider the flow Ft : 111 ~ M. t E I ~ lR generated by the vector field XlI.M. solution of the nonholonomic problem. Since (3) is geometricall~' rewritten as
aw a aoa (-a H)i)(q·P) = (a'; o Leg-I)(q , p). Pj q
_
op, .
for initial conditions (qo. Po) E 111 and A = A, dq'. We also denote by A = Leg*(A) the I-form on TQ ",-ich represents the constraint force once the Lagrange multipliers have been determined.
together with the constraint equations Wa(q.p) = O. Here Hij are the components of the inverse of the matrix (Hil) = (a2H/ apiapj). Note that
cally defined by FO
oH
{ P, - - ~: -
The equations of motion for the nonholonomic s~-stem on T*Q can now be written as follows (see [CaLel\la:99.:'.larl:95] and references therein) ., aH q =ap, aH aw a { p, - - aq' - Aa ap) H ji
=
is regular. The
compatibility condition is not too restrictive. since it is trivially verified by the usual systems of mechanical type (i .e. with a Lagrangian of the form kinetic minus potential energy). where the Hij represent the components of a positive definite Riemannian metric. The compatibility condition guarantees. in particular. the existence of a unique solution of the constrained equations of motion (2) which. henceforth. will be denoted by X H .:H on the Hamiltonian side. and Leg;I(XH ..H) = ~L.D on the Lagrangian side.
e= n=
7r;()Q - 7r;()Q .
7I2""':Q - 7ri""':Q = -de.
Denote by iFh : Graph(F,,) '----t rQ x rQ the inclusion map and observe that Graph(Fh) C ,\1 x 111. Then. from (4). iF" e is equal to
l\Ioreover. if we denote by X H the Hamiltonian function of H. i.e .. iXHWQ = dH then. using the constraint functions. we may explicitely determine the Lagrange multipliers An as An = -CabXH(Wb) . :'-J'ext. writing the I-form A = b DIJI" . -CabXH(W )~H)idq' . then. the nonholonomic PJ equations are equi\"alently rewritten as
Let (qo.Po.ql.pd be coordinates in T*Q x rQ in a neighborhood of some point in Graph(F,,). If (qo.Po.ql.pd E Graph(F,,) then wQ(qo.po) = 0 and w"(ql.pd = O. :\Ioreover. along Graph(Fh). ql = ql(qo·Po). PI = pdqo·Po) and
142
(1" -1"
PI dql - Podqo = d
integral (resp .. second integral) of the right-hand !:iide.
L(q(t). q(t)) dt)
Proof: It suffices to prove the result for 1'.; that is.
(6)
A(q(t).q(t)).
where (q(t). q(t)) = Ft (qo. qo) with Leg(qo· qo) = (qo. Po)· Equation (6) is satisfied along Graph(F,,),
S2h(qo.q2) = Sh(qo.qd
= 2.
+ Sh(ql.q2).
where ql Yerifies the condition (9).
Assume that. in a neighborhood of some point :1' E Graph(F,,). we can change this system of coordinates to a new coordinates (qo. qd. Denote by
Since
dql - Po dqu = dS"(qo. qIl _1h A(q(t). q(t)) .
PI
h
S"(qo. qd
= r L(q(t). q(t)) dt.
Jo
P2dq2-Pldql=dSh(ql.q2)-12hA(q(t).q(t)) .
where q(t) is a solution curve of the nonholonomic problem with q(O) = qo and q(h) = ql and an adequate extension of S". It is easy to show that this solution always exists for adequate values of qo and ql'
then
pz dq2 - Po dqo = d (S" (qo. qd
_1h A(q(t). q(t)) - 12h A(q(t) . q(t)) .
Thus, we deduce that
Po
1" lh - .
as" + = -~ uqo
os" { PI = ~ uql
oq . A(q(t). q(t))!:} uqo
0
oq
Since the variables ql do not appear on the left-hand side term, we obtain expression (9) . ~loreover. for this choice of ql . S2h (qo. q2) = Sh (qo. qd + Sh (ql , q2) is a "generating function of the first kind" of F 2 h . I
(7)
A(q(t). q(t))~ . uql
0
where (qo. ql) verifies the constraint functions <.po (qo. ql . h) = O. explici tely defined by -.pO (qo. ql . h)
llJa(qo.
=
OSh
-~(qo.
u%
qd
+
1" -
Equations (9) determine an implicit system of difference equations which permit us to obtain q2 from the initial data qo and ql'
oq (8) A(q(t). q(t))!:}).
2.3 Nonholonomic integrators
u%
0
where q(t) is a solution of the nonholonomic problem with q(O) = qo and q(h) = q".
In what follows . assume that Q is a vector space. Since Sh(qo. qd = Ioh L(q(t). q(t)) dt , where q(t) is a nonholonomic solution with q(O) = qo and q(h) = ql. we can obtain nonholonomic integrators by taking adequate approximations of the "generating function" Sit and the extra-term
Next. we will show how the group composite law of the flow F". F l\'" = F" 0 . . . 0 F". is expressed in "-v--"
s
terms of the corresponding "generating functions" Sh . ~loreover. the following Theorem "'ill result in a new construction of numerical integrators for nonholonomic mechanics when we change the "generating function" and the constraint forces by appropriate approximations.
Ioh A.(q(t). q(t)) . Consider. for instance. the approximation h
Sa(qo·qd
L
S"(q".qk+d·
q rh A(q(t). q(t)) aa Jo qo
where qk . 1 :S k :S 1\' - 1. are points verifying
" . 1 o
+ DIS"(qk.qk+l)
~ (1 -
=
oq 12h .\(q(t) . oq A(q(t).q(t))!:} + . q(t))!:}. uq\
h
uqo
(10)
A natural approximation of the constraint forces adapted to our choice of approximation for Sh are
.\; - 1
S ·'lih(qO.q.v) =
ql - qo
= hL((l - o)qo + oq\. - h - ) '
for some parameter 0 E [0. 1]. (In general. we will write S~(qo.qd ~ Sh(qo.qd.)
Theorem 2.1. The function SNh. the "generating function" for F.";}I' is giYen b!'
D 2S h (qk _ l.qk)
+ Sh (ql . q2 ) )
l
(9)
h -
o
and q(t) is a solution cun'e of the nonholonomic problem with q(O) = qk-I and q(h) = qk (respectively. q(h) = q" and q(2h) = qk+d for the first
-
o)hA((l - o)qo
oq
A(q(t) . q(t))-a ql
~
ql - qo
+ oql· - h - )
-
oh/\((l - o)qo
. ql - qo
+ oql· - , 1
Consequently. we obtain the following numerical method for nonholonomic systems
143
D2S~(qk-l·qd + DIS~(ql;·qk+d = ql; - ql; - l ohA((1 - O)qk-J + Oqk" h )
3. OPTL\lAL CONTROL THEORY 3. 1 Geometric formulation of optimal control problems
qk+! - ql; +(1 - o)hA((1 - o)ql; + Oqk+l· h ).
A general optimal control problem consists of a set of differential equations with 1 ::::; k ::::; N -1 and initial condition satisfying
(y
!Cl Q( qo.qIl uqo
ql - qo ) +(1 - Q)hA((1 - o)qo + Oql· - h - ) = 0 .
Example 2.2. Nonholonomic particle.
Consider the Lagrangian L : T~3
--t
.1(c) =
~
;::.:=gy
(12)
The ordinary differential equations (11) on Q depending on the parameters u can be seen as a vector field r along the projection map Tt , that is. r is a smooth map r : U -----+ TQ such that the diagram
r
U ---------------. TQ
I
is commutative. This vector field is locally written ' .'
as
':
.
··so······ iiiO···· ;·5.:.'····· 100
L(q(t). u(t))dt.
In a global description. one assumes a fiber bundle structure Tt : U -----+ Q. where Q is the configuration manifold with local coordinates qi and U is the bundle of controls, with local coordinates (qi.uo) , 1:::; i:::; n , 1 :::; a::::; m.
\ I: '.
:gc
i
t!
to
subject to the constraint (j) = z - yx = O. Taking o = 1/ 2 in (10) we obtain a geometric integrator for the continuous nonholonomic problem. The first figure compares the method introduced here to the traditional Runge-Kutta method of fourth order. showing an improvement in several orders of magnitude. Observe that, in this scale, the value of the energy in each step of our algorithm is practically undistinguishable from the initial value of the energy. - ".-...:""
(11)
where q'i denote the states and u the control variables. and a cost function L(q. u) . Given some boundary conditions (usually qo = q(to)) the aim is to find a C 2 -piecewise smooth curve c(t) = (q(t). u(t)), satisfying the control equations (11) and minimizing the functional
aSh
';;0 (qo.ql.h) = WO(qo . -
= ri(q(t) . u(t)). 1 :::; i :::; n.
2Sc
30c
'
r
.
a
= rl(q .u)"".
uq l
The solutions of such problem are provided by Pontryaguin's maximum principle. If we construct the Hamiltonian function · 3sc··········~··
.. ···· 4ii····
5(IC
H(q.p. u) = L(q. '11) The second figure is a comparison between our method and the one proposed in [Coddar:O 1]. A similar behaviour is obserwd. ='l'evertheless. a slightly better behaviour can also be appreciated. where the proposed algorithm shows on average a better preservation of the original energy.
+ Pir i(q. '11)
(13)
where PI' 1 ::::; i ::::; n. are now considered as Lagrange's multipliers. then a curve I : ~ --t U. -: (t) = (q( t). u( t)) is an optimal trajectory if there exist functions Pi (t). 1 ::::; i ::::; n such that they are solutions of the Hamilton equations: . oH q'(t ) = -;=}(q(t ).p(t).u(t )) UPi (14) { Pi( t) = oH oqi (q(t). p(t).u(t)) and
H(q(t).p(t). '11 (t)) = min H(q(t) . p(t) . i'J.
(15)
I'
with t E [to . t f]. This last condition is usually replaced by oH ou o = O. 1 ::::; (J :::; m (16)
144
when we are looking for extremal trajectories.
of (Po. wo) is equivalent to the regularity of the
It is well known that the Pontryaguin's necessary conditions for extremality have a geometric interpretation in terms of presymplectic hamiltonian system. The total space of the system will be T*Q xQ U . Let '-'-"Q be the canonical symplectic form on T*Q and consider the canonical projection prl : T*Q xQ U ---. TOQ. Denote b~' ,-,-,' = pr~..vQ the induced closed 2-form on TOQ xQ U. The 2-form ..v is degenerate and its characteristic distribution is locally spanned by fJ /au u . 1 :::; a :::; m. Define the Pontryaguin's hamiltonian function H : T*Q xQ U ----. lR as follows H(Q q. u q ) = L(uq)+Qq(r(u q )) where Q q E T;Q and u q E pr-l(q). Obviously. the coordinate expression of H is (13).
matrix (
i/(t) = { Pi(f) =
( 17)
P2
'-t
PI
'-t
Po
'-t
a:l~o (q(t).p(t)) a~A dqi (q(f).p(t))
(20)
0
3.2 Generating functions of the second kind Let (M. 0) be an exact symplectic manifold (0 is symplectic and exact. 0 = -de) and suppose that F : M --> M is a transformation from M to itself and Graph(F) the graph of F , Graph(F) C M x M. Denote by 7ri : M x M --> M , i = 1.2 the canonical projections and the forms :
g:;.
'-t ... '-t
The dynamical equa-
where we have substituted in (14) the control variables UU by its value UU = f"(q.p) applying the implicit function theorem to the primary constraints (/)Q = O. In such case. there exists a unique solution X Po of Equation (19) and its flow preserves the symplectic 2-form wo. i.e. it is a canonical transformation.
the Dirac-Bergmann-Gotay-Nester algorithm to the presymplectic system (T*Q xQ U. ..v. H) we obtain that equations (16) correspond to the primary constraints for the presymplectic system:
.
I
ixpo..vo = dH IPo (19) Taking coordinates (qi. p;) on Po. then the dynamical equations are:
Appl~'ing
. . . '-t
au au
tions for the optimal control problem will be
Equations (1-1) (15) and (16) are intrinsically written as
ix..v = dH
a2H ) u ' b
8 = 7r;e - 7r;e f.l =
7r;0 - 7r;0 = -de
Denote by iF : Graph(F) ' - t M x M the inclusion map. Then. F is a canonical transformation if and only if iFf.l = O. that is, if Graph(F) is a lagrangian submanifold of (M x M , f.l). In such a case, iFf.l = -di F8 = 0 and. at least locally, there exists a function 5 : Graph F --+ lR such that
T*Q x Q U
If this algorithm stabilizes. i.e. there exists a positive integer kEN such that P k = PHI and dim PA- f. O. then we will obtain an stable submanifold PI = P/.:. on which a vector field exists such that
(21) Taking (qi. Pi) as natural coordinates in Graph(F) and (qi. P" q'. Pi) the coordinates in M x ."'1. then. along Graph(F). q' = q'(q.p) and P, = Pi(q.P) and Pi dqi - Pi dqi = dS(q.p) . Suppose that (qi. Pi) are independent local coordinates on Graph(F) (see [Arn:78]): i.e. 5 = S(q. p). Since
(18) The constraints determining PI are known in the control literature as higher order conditions for optimality. Therefore, a necessary condition for optimality of the curve '") : lR ---. u. ,( t) = (q( t). u (t)) \\'ill be the existence of a lift 1 of , to PI such that ~ will be an integral curve of a solution of Equations (18).
Pi dqi - Pi dqi
= _qi dp; + d(qip;)
- Pi dqi = dS.
if we define S2(q. p) = qipi - S(q. p). where P is expressed in terms of p and q. then qi dPi + Pidq' = dS 2(q. p) Definition 3.1. The function S2(q. p) will be called a generating function of the second kind of the canonical transformation F.
In the regular case. the final constraint algorithm is Po (that is. Po = PI) and all the constraints are second class following the classical classification of Dirac. In such case (Po . ..vo) is a symplectic manifold. where ..vO denotes the restriction of the pres~'mplectic 2-form to the constraint submanifold Po . Locall~·. the symplecticity
:'-Jow. suppose that (M. O. H) is a hamiltonian system and X Hits hamiltonian "ector field. sa~' iXHO = dH . Denote by Fh : M --> A1 its flow.
145
where id and f d are adequate approximations to L lPo and r lPo. respectively.
Theorem 3.2. Let a function S2'Vh be defined by N-J
Sfh(qO'PcY) =
L (S~(qk , pk+d -
qk+JPk+d k=O where qk. 1 ::; k ::; N. and Pk. 0::; k ::; N - 1. are stationary points of the right-hand side. that is
Denote by j (qk" Pk+ Il the function hf d(qk.Pk+Il + qk· Then.
-,
S2'(qk,Pk+d
j (qk. Pk+d
-
=
-
= Ld(qk,Pk+d + pk+lf(qk·pk+d
and hence the equations
aS~ qk+l = ap (qk,Pk+d·
Pk
=
as;'
a; (qk·Pk+I).
0 ::; k ::; .1\' - 1
{
0::; k ::; .1\' - 1
N-I -
Proposition 3.3. A generating function of the second kind for Fh is given by Plql
-lh
REFERENCES [Arn:78] Arnold V I 1978 Mathematical Methods of Classical Mechanics (Graduate Text in Mathematics 60, Springer-Verlag New York) [CaLeMa:99] F. Cantrijn, t\I. de Leon, D. l\lartin de Diego: On almost-Poisson structures in nonholonomic mechanics. Nonlinearity 12 (1999),721-737. [CorMar:01] Cortes J and Martinez S 2001 Nonholonomic integrators Nonlinearity 14. 13651392 [HaLu\Va:02] Hairer E , Lubich C and Wanner G 2002 Geometric Numerical Integration , Structure-Preserving Algorithms for Ordinary Differential Equations (Springer Series in Computational ~Iathematics 31. SpringerVerlag Berlin Heidelberg) [Leot\Iar:96] de Leon t\I and t\Iartin de Diego D 1996 On the geometry of non-holonomic Lagrangian systems J. Math. Phys. 37 (7). 3389-3414 [Le:"IaSa:02] de Leon t\J. t\Iartfn de Diego D and Santamarfa A 2002 Geometric integrators and nonholonomic mechanics Preprint IMAFFCSIC [Le\\':86] Lewis F .L 1986 Optimal Control (John Wiley&: Sons. Kew York) [t\lar\\'es:01 ] ~larsden J E and \Vest ~1 2001 Discrete mechanics and variational integrators Acta Numerica • 357-514 [t\larl:95] ~Iarle Ch t\1 1995 Reduction of constrained mechanical s~'stems and stabilit~: of relative equilibria CommuTl . Math. Phys. 174. 295-318 [:'JeiFuf:72] Keimark J and Fufaev ;\ 1972 Dynamics of Nonholonomic Systems (Translations of ~Iathematical t\Ionographs Vo!. 33 Providence: Am. ~lath. Soc.)
(pdq - H dt)
where t ----t (q(t).p(t)) is an int.egral curve of the Hamilton equations such that q(O) = qo and p(h) = PI .
3.3 Generating functions of the second kind and discrete optimal control problems From Proposition 3.3, a generating function of the second kind for the Hamiltonian system (Po. HlPo) ' which determines the dynamics of t.he optimal control problem given by (11) and (12). is
no.
S~(qo. pIl
-1"
= Plql
(p(t)q(t) - HlPo(q(t ).p(t) )) dt
(22)
where t -+ (q(t).p(t)) is an integral curve of the vector field XPo with q(O) = qo and p(h) = PI' \\'e turn now to the construct.ion of a numerical int.egrator for the Hamilt.onian system (Po. wo· H lPo ) by using an approximation of t.he generating function. The proposed methods also realize the integration steps by canonical transformations: therefore. they are s ~!mplectic integrators.
Example 3.4. Consider. for instance. the following approximation to S~ :
-" (qk·Pk+d = Pk'+lqk+J - hpk+l (qk+l-qk) S2 h +hid(qk.Pk+J)
(23)
associate perfomance index: J = Lk=O Ld(qk. ud Observe that this discrete optimal control problem is symplectic in the sense explained in the subsection above.
I
Finally, we have the following
S~(qo.pIl =
=
are exactly the discrete equations corresponding to the classical discrete optimal control problem (see [Lew:86]). determined by the control equations: qk+l = P(qk. Uk). ((qo) given) and with
then S2'" h is a generating function of the second kind for FNh : M -+ M .
Proof: It is similar to that of Theorem 2.1.
aS~ --(qk·Pk+l) aq aS~ qk+l = ap (qk.Pk+I)
Pk
+ hpk+l f d(qk·Pk+l) 146