Numerical Optimal Control with DAEs Lecture 8: Direct Collocation S - - PowerPoint PPT Presentation

Numerical Optimal Control with DAEs Lecture 8: Direct Collocation

S´ ebastien Gros

AWESCO PhD course

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 1 / 24

Survival map of Direct Optimal Control

OCP Collocation Single-Shooting Multiple-Shooting NLP Interior-Point Interior-Point SQP Active-Set QP solver QP solver

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 2 / 24

Survival map of Direct Optimal Control

OCP Collocation Single-Shooting Multiple-Shooting NLP Interior-Point Interior-Point SQP Active-Set QP solver QP solver

Another way for going from OCP to NLP

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 2 / 24

Outline

1

Polynomial interpolation

2

Collocation-based integration

3

Collocation in multiple-shooting

4

Direct Collocation

5

NLP from direct collocation

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 3 / 24

Outline

1

Polynomial interpolation

2

Collocation-based integration

3

Collocation in multiple-shooting

4

Direct Collocation

5

NLP from direct collocation

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 4 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1]

0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5

• 0.05

0.05 0.1 0.15 0.2 0.25 0.3 0.35

x1 f (x0, u0) x2 f (x1, u1) x3 f (x2, u2) x4 f (x3, u3) x0 f (x4, u4) State t0 t1 t2 t3 t4 t5

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] E.g. tk = 1, tk+1 = 2

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K

E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

Pk,0(t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

Pk,0(t) , Pk,1(t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

Pk,0(t) , Pk,1(t) , Pk,2(t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

Pk,0(t) , Pk,1(t) , Pk,2(t) , Pk,3(t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

Pk,0(t) , Pk,1(t) , Pk,2(t) , Pk,3(t) , Pk,4(t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

Pk,0(t) , Pk,1(t) , Pk,2(t) , Pk,3(t) , Pk,4(t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

Pk,0(t) , Pk,1(t) , Pk,2(t) , Pk,3(t) , Pk,4(t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

θk,i

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Polynomial interpolation

Consider a time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Lagrange Polynomials: Pk,i (t) =

K

• j=0, j=i

t − tk,j tk,i − tk,j ∈ R

• f order K, with property:

Pk,i (tk,l) = 1 if l = i if l = i Interpolation with θk,i ∈ Rn x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

having the property: x (θ, tk,j) = θk,j E.g. tk = 1, tk+1 = 2 K = 4 {tk,0, ..., tk,K} = {1.0, 1.0694, 1.33, 1.67, 1.931}

1 1.2 1.4 1.6 1.8 2

• 0.5

0.5 1 1.5

tk tk+1 tk,0 tk,1 tk,2 tk,3 tk,4

Note: the Lagrange polynomials are orthogonal, i.e. tk+1

tk

Pk,i(t)Pk,j(t) dt = 0, ∀i = j

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 5 / 24

Outline

1

Polynomial interpolation

2

Collocation-based integration

3

Collocation in multiple-shooting

4

Direct Collocation

5

NLP from direct collocation

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 6 / 24

Collocation methods - key idea Approximate state trajectory x (t) via polynomials (order K)

0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5

• 0.05

0.05 0.1 0.15 0.2 0.25 0.3 0.35

x1 f (x0, u0) x2 f (x1, u1) x3 f (x2, u2) x4 f (x3, u3) x0 f (x4, u4) State t0 t1 t2 t3 t4 t5

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 7 / 24

Collocation methods - key idea Approximate state trajectory x (t) via polynomials (order K)

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 7 / 24

Collocation methods - key idea Approximate state trajectory x (t) via polynomials (order K)

Time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1]

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 7 / 24

Collocation methods - key idea Approximate state trajectory x (t) via polynomials (order K)

Time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Interpolate on each interval [tk, tk+1] using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials 0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 7 / 24

Collocation methods - key idea Approximate state trajectory x (t) via polynomials (order K)

Time grid: {tk,0, ..., tk,K} ∈ [tk, tk+1] Interpolate on each interval [tk, tk+1] using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Integration: adjust θk,i to approximate the dynamics ˙ x = F (x, u)

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 7 / 24

Collocation methods - how to adjust the θk,i ?

On each interval [tk, tk+1], approximate ˙ x = F (x, uk) using x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

with x (θk, tk,j) = θk,j Note: we have K + 1 degrees of freedom per state.

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 8 / 24

Collocation methods - how to adjust the θk,i ?

On each interval [tk, tk+1], approximate ˙ x = F (x, uk) using x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

with x (θk, tk,j) = θk,j Note: we have K + 1 degrees of freedom per state. Collocation uses the constraints: x (θk, tk) = θk,0 = xk

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 8 / 24

Collocation methods - how to adjust the θk,i ?

On each interval [tk, tk+1], approximate ˙ x = F (x, uk) using x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

with x (θk, tk,j) = θk,j Note: we have K + 1 degrees of freedom per state. Collocation uses the constraints: x (θk, tk) = θk,0 = xk ∂ ∂t x (θk, tk,j) = F (x (θk, tk,j) , uk) , j = 1, ..., K

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 8 / 24

Collocation methods - how to adjust the θk,i ?

On each interval [tk, tk+1], approximate ˙ x = F (x, uk) using x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

with x (θk, tk,j) = θk,j Note: we have K + 1 degrees of freedom per state. Collocation uses the constraints: x (θk, tk) = θk,0 = xk (note that xk, uk are coming from the NLP !!) ∂ ∂t x (θk, tk,j) = F (x (θk, tk,j) , uk) , j = 1, ..., K

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 8 / 24

Collocation methods - how to adjust the θk,i ?

On each interval [tk, tk+1], approximate ˙ x = F (x, uk) using x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

with x (θk, tk,j) = θk,j Note: we have K + 1 degrees of freedom per state. Collocation uses the constraints: x (θk, tk) = θk,0 = xk (note that xk, uk are coming from the NLP !!) ∂ ∂t x (θk, tk,j) = F (θk,j, uk) , j = 1, ..., K

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 8 / 24

Collocation methods - how to adjust the θk,i ?

On each interval [tk, tk+1], approximate ˙ x = F (x, uk) using x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

with x (θk, tk,j) = θk,j Note: we have K + 1 degrees of freedom per state. Collocation uses the constraints: x (θk, tk) = θk,0 = xk (note that xk, uk are coming from the NLP !!)

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) , j = 1, ..., K

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 8 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K.

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. End-state: x (θk, tk+1) =

K

• i=0

θk,i · Pk,i(tk+1) Solve for θk,i using Newton      θk,0 − xk K

i=0 θk,i ˙

Pk,i(tk,1) − F (θk,1, uk) . . . K

i=0 θk,i ˙

Pk,i(tk,K) − F (θk,K, uk)      = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Collocation methods - Implementation

Collocation uses the constraints: θk,0 = xk

K

• i=0

θk,i · ˙ Pk,i(tk,j) = F (θk,j, uk) for j = 1, ..., K. End-state: x (θk, tk+1) =

K

• i=0

θk,i · Pk,i(tk+1) Shooting constraints f (xk,uk)

• =x(θk,tk+1)

−xk+1 = 0 becomes:

K

• i=0

θk,iPk,i(tk+1) − xk+1 = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 9 / 24

Selection of the time grid tk,i

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 10 / 24

Selection of the time grid tk,i

Collocation points on [0, 1]: K Legendre Radau 1 0.5 1.0 2 0.211325 0.333333 0.788675 1.000000 3 0.112702 0.155051 0.500000 0.644949 0.887298 1.000000 4 0.069432 0.088588 0.330009 0.409467 0.669991 0.787659 0.930568 1.000000 5 0.046910 0.057104 0.230765 0.276843 0.500000 0.583590 0.769235 0.860240 0.953090 1.000000

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 10 / 24

Selection of the time grid tk,i

Collocation points on [0, 1]: K Legendre Radau 1 0.5 1.0 2 0.211325 0.333333 0.788675 1.000000 3 0.112702 0.155051 0.500000 0.644949 0.887298 1.000000 4 0.069432 0.088588 0.330009 0.409467 0.669991 0.787659 0.930568 1.000000 5 0.046910 0.057104 0.230765 0.276843 0.500000 0.583590 0.769235 0.860240 0.953090 1.000000 +one point at tk to enforce continuity !

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 10 / 24

Selection of the time grid tk,i

Collocation points on [0, 1]: K Legendre Radau 1 0.5 1.0 2 0.211325 0.333333 0.788675 1.000000 3 0.112702 0.155051 0.500000 0.644949 0.887298 1.000000 4 0.069432 0.088588 0.330009 0.409467 0.669991 0.787659 0.930568 1.000000 5 0.046910 0.057104 0.230765 0.276843 0.500000 0.583590 0.769235 0.860240 0.953090 1.000000 +one point at tk to enforce continuity !

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Why these points ?!? They deliver an exact integration for any polynomial P of

• rder < 2K (Legendre) and < 2K − 1

(Radau). I.e. for ˙ x = F (x, u) = P(t) the collocation equations deliver an exact solution, namely: x (tk+1, θk) = xk + tk+1

tk

P (τ) dτ

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 10 / 24

Selection of the time grid tk,i

Collocation points on [0, 1]: K Legendre Radau 1 0.5 1.0 2 0.211325 0.333333 0.788675 1.000000 3 0.112702 0.155051 0.500000 0.644949 0.887298 1.000000 4 0.069432 0.088588 0.330009 0.409467 0.669991 0.787659 0.930568 1.000000 5 0.046910 0.057104 0.230765 0.276843 0.500000 0.583590 0.769235 0.860240 0.953090 1.000000 +one point at tk to enforce continuity !

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Interval [tk, tk+1] ?? Rescale & translate the collocation points to [tk, tk+1]

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 10 / 24

Selection of the time grid tk,i

Collocation points on [0, 1]: K Legendre Radau 1 0.5 1.0 2 0.211325 0.333333 0.788675 1.000000 3 0.112702 0.155051 0.500000 0.644949 0.887298 1.000000 4 0.069432 0.088588 0.330009 0.409467 0.669991 0.787659 0.930568 1.000000 5 0.046910 0.057104 0.230765 0.276843 0.500000 0.583590 0.769235 0.860240 0.953090 1.000000 +one point at tk to enforce continuity !

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Interval [tk, tk+1] ?? Rescale & translate the collocation points to [tk, tk+1], or... Modification of the collocation equations with hk = tk+1 − tk:

K

• j=0

θk,j ˙ Pk,j(tk,i) = hkF (θk,i, uk)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 10 / 24

Selection of the time grid tk,i

Collocation points on [0, 1]: K Legendre Radau 1 0.5 1.0 2 0.211325 0.333333 0.788675 1.000000 3 0.112702 0.155051 0.500000 0.644949 0.887298 1.000000 4 0.069432 0.088588 0.330009 0.409467 0.669991 0.787659 0.930568 1.000000 5 0.046910 0.057104 0.230765 0.276843 0.500000 0.583590 0.769235 0.860240 0.953090 1.000000 +one point at tk to enforce continuity !

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Interval [tk, tk+1] ?? Rescale & translate the collocation points to [tk, tk+1], or... Modification of the collocation equations with hk = tk+1 − tk:

K

• j=0

θk,j ˙ Pk,j(tk,i) = hkF (θk,i, uk) Careful if F is time-dependent !

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 10 / 24

Selection of the time grid tk,i

Collocation points on [0, 1]: K Legendre Radau 1 0.5 1.0 2 0.211325 0.333333 0.788675 1.000000 3 0.112702 0.155051 0.500000 0.644949 0.887298 1.000000 4 0.069432 0.088588 0.330009 0.409467 0.669991 0.787659 0.930568 1.000000 5 0.046910 0.057104 0.230765 0.276843 0.500000 0.583590 0.769235 0.860240 0.953090 1.000000 +one point at tk to enforce continuity !

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Interval [tk, tk+1] ?? Rescale & translate the collocation points to [tk, tk+1], or... Modification of the collocation equations with hk = tk+1 − tk:

K

• j=0

θk,j ˙ Pk,j(tk,i) = hkF (θk,i, uk) Careful if F is time-dependent ! Note that Radau has a collocation point at the end of the interval, i.e. θk,K provides the end-state of the integration !

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 10 / 24

Stability & Order

Collocation methods are A-stable (i.e. can handle stiff equations). They have no stability limitation on the time intervals h = tk+1 − tk for stiff problems. I.e. even large time steps h = tk+1 − tk allow for capturing steady state and slow dynamics. Radau collocation is additionally L-stable. I.e. it can handle eigenvalues at −∞. On an interval hk = tk+1 − tk, the integration error is O(h2K

k ) for Legendre and

O(h2K−1

k

) for Radau. Losing one order is the ”price” for having a collocation point at tk+1. The integration error applies to the end-state of the integrator, but not to the intermediate points ! Collocation-based integration is an Implicit Runge-Kutta scheme. Implicit Euler is an order-1 scheme !

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 11 / 24

Collocation - Sensitivity

Collocation constraints...

θk,0 = xk

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk , i = 1, ..., K

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 12 / 24

Collocation - Sensitivity

Collocation constraints...

θk,0 = xk

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk , i = 1, ..., K

... can be seen as

c (xk, uk, θk) = 0

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 12 / 24

Collocation - Sensitivity

Collocation constraints...

θk,0 = xk

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk , i = 1, ..., K

... can be seen as

c (xk, uk, θk) = 0 Solved by iterating: ∆θk = − ∂c (xk, uk, θk) ∂θk

−1

c (xk, uk, θk)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 12 / 24

Collocation - Sensitivity

Collocation constraints...

θk,0 = xk

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk , i = 1, ..., K Integrator function given by: f (xk, uk) = x (θk, tk+1)

... can be seen as

c (xk, uk, θk) = 0 Solved by iterating: ∆θk = − ∂c (xk, uk, θk) ∂θk

−1

c (xk, uk, θk)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 12 / 24

Collocation - Sensitivity

Collocation constraints...

θk,0 = xk

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk , i = 1, ..., K Integrator function given by: f (xk, uk) = x (θk, tk+1)

... can be seen as

c (xk, uk, θk) = 0 Solved by iterating: ∆θk = − ∂c (xk, uk, θk) ∂θk

−1

c (xk, uk, θk) Get sensitivities using: ∂f (xk, uk) ∂xk = ∂x (θk, tk+1) ∂θk ∂θk ∂xk , ∂f (xk, uk) ∂uk = ∂x (θk, tk+1) ∂θk ∂θk ∂uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 12 / 24

Collocation - Sensitivity

Collocation constraints...

θk,0 = xk

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk , i = 1, ..., K Integrator function given by: f (xk, uk) = x (θk, tk+1)

... can be seen as

c (xk, uk, θk) = 0 Solved by iterating: ∆θk = − ∂c (xk, uk, θk) ∂θk

−1

c (xk, uk, θk) Get sensitivities using: ∂f (xk, uk) ∂xk = ∂x (θk, tk+1) ∂θk ∂θk ∂xk , ∂f (xk, uk) ∂uk = ∂x (θk, tk+1) ∂θk ∂θk ∂uk Implicit function theorem states that ∂c ∂θk ∂θk ∂xk + ∂c ∂xk = 0, ∂c ∂θk ∂θk ∂uk + ∂c ∂uk = 0

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 12 / 24

Collocation - Sensitivity

Collocation constraints...

θk,0 = xk

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk , i = 1, ..., K Integrator function given by: f (xk, uk) = x (θk, tk+1)

... can be seen as

c (xk, uk, θk) = 0 Solved by iterating: ∆θk = − ∂c (xk, uk, θk) ∂θk

−1

c (xk, uk, θk) Get sensitivities using: ∂f (xk, uk) ∂xk = ∂x (θk, tk+1) ∂θk ∂θk ∂xk , ∂f (xk, uk) ∂uk = ∂x (θk, tk+1) ∂θk ∂θk ∂uk Implicit function theorem states that ∂c ∂θk ∂θk ∂xk + ∂c ∂xk = 0, ∂c ∂θk ∂θk ∂uk + ∂c ∂uk = 0 Hence: ∂θk ∂xk = − ∂c ∂θk

−1 ∂c

∂xk , ∂θk ∂uk = − ∂c ∂θk

−1 ∂c

∂uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 12 / 24

Collocation - Sensitivity

Collocation constraints...

θk,0 = xk

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk , i = 1, ..., K Integrator function given by: f (xk, uk) = x (θk, tk+1)

... can be seen as

c (xk, uk, θk) = 0 Solved by iterating: ∆θk = − ∂c (xk, uk, θk) ∂θk

−1

c (xk, uk, θk) Get sensitivities using: ∂f (xk, uk) ∂xk = ∂x (θk, tk+1) ∂θk ∂θk ∂xk , ∂f (xk, uk) ∂uk = ∂x (θk, tk+1) ∂θk ∂θk ∂uk Implicit function theorem states that ∂c ∂θk ∂θk ∂xk + ∂c ∂xk = 0, ∂c ∂θk ∂θk ∂uk + ∂c ∂uk = 0 Hence: ∂θk ∂xk = − ∂c ∂θk

−1 ∂c

∂xk , ∂θk ∂uk = − ∂c ∂θk

−1 ∂c

∂uk Note that

∂c ∂θk −1 is

computed in the Newton iteration, i.e. it comes for free !!

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 12 / 24

Outline

1

Polynomial interpolation

2

Collocation-based integration

3

Collocation in multiple-shooting

4

Direct Collocation

5

NLP from direct collocation

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 13 / 24

Collocation-based integrators in Multiple-shooting

Collocation-based integrator solves: c (xk, uk, θk) = 0

• n each time interval [tk, tk+1],

provides: f (xk, uk) = x (θk, tk+1) with sensitivities.

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 14 / 24

Collocation-based integrators in Multiple-shooting

Collocation-based integrator solves: c (xk, uk, θk) = 0

• n each time interval [tk, tk+1],

provides: f (xk, uk) = x (θk, tk+1) with sensitivities. NLP with multiple-shooting min

w

Φ (w) s.t. g (w) =     x0 − ¯ x0 f (x0, u0) − x1 ... f (xN−1, uN−1) − xN     where w = {x0, u0, ..., xN−1, uN−1, xN}

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 14 / 24

Collocation-based integrators in Multiple-shooting

Collocation-based integrator solves: c (xk, uk, θk) = 0

• n each time interval [tk, tk+1],

provides: f (xk, uk) = x (θk, tk+1) with sensitivities. NLP with multiple-shooting min

w

Φ (w) s.t. g (w) =     x0 − ¯ x0 f (x0, u0) − x1 ... f (xN−1, uN−1) − xN     where w = {x0, u0, ..., xN−1, uN−1, xN}

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t) NLP solves: ∇wL (w, λ) = 0 g (w) = 0

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 14 / 24

Collocation-based integrators in Multiple-shooting

Collocation-based integrator solves: c (xk, uk, θk) = 0

• n each time interval [tk, tk+1],

provides: f (xk, uk) = x (θk, tk+1) with sensitivities. NLP with multiple-shooting min

w

Φ (w) s.t. g (w) =     x0 − ¯ x0 f (x0, u0) − x1 ... f (xN−1, uN−1) − xN     where w = {x0, u0, ..., xN−1, uN−1, xN}

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t) NLP solves: ∇wL (w, λ) = 0 g (w) = 0 Collocation-based integrator inside the NLP becomes a two-level Newton scheme !!

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 14 / 24

Collocation-based integrators in Multiple-shooting (cont’)

. . . . . .

NLP solver ∇wL (w, λ) = 0 g (w) = 0 w = {x0, u0, ..., xN−1, uN−1, xN}

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 15 / 24

Collocation-based integrators in Multiple-shooting (cont’)

. . . . . .

NLP solver ∇wL (w, λ) = 0 g (w) = 0 w = {x0, u0, ..., xN−1, uN−1, xN}

NLP level Constraints g = 0 Newton iterations (SQP/IP)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 15 / 24

Collocation-based integrators in Multiple-shooting (cont’)

. . . . . .

NLP solver ∇wL (w, λ) = 0 g (w) = 0 Integrator [t0, t1] c (x0, u0, θ0) = 0 c (xk, uk, θk) = 0 with sensitivities Integrator [tk, tk+1] with sensitivities x0, u0 xk, uk w = {x0, u0, ..., xN−1, uN−1, xN} f (x0, u0) = x (θ0, t1) f (xk, uk) = x (θk, tk+1)

NLP level Constraints g = 0 Newton iterations (SQP/IP)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 15 / 24

Collocation-based integrators in Multiple-shooting (cont’)

. . . . . .

NLP solver ∇wL (w, λ) = 0 g (w) = 0 Integrator [t0, t1] c (x0, u0, θ0) = 0 c (xk, uk, θk) = 0 with sensitivities Integrator [tk, tk+1] with sensitivities x0, u0 xk, uk w = {x0, u0, ..., xN−1, uN−1, xN} f (x0, u0) = x (θ0, t1) f (xk, uk) = x (θk, tk+1)

NLP level Constraints g = 0 Newton iterations (SQP/IP) Integrator level Constraints c = 0 Newton iterations

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 15 / 24

Collocation-based integrators in Multiple-shooting (cont’)

. . . . . .

NLP solver ∇wL (w, λ) = 0 g (w) = 0 Integrator [t0, t1] c (x0, u0, θ0) = 0 c (xk, uk, θk) = 0 with sensitivities Integrator [tk, tk+1] with sensitivities x0, u0 xk, uk w = {x0, u0, ..., xN−1, uN−1, xN} f (x0, u0) = x (θ0, t1) f (xk, uk) = x (θk, tk+1)

NLP level Constraints g = 0 Newton iterations (SQP/IP) Integrator level Constraints c = 0 Newton iterations

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 15 / 24

Collocation-based integrators in Multiple-shooting (cont’)

. . . . . .

NLP solver ∇wL (w, λ) = 0 g (w) = 0 Integrator [t0, t1] c (x0, u0, θ0) = 0 c (xk, uk, θk) = 0 with sensitivities Integrator [tk, tk+1] with sensitivities x0, u0 xk, uk w = {x0, u0, ..., xN−1, uN−1, xN} f (x0, u0) = x (θ0, t1) f (xk, uk) = x (θk, tk+1)

NLP level Constraints g = 0 Newton iterations (SQP/IP) Integrator level Constraints c = 0 Newton iterations Constraints are solved at the NLP and at the integrator level separately !!

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 15 / 24

Collocation-based integrators in Multiple-shooting (cont’)

. . . . . .

NLP solver ∇wL (w, λ) = 0 g (w) = 0 Integrator [t0, t1] c (x0, u0, θ0) = 0 c (xk, uk, θk) = 0 with sensitivities Integrator [tk, tk+1] with sensitivities x0, u0 xk, uk w = {x0, u0, ..., xN−1, uN−1, xN} f (x0, u0) = x (θ0, t1) f (xk, uk) = x (θk, tk+1)

NLP level Constraints g = 0 Newton iterations (SQP/IP) Integrator level Constraints c = 0 Newton iterations Constraints are solved at the NLP and at the integrator level separately !! ... what about handling them altogether in the NLP ?!?

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 15 / 24

Outline

1

Polynomial interpolation

2

Collocation-based integration

3

Collocation in multiple-shooting

4

Direct Collocation

5

NLP from direct collocation

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 16 / 24

Direct collocation - Give all constraints to the NLP solver

On each interval [tk, tk+1] ˙ x = F (x, uk) is approximated using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Note: x

• θk,i, tk,i
• = θk,i

K + 1 degrees of freedom per state.

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 17 / 24

Direct collocation - Give all constraints to the NLP solver

On each interval [tk, tk+1] ˙ x = F (x, uk) is approximated using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Note: x

• θk,i, tk,i
• = θk,i

K + 1 degrees of freedom per state.

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Integration constraints (i = 1, ..., K) ∂ ∂t x θk, tk,i = F x θk, tk,i , uk

• i.e.

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 17 / 24

Direct collocation - Give all constraints to the NLP solver

On each interval [tk, tk+1] ˙ x = F (x, uk) is approximated using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Note: x

• θk,i, tk,i
• = θk,i

K + 1 degrees of freedom per state. NLP with direct collocation min

w

Φ (w) s.t. g (w) =                    

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Integration constraints (i = 1, ..., K) ∂ ∂t x θk, tk,i = F x θk, tk,i , uk

• i.e.

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 17 / 24

Direct collocation - Give all constraints to the NLP solver

On each interval [tk, tk+1] ˙ x = F (x, uk) is approximated using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Note: x

• θk,i, tk,i
• = θk,i

K + 1 degrees of freedom per state. NLP with direct collocation min

w

Φ (w) s.t. g (w) =           θ0,0 − ¯ x0           Initial conditions ¯ x0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Integration constraints (i = 1, ..., K) ∂ ∂t x θk, tk,i = F x θk, tk,i , uk

• i.e.

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 17 / 24

Direct collocation - Give all constraints to the NLP solver

On each interval [tk, tk+1] ˙ x = F (x, uk) is approximated using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Note: x

• θk,i, tk,i
• = θk,i

K + 1 degrees of freedom per state. NLP with direct collocation min

w

Φ (w) s.t. g (w) =           θ0,0 − ¯ x0 x (θ0, t1) − θ1,0           Continuity constraints (≡ shooting gaps)

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Integration constraints (i = 1, ..., K) ∂ ∂t x θk, tk,i = F x θk, tk,i , uk

• i.e.

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 17 / 24

Direct collocation - Give all constraints to the NLP solver

On each interval [tk, tk+1] ˙ x = F (x, uk) is approximated using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Note: x

• θk,i, tk,i
• = θk,i

K + 1 degrees of freedom per state. NLP with direct collocation min

w

Φ (w) s.t. g (w) =           θ0,0 − ¯ x0 x (θ0, t1) − θ1,0 F

• θ0,i, u0
• − K

j=0 θ0,j ˙

P0,j(t0,i )           Integration constraints for k = 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Integration constraints (i = 1, ..., K) ∂ ∂t x θk, tk,i = F x θk, tk,i , uk

• i.e.

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 17 / 24

Direct collocation - Give all constraints to the NLP solver

On each interval [tk, tk+1] ˙ x = F (x, uk) is approximated using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Note: x

• θk,i, tk,i
• = θk,i

K + 1 degrees of freedom per state. NLP with direct collocation min

w

Φ (w) s.t. g (w) =           θ0,0 − ¯ x0 x (θ0, t1) − θ1,0 F

• θ0,i, u0
• − K

j=0 θ0,j ˙

P0,j(t0,i ) ... x (θk, tk+1) − θk+1,0 F θk,i, uk − K

j=0 θk,j ˙

Pk,j(tk,i ) ...           Remaining integration constraints k = 1, ..., N − 1

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Integration constraints (i = 1, ..., K) ∂ ∂t x θk, tk,i = F x θk, tk,i , uk

• i.e.

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 17 / 24

Direct collocation - Give all constraints to the NLP solver

On each interval [tk, tk+1] ˙ x = F (x, uk) is approximated using: x (θk, t) =

K

• i=0

θk,i

• parameters

· Pk,i(t)

polynomials

Note: x

• θk,i, tk,i
• = θk,i

K + 1 degrees of freedom per state. NLP with direct collocation min

w

Φ (w) s.t. g (w) =           θ0,0 − ¯ x0 x (θ0, t1) − θ1,0 F

• θ0,i, u0
• − K

j=0 θ0,j ˙

P0,j(t0,i ) ... x (θk, tk+1) − θk+1,0 F θk,i, uk − K

j=0 θk,j ˙

Pk,j(tk,i ) ...           Decision variables: w =

• θ0,0, ..., θ0,K , u0, ..., θN−1,0, ..., θN−1,K , uN−1
• 0.35

0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Integration constraints (i = 1, ..., K) ∂ ∂t x θk, tk,i = F x θk, tk,i , uk

• i.e.

K

• j=0

θk,j ˙ Pk,j(tk,i) = F θk,i, uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 17 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

N = 20 K = 4 with Legendre, order 8 !! 420 variables 404 constraints

Reminder: x (θk, t) =

K

• i=0

θk,i · Pk,i(t) x (θk, tk,i) = θk,i

NLP with direct collocation min

w N−1

• k=0

u2

k

s.t. g (w) =             θ0,0 − ¯ x0 x (θ0, t1) − θ1,0 F

• θ0,i, u0
• − K

j=0 θ0,j ˙

P0,j(t0,i ) ... x (θk, tk+1) − θk+1,0 F

• θk,i, uk
• − K

j=0 θk,j ˙

Pk,j(tk,i ) ... x (θN−1, tN)             = 0

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 0

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 1

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 2

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 3

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 4

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 5

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 6

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 7

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Direct Collocation - Example: swing-up of a pendulum

OCP min

u0,...,uN−1 N−1

• k=0

u2

k

s.t. ˙ x = F (x, uk) , ∀t ∈ [tk, tk+1] x(0) =

• π
• ,

x (tf) = 0

M L u m θ x

x =

• x

θ ˙ x ˙ θ ⊤

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 8

K + 1 = 5 all nodes are initialised

0.0 0.5 1.0 1.5 2.0 −0.5 0.0 0.5 1.0 1.5

x

0.0 0.5 1.0 1.5 2.0 0.0 0.5 1.0 1.5 2.0 2.5 3.0 3.5 4.0

θ

0.0 0.5 1.0 1.5 2.0 −5 −4 −3 −2 −1 1 2 3

dx dt

0.0 0.5 1.0 1.5 2.0 −6 −4 −2 2

dθ dt

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 18 / 24

Cost and constraints discretisation in Direct Collocation

OCP: min T (x (tf)) + tf L (x (t) , u (t)) dt s.t. ˙ x = F (x, u) h (x (t) , u (t)) ≤ 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 19 / 24

Cost and constraints discretisation in Direct Collocation

OCP: min T (x (tf)) + tf L (x (t) , u (t)) dt s.t. ˙ x = F (x, u) h (x (t) , u (t)) ≤ 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Inequality constraints: h (x (t) , u (t)) ≤ 0 can be enforced on all collocation nodes: h x θk, tk,i , uk ≤ 0, ∀ k = 0, ..., N − 1, i = 0, ..., K but often only on the ”shooting” nodes t0,0, t1,0, ..., tN,0

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 19 / 24

Cost and constraints discretisation in Direct Collocation

OCP: min T (x (tf)) + tf L (x (t) , u (t)) dt s.t. ˙ x = F (x, u) h (x (t) , u (t)) ≤ 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Inequality constraints: h (x (t) , u (t)) ≤ 0 can be enforced on all collocation nodes: h x θk, tk,i , uk ≤ 0, ∀ k = 0, ..., N − 1, i = 0, ..., K but often only on the ”shooting” nodes t0,0, t1,0, ..., tN,0 Cost function often approximated as (rectangular quadrature): T x θN−1, tN−1,K +

N−1

• k=0

(tk+1 − tk) L θk,0, uk

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 19 / 24

Cost and constraints discretisation in Direct Collocation

OCP: min T (x (tf)) + tf L (x (t) , u (t)) dt s.t. ˙ x = F (x, u) h (x (t) , u (t)) ≤ 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Inequality constraints: h (x (t) , u (t)) ≤ 0 can be enforced on all collocation nodes: h x θk, tk,i , uk ≤ 0, ∀ k = 0, ..., N − 1, i = 0, ..., K but often only on the ”shooting” nodes t0,0, t1,0, ..., tN,0 Cost function often approximated as (rectangular quadrature): T x θN−1, tN−1,K +

N−1

• k=0

(tk+1 − tk) L θk,0, uk

• Careful: if you want to use θk,i for i = 1, ..., K, the time grid is not uniform !!
• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 19 / 24

Cost and constraints discretisation in Direct Collocation

OCP: min T (x (tf)) + tf L (x (t) , u (t)) dt s.t. ˙ x = F (x, u) h (x (t) , u (t)) ≤ 0

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

Inequality constraints: h (x (t) , u (t)) ≤ 0 can be enforced on all collocation nodes: h x θk, tk,i , uk ≤ 0, ∀ k = 0, ..., N − 1, i = 0, ..., K but often only on the ”shooting” nodes t0,0, t1,0, ..., tN,0 Cost function often approximated as (rectangular quadrature): T x θN−1, tN−1,K +

N−1

• k=0

(tk+1 − tk) L θk,0, uk

• Careful: if you want to use θk,i for i = 1, ..., K, the time grid is not uniform !!

Quadratic term in cost function L (x, u) = 1

2xTQx + ... can be implemented using:

tk+1

tk

1 2x (t)TQx (t) dt = 1 2

K

• l=0

K

• j=0

θk,l

TQθk,j

tk+1

tk

Pk,l(t)Pk,j(t)dt

• =αj δl,j (P:s are orthogonal)

= 1 2

K

• j=0

αjθk,j

TQθk,j

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 19 / 24

Some remarks

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 8

Direct collocation is a ”fully simultanuous” approach, as the integration and the

• ptimization are performed together in the NLP solver.

The decision variables are: w = {θ0,0, ..., θ0,K, u0, ..., θN−1,0, ..., θN−1,K , uN−1} Observe that θk,i, i.e. the state at the collocation point tk,i of the interval [tk, tk+1] is in Rn (size of the state). Manipulating these variables properly in a computer code can be tricky.

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 20 / 24

Refining the input discretization

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 8

Input u(t) is usually chosen piecewise-constant, i.e. constant in every [tk, tk+1]

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 21 / 24

Refining the input discretization

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 8

Input u(t) is usually chosen piecewise-constant, i.e. constant in every [tk, tk+1] However one can pick a different input uk,i for each collocation time tk,i. Gives K input vector per collocation interval, i.e. uk,1, ..., uk,K Collocation constraints: x (θk, tk) = xk ∂ ∂t x (θk, tk,i) = F (θk,i, uk) for i = 1, ..., K

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 21 / 24

Refining the input discretization

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 8

Input u(t) is usually chosen piecewise-constant, i.e. constant in every [tk, tk+1] However one can pick a different input uk,i for each collocation time tk,i. Gives K input vector per collocation interval, i.e. uk,1, ..., uk,K The continuous input is then given by the K − 1th order polynomial interpolation of uk,1, ..., uk,K Collocation constraints: x (θk, tk) = xk ∂ ∂t x (θk, tk,i) = F (θk,i, uk) for i = 1, ..., K

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 21 / 24

Refining the input discretization

0.35 0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85

• 2
• 1

1 2 3

xk xk+1

f (xk, uk)

tk tk+1 tk,0 tk,1 tk,2 tk,3

θk,0 θk,1 θk,2 θk,3

x (θk, t)

0.0 0.5 1.0 1.5 2.0 −25 −20 −15 −10 −5 5 10 15

u Newton step 8

Input u(t) is usually chosen piecewise-constant, i.e. constant in every [tk, tk+1] However one can pick a different input uk,i for each collocation time tk,i. Gives K input vector per collocation interval, i.e. uk,1, ..., uk,K The continuous input is then given by the K − 1th order polynomial interpolation of uk,1, ..., uk,K Drawbacks: 1. the input profile can present important ”oscillations”, 2. the linear algebra can loose some conditioning Collocation constraints: x (θk, tk) = xk ∂ ∂t x (θk, tk,i) = F (θk,i, uk) for i = 1, ..., K

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 21 / 24

Outline

1

Polynomial interpolation

2

Collocation-based integration

3

Collocation in multiple-shooting

4

Direct Collocation

5

NLP from direct collocation

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 22 / 24

Hessian in Direct Collocation

Lagrange function: L (w, λ) = Φ (w) + λTg (w) + µTh (w)

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 23 / 24

Hessian in Direct Collocation

Lagrange function: L (w, λ) = Φ (w) + λTg (w) + µTh (w) Hessian: ∇2

wL (w, λ) = ∇2Φ + ∇2 w

• λTg
• + ∇2

w

• µTh
• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 23 / 24

Hessian in Direct Collocation

Lagrange function: L (w, λ) = Φ (w) + λTg (w) + µTh (w) Hessian: ∇2

wL (w, λ) = ∇2Φ + ∇2 w

• λTg
• + ∇2

w

• µTh
• Reminder: dynamics yield

g (w) =           θ0,0 − ¯ x0 x (θ0, t1) − θ1,0 F

• θ0,i , u0
• − K

j=0 θ0,j ˙

P0,j(t0,i ) ... x

• θk , tk+1
• − θk+1,0

F

• θk,i , uk
• − K

j=0 θk,j ˙

Pk,j(tk,i ) ...          

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 23 / 24

Hessian in Direct Collocation

Lagrange function: L (w, λ) = Φ (w) + λTg (w) + µTh (w) Hessian: ∇2

wL (w, λ) = ∇2Φ + ∇2 w

• λTg
• + ∇2

w

• µTh
• Reminder: dynamics yield

g (w) =           θ0,0 − ¯ x0 x (θ0, t1) − θ1,0 F

• θ0,i , u0
• − K

j=0 θ0,j ˙

P0,j(t0,i ) ... x

• θk , tk+1
• − θk+1,0

F

• θk,i , uk
• − K

j=0 θk,j ˙

Pk,j(tk,i ) ...          

Contribution of the dynamics: ∇2

w

• λTg
• = ∇2

w

 

• k=0,...,N−1
• i=1,..,K

λk,i

T

• F (θk,i, uk) −

K

• j=0

θk,j ˙ Pk,j(tk,i)  

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 23 / 24

Hessian in Direct Collocation

Lagrange function: L (w, λ) = Φ (w) + λTg (w) + µTh (w) Hessian: ∇2

wL (w, λ) = ∇2Φ + ∇2 w

• λTg
• + ∇2

w

• µTh
• Reminder: dynamics yield

g (w) =           θ0,0 − ¯ x0 x (θ0, t1) − θ1,0 F

• θ0,i , u0
• − K

j=0 θ0,j ˙

P0,j(t0,i ) ... x

• θk , tk+1
• − θk+1,0

F

• θk,i , uk
• − K

j=0 θk,j ˙

Pk,j(tk,i ) ...          

Contribution of the dynamics: ∇2

w

• λTg
• = ∇2

w

 

• k=0,...,N−1
• i=1,..,K

λk,i

T

• F (θk,i, uk) −

K

• j=0

θk,j ˙ Pk,j(tk,i)   =

• k=0,...,N−1
• i=1,..,K

∇2

w

• λk,i

TF (θk,i, uk)

• With w = {θ0,0, ..., θ0,K, u0, ..., θN−1,0, ..., θN−1,K, uN−1}, the contributions

∇2

w

• λk,i

TF (θk,i, uk)

• are sparse and trivial to compute !! (e.g. CasADi)
• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 23 / 24

Sparsity pattern

E.g. for the crane, the KKT matrix M is:

100 200 300 400 500 600 700 800 100 200 300 400 500 600 700 800

M L u m θ x

M =

• H

∇g ∇g⊤

• S. Gros

Optimal Control with DAEs, lecture 8 18th of February, 2016 24 / 24