Variational Integration in Optimal Control

Kathrin Flaßkamp

Sep 6, 2016

GAMM Juniors’ School on Applied Mathematics and Mechanics

1 / 48

Outline for today’s lectures

Motivation for optimal control

Quick review of yesterday’s lecture

Optimal control for mechanical systems (continuous time)

Optimal control for discrete mechanical systemsError AnalysisImplementation of direct method

2 / 48

Motivation

Optimal Control for Mechanical Systems

Space Mission Design Robotics Biomechanics ...

Optimization of the systems’ dynamical behavior by findingoptimal control functions

min. control effort min. time max. comfortable max. safety ...

3 / 48

Quick review of yesterday’s lectureI Lagrangian L : TQ → RI Lagrange-d’Alembert principle:

δ

∫ T

0

L(q, q) dt︸ ︷︷ ︸action

+

∫ T

0

f (q, q, u) · δq dt︸ ︷︷ ︸virtual work

= 0

q(0) q(T)

curve q(t)

δq(t)

Q

I Euler-Lagrange eqs. with forces

∂L

∂q− d

dt

∂L

∂q+ f (q, q, u) = 0

I discrete action & forces

Sd =N−1∑k=0

Ld (qk , qk+1)︸ ︷︷ ︸discrete Lagrangian

Wd =N−1∑k=0

f −k · δqk + f +k · δqk+1

q(0) q(T)

qk

δqk

Q

variedpoint

I discrete E-L eqs. with forces

D1Ld (qk , qk+1) + f −k (· · ·, uk )

+ D2Ld (qk−1, qk ) + f +k−1(· · ·, uk ) = 0

I Simulation (variational Integrator)4 / 48

Quick review of yesterday’s lectureI Euler-Lagrange eqs.

∂L

∂q− d

dt

∂L

∂q= 0

I Hamilton eqs.

q =∂H

∂p, p = −∂H

∂q

I flow of Hamiltonian/Lagrangiansystems is symplectic, i.e. areapreserving

I discrete E-L eqs.

D1Ld(qk , qk+1) + D2Ld(qk−1, qk) = 0

I discrete Hamilton eqs.

pk = −D1Ld(qk , qk+1)

pk+1 = D2Ld(qk , qk+1)

I aim: preserve symplecticity innumerical integration → VI aresymplectic integrators

Graphic from Hairer,Lubich, Wanner 2006

5 / 48

Optimal control for mechanical systems (continuous time)

Optimal control problem (OCP):

minq∈C 2([0,T],Q),u∈C([0,T],U)

J(q, u)

=

∫ T

0C (q(t), q(t), u(t)) dt + Φ(q(T), q(T))

s.t. δ

∫ T

0L(q(t), q(t)) dt +

∫ T

0fL(q(t), q(t), u(t)) · δq(t) dt = 0,

(q(0), q(0)) = (q0, q0),

h(q(t), q(t), u(t)) ≥ 0, t ∈ [0,T],

r(q(T), q(T), qT, qT) = 0.

6 / 48

Notation

I mechanical system described by L as before

I force fL depending on control parameter u

I time interval [0,T ]

I initial state (q(0), q(0)) = (q0, q0) ∈ TQ

I objective functional / cost functional

J(q, u) =

∫ T

0C (q(t), q(t), u(t)) dt + Φ(q(T), q(T))

C : TQ × U → R, Φ : TQ → R continuously differentiable.

I constraint r(q(T), q(T), qT, qT) = 0 with desired final state(qT, qT) ∈ TQ

I constraint h(q(t), q(t), u(t)) ≥ 0

7 / 48

Feasible & optimal solutions: “as optimization in R”

I A curve (q, u) ∈ C 2([0,T],Q)× C ([0,T],U) is feasible, if itfulfills the constraints of OCP.

I A feasible curve (q∗, u∗) is an optimal solution of OCP, ifJ(q∗, u∗) ≤ J(q, u) for all feasible curves (q, u).

I A feasible curve (q∗, u∗) is a local optimal solution, ifJ(q∗, u∗) ≤ J(q, u) in a neighborhood of (q∗, u∗).

The function q∗ is called (locally) optimal trajectory, and u∗

is the (locally) optimal control.

8 / 48

How does the OCP for mechanical systems fit into a more generalproblem formulation?

⇒ rewrite Euler-Lagrange equations (2nd order ODE innon-standard form) as Hamilton equations (set of 1st order ODEin standard form):

q =∂H(q, p)

∂p

p = −∂H(q, p)

∂q+ fH(q, p, u)

⇒ x = f (x , u)

with x = (q, p) and f (x , u) =(∂H(q,p)∂p ,−∂H(q,p)

∂q + fH(q, p, u))T

9 / 48

Generalize OCP:but assume:

I no state and control constraints (U = Rm)

I fixed final time

minx∈C 1([0,T],TQ),u∈C([0,T],U)

J(x , u) =

∫ T

0C (x(t), u(t)) dt + Φ(x(T))

subject to x(t) = f (x(t), u(t))

x(0) = x0

0nψ = ψ(x(T ))

Mechanical structure hidden for now, but we’ll get back to it later!

10 / 48

Necessary Optimality Conditions

Theorem (Pontryagin Maximum Principle (PMP))

Let (x∗, u∗) be an optimal solution to (OCP). Define the optimalcontrol Hamiltonian as

H(x , u, λ) := −C (x , u) + 〈λ, f (x , u)〉.

Then there exists a function λ : [0,T ]→ Rn and a vector ρ ∈ Rnψ

such that

H(x∗(t), u∗(t), λ(t)) = maxu∈UH(x(t), u, λ(t)) t ∈ [0,T],

and λ solves the following initial value problem:

λ(T) = −∇Φ(x(T ))−(∂ψ

∂xT

)T

ρ, λ = −∂H∂x

(x∗, u∗, λ).

11 / 48

Proof is involved (too long to show here), but there is a heuristicderivation via the calculus of variations that we’re going toreproduce. Therefore:

I recall definition of a variation of a C 2 curve c : [0,T ]→ Q:σ : [0,T ]× [−ε0, ε0]→ Q with

(i) σ(t, 0) = c(t),(ii) σ(t0, ε) = c(t0) und(iii) σ(tf , ε) = c(tf ).

I remind yourself of howconstrained optimization in Rworks: → ∃ Lagrange multiplierλ, such that an optimal pointsatisfies∇f (x∗, y∗) + λ · ∇g(x∗, y∗) = 0

12 / 48

Back to our control problem:

minx,u

J(x , u) =

∫ T

0

C (x , u) dt + Φ(x(T))

subject to x(t) = f (x(t), u(t))

x(0) = x0, 0nψ = ψ(x(T ))

Define an augmented cost functional

L(x , u, λ, ρ) =

∫ T

0C (x(t), u(t)) + 〈λ(t), x − f (x(t), u(t))〉 dt

+ Φ(x(T)) + 〈ρ, ψ(x(T ))〉,

where λ ∈ C 1([0,T],Rnx ) is the adjoint variable/ costate andρ ∈ Rnψ .

13 / 48

By taking independent variations δx , δu, δλ, δρ of

L(x , u, λ, ρ) =

∫ T

0

C (x(t), u(t)) + 〈λ(t), x − f (x(t), u(t))〉 dt

+ Φ(x(T)) + 〈ρ, ψ(x(T ))〉,

we obtain

δL =

∫ T

0

⟨∂C

∂x, δx

⟩+

⟨∂C

∂u, δu

⟩+ 〈δλ, x − f (x , u)〉 − 〈λ, δx〉

−

⟨(∂f

∂x

)T

λ, δx

⟩−

⟨(∂f

∂u

)T

λ, δu

⟩dt + 〈λ, δx〉|T0

+ 〈∇Φ, δx(T )〉+

⟨(∂ψ

∂xT

)T

ρ, δx(T )

⟩+ 〈δρ, ψ(x(T ))〉 = 0

Recall δx(0) = 0. We used integration by parts to transform∫ T0 〈λ, δx〉dt =

∫ T0 〈λ,

ddt δx〉dt = 〈λ, δx〉|T0 −

∫ T0 〈λ, δx〉.

14 / 48

An optimal solution extremizes L (δL = 0) for arbitraryindependent variations. This leads to the conditions

δλ : x = f (x , u)

δx : λ =∂C

∂x−(∂f

∂x

)T

λ

δu : 0 =∂C

∂u−(∂f

∂u

)T

λ

δx(T ) : λ(T ) = −∇Φ(x(T ))−(∂ψ

∂xT

)T

ρ

δρ : 0 = ψ(x(0), x(T ))

15 / 48

With the optimal control Hamiltonian

H(x , u, λ) := −C (x , u) + 〈λ, f (x , u)〉

the necessary optimality conditions can be rewritten as

x =∂H∂λ

, λ = −∂H∂x

, 0 =∂H∂u

.

Recall now that the system is a mechanical system, i.e. x = (q, p),

q =∂H(q, p)

∂p, p = −∂H(q, p)

∂q+ fH(q, p, u)

⇒ Hamiltonian structure at two levels: Hamiltonian H of themechanics and optimal control Hamiltonian H (and also twoLagrangians)

⇒ Variational integration comes to mind to preserve thisstructure when numerically solving OPCs for mech. systems.

16 / 48

Solution approaches for OCPs

OCP

↙ ↘

Indirect approach

“First optimize than discretize”

1. Pontryagin MaximumPrinciple

test

⇒ Two point boundary valueproblem

2. solve numerically e.g. byshooting or collocationmethods

Direct approach

“First discretize than optimize”

1. apply time-discretization toOCP (infinite dimensionalproblem)

⇒ constrained optimizationproblem (finite dimensional)

2. solve numerically by e.g.Sequential QuadraticProgramming (sqp)

17 / 48

Summary

I optimal control problems consist ofI objective functional that depend on trajectory and control

curvesI constraints from the system dynamics, given as Euler-Lagrange

equations or Hamilton equationsI further constraints on initial & final point, control values, state

path,...

I Pontryagin Maximum Principle gives necessary optimalityconditions→ interesting for theoretical considerations, indirect numericalsolution methods and error analysis→ PMP gives Hamilton equations in state and adjointvariables

I direct solution methods start with a time-discretization ofOCP → numerical integration schemes needed. Since weconsider mechanical systems → variational integrators

18 / 48

Optimal control for discrete mechanical systems

D I S C R E T I Z A T I O N

1. configuration and control curves

I replace TQ by Q × QI q : [0,T]→ Q by qd : {0, h, 2h, . . . ,Nh = T} → Q with

qk = qd (kh)I and u : [0,T]→ U by ud : ∆t → U (writing

uk = (ud (kh + c`h))s`=1 ∈ Us)

2. equations of motion

I replace Lagrange-d’Alembert principle by the discrete principleor (equivalently)

I by discrete forced Euler-Lagrange (DEL) equations

D2Ld (qk−1, qk ) + D1Ld (qk , qk+1)

+ f +d (qk−1, qk , uk−1) + f −

d (qk , qk+1, uk ) = 0, k = 1, . . . ,N − 1

19 / 48

3. boundary conditionsI on configuration level:

straightforward q(0) = q0 and q(T ) = qT transformed toq0 = q0 and qN = qT

I on velocity level:

I transform q(0) = q0 and q(T ) = qT to conditions onmomenta p(0) = p0 and p(T ) = pT with p = ∂L

∂q

I discrete momenta by discrete Legendre transforms Ff −Ld andFf +Ld

I boundary constraints

D2L(q0, q0)︸ ︷︷ ︸

p0

+D1Ld(q0, q1) + f −d (q0, q1, u0)︸ ︷︷ ︸−p0

= 0

−D2L(qT, qT)︸ ︷︷ ︸

−pT

+D2Ld(qN−1, qN) + f +d (qN−1, qN , uN−1)︸ ︷︷ ︸

pN

= 0

I similarly for more general boundary constraintr(q(T ), q(T ), qT qT ) = 0 rd(qN−1, qN , uN−1, q

T, qT) = 0

20 / 48

4. path constraintsapproximate on each time interval [tk , tk+1] = [kh, (k + 1)h](rather than at each time node) → formulation on velocitylevelreplace h(q(t), q(t), u(t)) ≥ 0 by hd : Q × Q × Us → Rsnh

with hd (qk , qk+1, uk ) ≥ 0, k = 0, . . . ,N − 1

5. discrete objective functionapproximate objective functional on [kh, (k + 1)h] by

Cd (qk , qk+1, uk ) ≈∫ (k+1)h

khC (q(t), q(t), u(t)) dt

and final condition via Φd : Q × Q × Us → R such that

Jd (qd , ud ) =N−1∑k=0

Cd (qk , qk+1, uk ) + Φd (qN−1, qN , uN−1)

21 / 48

Discrete Lagrangian optimal control problem

min(qd ,ud )∈Cd (Q)×Cd (U)

Jd (qd , ud ) =N−1∑k=0

Cd (qk , qk+1, uk )

+ Φd (qN−1, qN , uN−1)

subject toq0 = q0,

D2L(q0, q0) + D1Ld (q0, q1) + f −0 = 0,

D2Ld (qk−1, qk ) + D1Ld (qk , qk+1) + f +k−1 + f −k = 0, k = 1, . . . ,N − 1,

hd (qk , qk+1, uk ) ≥ 0, k = 0, . . . ,N − 1,

rd (qN−1, qN , uN−1, qT, qT) = 0.

(Recall that the f ±k are dependent on uk ∈ Us).

This is a constrained nonlinear optimization problem (nonlinearprogramming problem).

22 / 48

Nonlinear programming problem (NLP)

Denote the set of optimization parameters by ξ = (qd , ud ) andwrite the NLP as

minξϕ(ξ), subject to a(ξ) = 0, b(ξ) ≥ 0,

where ϕ : Rn → R, a : Rn → Rm, and b : Rn → Rp arecontinuously differentiable.

I ξ ∈ Rn is feasible if a(ξ) = 0 and b(ξ) ≥ 0

I ξ∗ feasible is a local minimum if ϕ(ξ∗) ≤ ϕ(ξ) for all feasibleξ in a neighborhood of ξ∗.

I active inequality constraints bact(ξ) at a feasible ξ arecomponents of b(ξ) with bj (ξ) = 0.

I active set at ξ is a(ξ) := (a(ξ), bact(ξ))

I ξ is a regular point, if the Jacobian of the active constraints,∇a(ξ)T , has full rank

23 / 48

Necessary optimality conditions

NLP:minξϕ(ξ), subject to a(ξ) = 0, b(ξ) ≥ 0.

We restrict to regular points for brevity.

Define the Lagrangian

L(ξ, λ, µ) := ϕ(ξ)− λT a(ξ)− µT b(ξ).

with Lagrange multipliers (also called adjoints) λ ∈ Rm andµ ∈ Rp.

24 / 48

Necessary optimality conditions

Theorem (Karush-Kuhn-Tucker conditions (KKT))

If a regular point ξ∗ ∈ Rn is a local optimum of the NLP problem,then there exist unique Lagrange multiplier vectors λ∗ ∈ Rm andµ∗ ∈ Rp such that the triple (ξ∗, λ∗, µ∗) satisfies the followingnecessary conditions:

∇ξL(ξ∗, λ∗, µ∗) = 0, a(ξ∗) = 0, b(ξ∗) ≥ 0,

µ∗ ≥ 0, µ∗j bj (ξ∗) = 0, j = 1, . . . , p.

A triple (ξ∗, λ∗, µ∗) that satisfies these conditions is called aKarush-Kuhn-Tucker point (KKT point).

Karush 1939, Kuhn and Tucker 1951

25 / 48

Summary

OCP

Pontryagin maxprinciple

necessary optimalityconditions

(symplectic)discretization

discretized OCP (NLP)

necessary optimalityconditions

Karush-Kuhn-Tucker?

26 / 48

Error analysis

I compare continuous formulation of OCP and discreteformulation

I to this aim, use necessary optimality conditions

I have to take discretization scheme for adjoints into account

P L A N

I error analysis for standard discretization schemes

I benefits of variational discretization

27 / 48

Recall the necessary optimality conditions in continuous-time (fromPontryagin max principle):

x =∂H∂λ

(x , u, λ), ψ(x(0), x(T )) = 0

λ = −∂H∂x

(x , u, λ), λ(T ) = −∇Φ(x(T ))−(∂ψ

∂xT

)T

ρ

0 =∂H∂u

(x , u, λ)

Assumption: ∂uuH(x , λ, u) is invertible, such that we can writeu(t) = g(x(t), λ(t)).⇒

x =∂H∂λ

(x , g(x , λ), λ) = k(x , λ)

λ = −∂H∂x

(x , g(x , λ), λ) = l(x , λ)

28 / 48

Example

minx ,u

J(u) =

∫ T

0

1

2u(t)2 dt

subject to x(t) = f (x(t)) + u(t) for t ∈ [0,T ], x(0) = x0

The OCP Hamiltonian is

H(x , λ, u) = −1

2u(t)2 + λT · (f (x(t)) + u(t)).

From Pontryagin max principle we get

0 =∂H∂u

⇒ u(t) = λ(t)

x(t) =∂H∂λ

⇒ x(t) = f (x(t)) + λ(t), x(0) = x0

λ(t) = −∂H∂x

⇒ λ(t) = −λ(t)T∇f (x(t)), λ(T ) = 0

which is a two point boundary value problem for (x , λ).29 / 48

Example (cont’)Discretization by explicit Euler integrator:

minxd ,ud

Jd (ud ) =N−1∑k=0

h1

2u2

k

subject to

xk+1 = xk + h(f (xk ) + uk ), k = 0, . . . ,N − 1 x0 = x0.

The problem Lagrangian is

L(xd , ud , λd ) =N−1∑k=0

h1

2u2

k +λTk+1(xk+1−xk−hf (xk )−huk )+λT

0 (x0−x0)

Karush-Kuhn-Tucker conditions give

0 =∂L∂uk

⇒ uk = λk+1, k = 1, . . . ,N − 1

0 =∂L∂xk

⇒ λk+1 = λk − hλTk+1∇f (xk ), k = 0, . . .N − 1, λN = 0,

30 / 48

λk+1 = λk − hλTk+1∇f (xk ), k = 0, . . .N − 1, λN = 0 is an

implicit Euler scheme in forward time for the adjoint λ.

Thus, altogether we have:

x(t) = f (x(t)) + λ(t), x(0) = x0

λ(t) = −λ(t)T∇f (x(t)), λ(T ) = 0

−−−−−−−→

direct discretization

−−−−−−−→

xk+1 = xk + h(f (xk ) + λk+1), x0 = x0

λk+1 = λk − hλTk+1∇f (xk ) λN = 0

which is the symplectic Euler scheme (explicit in x , implicit in λ).

31 / 48

Theorem (Hager 1976, 2000)

By using a Runge-Kutta scheme for the state discretization of anoptimal control problem, one obtains a symplectic partionedRunge-Kutta scheme for the optimality system.

Generalization of our example

I explicit Euler → Runge-Kutta scheme

I symplectic Euler → symplectic partitioned Runge-Kuttascheme, i.e. two RK schemes for state and adjoints withcoefficients such that map is symplectic

Attention

I In general, order of state discretization 6= order of discreteoptimality system. Typically, ≤ and < for integration orderthree and up. (See work of Hager or Bonnans andLaurent-Varin 2004.)

32 / 48

Error analysis in optimal control

OCP

Pontryagin maxprinciple

necessary optimalityconditions

discretization

discretized OCP (NLP)

necessary optimalityconditions

Karush-Kuhn-Tucker

symplectic scheme forstate and adjoint

33 / 48

This result is not fully satisfactory for us, since we considermechanical systems that can be described variationally. To preservetheir structures (symplecticity, momenta, “energy”,...), we wouldlike to discretize the state equations by a variational integrator!

34 / 48

Another Example

I regular Lagrangian L(q, q) = 12 qT Mq − V (q) and control

force f = u(t)

I Euler-Lagrange equations

M ¨q(t) = −∇V (q(t)) + u(t)

I Hamilton equations

q = M−1p, p = −∇V (q(t)) + u(t)

I objective

J =

∫ T

0

1

2u(t)2 dt

and initial conditions (q(0), p(0)) = (q0, p0)

35 / 48

Example (cont’)

The Hamiltonian is

H(q, p, λ1, λ2, u) = −1

2u(t)2+λT

1 ·M−1p+λT2 ·(−∇V (q(t))+u(t))

So, the Pontryagin max principle gives

0 =∂H∂u

⇒ u(t) = λ2(t)

q(t) =∂H∂λ1

⇒ q(t) = M−1p, q(0) = q0

p(t) =∂H∂λ2

⇒ p(t) = −∇V (q(t)) + λ2(t), p(0) = p0

λ1(t) = −∂H∂q

⇒ λ1(t) = λ2(t)T∇2V (q(t)), λ1(T ) = 0

λ2(t) = −∂H∂p

⇒ λ2(t) = λ1(t)TM−1, λ2(T ) = 0

a boundary value problem for (q, p, λ1, λ2)

36 / 48

Symplectic Discretization (here we choose a symplectic Eulerscheme)

minqd ,pd ,ud

Jd (ud ) =N−1∑k=0

h1

2u2

k

subject to

qk+1 = qk + h(M−1pk+1),

pk+1 = pk + h(−∇V (qk ) + uk ) k = 0, . . . ,N − 1 (q0, p0) = (q0, p0)

which are the equality constraints a(qd , pd , ud ) = 0

37 / 48

The Lagrangian is

L(qd , pd , ud , λ1,d , λ2,d )

=N−1∑k=0

h1

2u2

k + λT1,k+1(qk+1 − qk − hM−1pk+1) + λT

1,0(q0 − q0)

+ λT2,k+1(pk+1 − pk − h(−∇V (qk ) + uk )) + λT

2,0(p0 − p0)

So the Karush-Kuhn-Tucker equations read

0 =∂L∂uk

⇒ uk = λ2,k+1, k = 1, . . . ,N − 1

0 =∂L∂qk

⇒ λ1,k+1 = λ1,k + hλT2,k+1∇2V (qk), k = 0, . . .N − 1, λ1,N = 0,

0 =∂L∂qk

⇒ λ2,k+1 = λ2,k + hλT1,kM

−1, k = 0, . . .N − 1, λ2,N = 0

⇒ a symplectic Euler scheme for the adjoint system (λ1, λ2)(explicit in λ1, implicit in λ2)

38 / 48

Theorem (Ober-Blobaum 2008)

If the state scheme for the discretization of the Lagrangian systemis a symplectic Runge-Kutta scheme, we obtain the samesymplectic Runge-Kutta scheme for the adjoint equation. Thus,the discrete optimality system is again a symplectic Runge-Kuttascheme of the same approximation order as the state scheme.

39 / 48

Error analysis in structure-preserving optimal control

OCP

Pontryagin maxprinciple

necessary optimalityconditions

symplecticdiscretization

discretized OCP (NLP)

necessary optimalityconditions

Karush-Kuhn-Tuckerthe same symplectic

schemes for state andadjoint

40 / 48

Summary

Solving an OCP

I using a direct method (“first discretize than optimize”)

I discretizing the state equation via a symplectic scheme (=discrete variational approach)

I solve the resulting NLP

I “Theory behind the scenes”: discrete necessary optimalityconditions guaranteed to be of same order

Not talked about Backwards Error Analysis

I probably the most important approach for VI, since a VIintegrator ensures that a nearby Hamiltonian system (modifiedHamiltonian close to original Hamiltonian) is exactly sampled

I impact of BEA for optimal control still an open problem(some “discouraging” results by Vilmart 2008)

41 / 48

Implementation of direct solution approach

I discretize OCP, i.e. cost function, dynamics and constraints→ discrete Lagrangian optimal control problem

min(qd ,ud )∈Cd (Q)×Cd (U)

Jd (qd , ud ) =

N−1∑k=0

Cd (qk , qk+1, uk )

+ Φd (qN−1, qN , uN−1)

subject to q0 = q0,

D2L(q0, q0) + D1Ld (q0, q1) + f −0 = 0,

D2Ld (qk−1, qk ) + D1Ld (qk , qk+1) + f +k−1 + f −k = 0, k = 1, . . . ,N − 1,

hd (qk , qk+1, uk ) ≥ 0, k = 0, . . . ,N − 1,

rd (qN−1, qN , uN−1, qT, qT) = 0.

I rewrite as nonlinear, constrained optimization problem

minz

F (z) s.t. z ∈ S , G (z) ≤ 0, H(z) = 0

42 / 48

Implementation

Full discretization vs. reduced discretization

I We define z = (qd , ud ) persueing a full discretizationapproach.

I Alternatively, qd as dependent variables that can bedetermined from discrete EL-eq., so z = (q0, ud ) → feweroptimization variables

I Advantage of full discretization approach: sparsity of Jacobianmatrices F ′(z), G ′(z), H ′(z)

43 / 48

Sequential quadratic programming

NLP: minz

F (z) s.t. z ∈ S , G (z) ≤ 0, H(z) = 0

I iterative procedure z [k+1] = z [k] + αk d [k]

I find search direction d [k] by solving a quadratic optimizationproblem in z [k]

Quadratic Problem: mind∈Rn

1

2dT Bk d + F ′(z [k])d

s.t. G (z [k]) + G ′(z [k])d ≤ 0,

H(z [k]) + H ′(z [k])d = 0

z ≤ z [k] + d ≤ z

with Bk suitable update matrix (approximates the Hessian, e.g.BFGS matrices)

44 / 48

SQP

Lagrangian of NLP: L(z , l0, µ, λ) = l0F (z) + µT G (z) + λT H(z)

(Local) SQP Algorithm

1. initial value (z [0], µ[0], λ[0]) ∈ Rn × Rm × Rp, k = 0.

2. If (z [k], µ[k], λ[k]) Karush-Kuhn-Tucker point, STOP.

3. Find a KKT point (d [k], µ[k+1], λ[k+1]) of the quadraticproblem.

4. Set z [k+1] = z [k] + d [k], k := k + 1 return to 2.

I globalizable by step-size strategies

I “THE” state-of-the-art solution method for large-scale NLPs,goes back to Han, Powell, Gill et al., ... (late 70ies, 80ies),nowadays implemented in many software tools

45 / 48

Further details for implementation

I derivatives of cost-function and constraints for SQP algorithmI numerical approximation by finite differencesI provide derivative functions analyticallyI by automatic/algorithmic differentiation

I box constraints: typical type of (simple) constraints:uk ∈ [umin, umax], qk ∈ [qmin, qmax]

I free final time: use step-size h as an additional degree offreedom in optimization problem

46 / 48

Software

I Ober-Blobaum et al.: DMOC – Discrete Mechanics andOptimal Controldirect method w/ discrete EL eqs. + SQP-solver from

I NAG (numerical algorithms group) libraryI IPOPTI Matlab: fmincon

+ algorithmic differentiation with ADOL-CI Murphey et al.: Trep: Mechanical Simulation and Optimal

Control SoftwareI Banach space method + discretization by discrete

Euler-Lagrange eqs.I Phyton module, download from GitHub or ROS repositories

I ...

I your implementation ;-) e.g. using Octave’s SQP method

47 / 48

Thanks & ReferencesI S. Ober-Blobaum: Lecture Notes on Discrete mechanics and optimal control

theory, 7th International Young Researchers Workshop on Geometry, Mechanics,and Control, 2012

I T. Murphey: Lecture Notes on Trajectory Optimization In Discrete Mechanics,CISM 2013: Differential-Geometric Methods in Computational MultibodySystem Dynamics, 2013

I J. E. Marsden and M. West. Discrete mechanics and variational integrators.Acta Numerica, 10:357–514, 2001Variational integrators

I E. Hairer, C. Lubich, and G. Wanner: Geometric Numerical Integration:Structure-Preserving Algorithms for Ordinary Differential Equations, 2006Variational integrators, Symplectic integrators, Backwards Error Analysis

I S. Ober-Blobaum, O. Junge, and J. E. Marsden. Discrete mechanics andoptimal control: an analysis. Control, Optimisation and Calculus of Variations,17(2):322–352, 2011Variational integrators for direct discretization in optimal control, error analysis

I D. Liberzon. Calculus of Variations and Optimal Control Theory. PrincetonUniversity Press, 2012.Proof of Pontryagin max principle

I M. Gerdts: Lecture Notes on Optimale Steuerung, 2009Pontryagin max principle, direct vs. indirect methods, Jacobi matrices

48 / 48