State constraint

This example shows how state constraints of different orders shape the solutions of the energy-minimal double integrator, and how to compute them by the direct and the indirect methods. Bryson et al.^[1] and Jacobson et al.^[2] solve such problems analytically.

A wagon moves along a rail, and we control its acceleration with a force . Its state is  , where is the position and the velocity.

The mass is one and there is no friction, so the dynamics are those of the double integrator,  ,  , and we minimise the energy  . Without a constraint, this is Energy minimisation.

using OptimalControl
using NLPModelsIpopt
using OrdinaryDiffEqTsit5
using NonlinearSolve
using Plots

A first-order constraint: a speed limit

The problem

The wagon goes from rest at    to rest at  , as in the energy example, and its velocity is bounded,   . Without the bound, the velocity    reaches at  , so the bound is active.

t0, tf = 0, 1
x0, xf = [-1, 0], [0, 0]
v_max = 1.2

ocp = @def begin
    t ∈ [t0, tf], time
    x = (q, v) ∈ R², state
    u ∈ R, control

    v(t) ≤ v_max

    x(t0) == x0
    x(tf) == xf

    ẋ(t) == [v(t), u(t)]

    0.5∫(u(t)^2) → min
end

The constraint is of first order: with    , the first derivative already involves the control,   .

Direct method

A direct method on a coarse grid shows the structure of the solution, and gives the initial guess of the indirect method:

direct_sol = solve(ocp; grid_size=50, display=false)
plt = plot(direct_sol; label="direct", size=(800, 600))

The solution has three arcs: an unconstrained arc, a boundary arc where  , and an unconstrained arc.

The exact solution

With the conventions of Notation and conventions, the pseudo-Hamiltonian is

with   off the boundary arc. On an unconstrained arc,  . Along the boundary arc,  , so its derivative vanishes:  . The zero control maximises , so   along the arc, and the adjoint equation      gives the multiplier,  . For a first-order constraint, the costate is continuous at the junctions.

The costate is constant, so on the first arc,   is affine, and it vanishes at the entry time :   . The velocity reaches at , so  , and the last arc is symmetric. The distance covered,    , then gives

The cost is      .

Indirect method

Each arc has its flow. The flow on the boundary arc takes the constraint and the multiplier , both as functions (see Constrained arcs):

f_interior = Flow(ocp, (x, p) -> p[2])

c(x) = v_max - x[2]   # the constraint, c ≥ 0
μ(x, p) = p[1]        # the multiplier on the boundary arc

f_boundary = Flow(ocp, (x, p) -> 0; constraint=(x, u) -> c(x), multiplier=μ)

Since the costate is continuous, the unknowns are   and the junction times and . The target gives two equations, the entry on the boundary,  , a third, and the switching condition   the fourth:

function shoot!(s, ξ, _)
    p0, t1, t2 = ξ[1:2], ξ[3], ξ[4]
    x1, p1 = f_interior(t0, x0, p0, t1)
    x2, p2 = f_boundary(t1, x1, p1, t2)
    x3, _ = f_interior(t2, x2, p2, tf)
    s[1:2] = x3 - xf      # target
    s[3] = c(x1)          # entry on the boundary
    s[4] = p1[2]          # switching condition
    return nothing
end

The direct solution gives the initial guess: its costate at , and the first and last grid points where the constraint is active:

tg = time_grid(direct_sol)
x_d = state(direct_sol)

active = findall(t -> 0 ≤ c(x_d(t)) ≤ 1e-3, tg)
ξ_guess = [costate(direct_sol)(t0)..., tg[first(active)], tg[last(active)]]

4-element Vector{Float64}:
 38.46267385337124
  9.23085239669563
  0.24
  0.76

prob = NonlinearProblem(shoot!, ξ_guess)
shooting_sol = NonlinearSolve.solve(prob; show_trace=Val(false))
p0_sol, t1_sol, t2_sol = shooting_sol.u[1:2], shooting_sol.u[3], shooting_sol.u[4]

([38.40000000000063, 9.600000000000065], 0.2499999999999977, 0.7500000000000017)

The shooting finds the exact values,  ,   and  .

Comparison

The concatenation of the three flows gives the extremal, with the cost ; the direct cost differs by the discretisation error:

φ = f_interior * (t1_sol, f_boundary) * (t2_sol, f_interior)
indirect_sol = φ((t0, tf), x0, p0_sol)
objective(direct_sol), objective(indirect_sol)

(7.692307468707567, 7.680000112835119)

For the plot, the flows are built with saveat, so that the trajectory has 101 points (see Plotting a flow):

fine = (saveat=range(t0, tf, 101), dense=false)
φ_plot = Flow(ocp, (x, p) -> p[2]; fine...) *
    (t1_sol, Flow(ocp, (x, p) -> 0; constraint=(x, u) -> c(x), multiplier=μ, fine...)) *
    (t2_sol, Flow(ocp, (x, p) -> p[2]; fine...))
plot!(plt, φ_plot((t0, tf), x0, p0_sol); label="indirect", linestyle=:dash)

A second-order constraint: the Bryson–Denham problem

The problem

The wagon now starts at   with the velocity , and must come back to   with the velocity :   and   . Its position is bounded,  . This is the Bryson–Denham problem^[1:1]. The constraint is of second order: with   , the control appears only in the second derivative,

so along a boundary arc,   and  .

Without the constraint,    and   , whose maximum is , at  . With the constraint, the solution depends on ^[3]:

• for  , the constraint is never active;

• for   , the trajectory touches   at a single time,  ;

• for  , the trajectory stays on   along a boundary arc, on  .

We take   for a touch point, and   for a boundary arc:

x0_bd, xf_bd = [0, 1], [0, -1]

function bryson_denham(a)
    return @def begin
        t ∈ [t0, tf], time
        x = (q, v) ∈ R², state
        u ∈ R, control

        q(t) ≤ a

        x(t0) == x0_bd
        x(tf) == xf_bd

        ẋ(t) == [v(t), u(t)]

        0.5∫(u(t)^2) → min
    end
end

Direct method

a_touch, a_arc = 0.2, 0.1
ocp_touch, ocp_arc = bryson_denham(a_touch), bryson_denham(a_arc)

sol_touch = solve(ocp_touch; grid_size=100, display=false)
sol_arc = solve(ocp_arc; grid_size=100, display=false)

legend_off = (state_style=(legend=false,), costate_style=(legend=false,))
plt_bd = plot(sol_touch; label="a = 0.2", size=(800, 600), legend_off...)
plot!(plt_bd, sol_arc; label="a = 0.1", linestyle=:dash, legend_off...)

The touch point

For  , the solution has two unconstrained arcs, on and , joined at the contact time , where   and  . At , the costate jumps: increases by  , and is continuous.

By symmetry,  . On the first arc,   is affine, so is a cubic, and the conditions  ,  ,   and   give    . Since   ,    on , and by symmetry   on :

The unknowns of the shooting are , and ; the target,   and   give four equations. A vector jump on a Hamiltonian flow is a jump of the costate (see Multi-phase flows):

f_touch = Flow(ocp_touch, (x, p) -> p[2])

function shoot_touch!(s, ξ, _)
    p0, t1, Δpq = ξ[1:2], ξ[3], ξ[4]
    x1, p1 = f_touch(t0, x0_bd, p0, t1)
    x2, _ = f_touch(t1, x1, p1 + [Δpq, 0], tf)   # costate jump at t1
    s[1:2] = x2 - xf_bd      # target
    s[3] = a_touch - x1[1]   # contact: q(t1) = a
    s[4] = x1[2]             # tangency: v(t1) = 0
    return nothing
end

The direct solution gives the guess: the contact time is where is closest to , and the jump is the variation of its costate around that time:

tg_t = time_grid(sol_touch)
p_t = costate(sol_touch)

t1_guess = tg_t[argmin(abs.(a_touch .- first.(state(sol_touch).(tg_t))))]
Δpq_guess = p_t(t1_guess + 0.05)[1] - p_t(t1_guess - 0.05)[1]
ξ_guess_t = [p_t(t0)..., t1_guess, Δpq_guess]

shooting_touch = NonlinearSolve.solve(NonlinearProblem(shoot_touch!, ξ_guess_t); show_trace=Val(false))
p0_touch, t1_touch, Δpq_touch = shooting_touch.u[1:2], shooting_touch.u[3], shooting_touch.u[4]

([-4.800000000000061, -3.200000000000015], 0.49999999999999784, 9.600000000000042)

These are the exact values. The concatenation with the jump gives the extremal and its cost, :

φ_touch = f_touch * (t1_touch, [Δpq_touch, 0], f_touch)
indirect_touch = φ_touch((t0, tf), x0_bd, p0_touch)
objective(sol_touch), objective(indirect_touch)

(2.240095942594309, 2.2400006467009037)

The boundary arc

For  , the solution has three arcs: unconstrained on , on the boundary   on , and unconstrained on . The pseudo-Hamiltonian is

Along the boundary arc,  , so   by the maximisation condition. Then    , and   : the multiplier is zero, and the costate is zero on the arc. It jumps at the entry and at the exit, by and .

On the first arc, the conditions  ,   and    give   and    , so   ; the last arc is symmetric:

The flow on the boundary arc takes the constraint and its multiplier, here zero:

f_free = Flow(ocp_arc, (x, p) -> p[2])
f_arc = Flow(ocp_arc, (x, p) -> 0; constraint=(x, u) -> a_arc - x[1], multiplier=(x, p) -> 0)

function shoot_arc!(s, ξ, _)
    p0, t1, t2, Δ1, Δ2 = ξ[1:2], ξ[3], ξ[4], ξ[5], ξ[6]
    x1, p1 = f_free(t0, x0_bd, p0, t1)
    p1⁺ = p1 + [Δ1, 0]                          # jump at the entry
    x2, p2 = f_arc(t1, x1, p1⁺, t2)
    x3, _ = f_free(t2, x2, p2 + [Δ2, 0], tf)    # jump at the exit
    s[1:2] = x3 - xf_bd      # target
    s[3] = a_arc - x1[1]     # entry: q(t1) = a
    s[4] = x1[2]             # tangency: v(t1) = 0
    s[5] = p1⁺[2]            # p_v = 0 on the arc
    s[6] = p1⁺[1]            # p_q = 0 on the arc
    return nothing
end

The guess comes from the direct solution, as before:

tg_a = time_grid(sol_arc)
x_a, p_a = state(sol_arc), costate(sol_arc)

active_a = findall(t -> 0 ≤ a_arc - x_a(t)[1] ≤ 1e-3, tg_a)
t1_g, t2_g = tg_a[first(active_a)], tg_a[last(active_a)]
Δ1_g = p_a(t1_g + 0.1)[1] - p_a(t1_g - 0.1)[1]
Δ2_g = p_a(t2_g + 0.1)[1] - p_a(t2_g - 0.1)[1]
ξ_guess_a = [p_a(t0)..., t1_g, t2_g, Δ1_g, Δ2_g]

shooting_arc = NonlinearSolve.solve(NonlinearProblem(shoot_arc!, ξ_guess_a); show_trace=Val(false))
p0_arc, t1_arc, t2_arc = shooting_arc.u[1:2], shooting_arc.u[3], shooting_arc.u[4]
Δ1, Δ2 = shooting_arc.u[5], shooting_arc.u[6]
(t1_arc, t2_arc), (Δ1, Δ2)

((0.29999999999999627, 0.7000000000000011), (22.222222222222676, 22.222222222222516))

The junction times are and  , and the two jumps are equal,  . The cost is  :

φ_arc = f_free * (t1_arc, [Δ1, 0], f_arc) * (t2_arc, [Δ2, 0], f_free)
indirect_arc = φ_arc((t0, tf), x0_bd, p0_arc)
objective(sol_arc), objective(indirect_arc)

(4.4456792023655485, 4.444444338873086)

Comparison

The two indirect solutions, plotted with flows built with saveat:

fine = (saveat=range(t0, tf, 101), dense=false)
ft = Flow(ocp_touch, (x, p) -> p[2]; fine...)
ff = Flow(ocp_arc, (x, p) -> p[2]; fine...)
fa = Flow(ocp_arc, (x, p) -> 0; constraint=(x, u) -> a_arc - x[1], multiplier=(x, p) -> 0, fine...)

plt_ind = plot((ft * (t1_touch, [Δpq_touch, 0], ft))((t0, tf), x0_bd, p0_touch);
               label="a = 0.2", size=(800, 600), legend_off...)
plot!(plt_ind, (ff * (t1_arc, [Δ1, 0], fa) * (t2_arc, [Δ2, 0], ff))((t0, tf), x0_bd, p0_arc);
      label="a = 0.1", linestyle=:dash, legend_off...)

The costate jumps once at the touch point, and twice around the boundary arc, where the costate is zero.

Notes

• Constrained arcs explains the flows on a boundary arc, with another Bryson–Denham instance.

• Multi-phase flows explains the concatenation of flows, and the jumps.

• MINPACK.jl can replace NonlinearSolve to solve the shooting equations.

---

1. A. E. Bryson, W. F. Denham and S. E. Dreyfus, Optimal programming problems with inequality constraints I: necessary conditions for extremal solutions, AIAA Journal 1 (1963), no. 11, 2544–2550. doi.org/10.2514/3.2107 ↩︎ ↩︎

2. D. H. Jacobson, M. M. Lele and J. L. Speyer, New necessary conditions of optimality for control problems with state-variable inequality constraints, Journal of Mathematical Analysis and Applications 35 (1971), 255–284. ↩︎

3. A. E. Bryson and Y.-C. Ho, Applied Optimal Control: Optimization, Estimation and Control, CRC Press, 1975. ↩︎