Double integrator: energy minimisation

Let us consider a wagon moving along a rail, whose acceleration can be controlled by a force $u$. We denote by $x = (q, v)$ the state of the wagon, where $q$ is the position and $v$ the velocity.

We assume that the mass is constant and equal to one, and that there is no friction. The dynamics are given by

\[ \dot q(t) = v(t), \quad \dot v(t) = u(t),\quad u(t) \in \R,\]

which is simply the double integrator system. Let us consider a transfer starting at time $t_0 = 0$ and ending at time $t_f = 1$, for which we want to minimise the transfer energy

\[ \frac{1}{2}\int_{0}^{1} u^2(t) \, \mathrm{d}t\]

starting from $x(0) = (-1, 0)$ and aiming to reach the target $x(1) = (0, 0)$.

First, we need to import the OptimalControl.jl package to define the optimal control problem, NLPModelsIpopt.jl to solve it, and Plots.jl to visualise the solution.

using OptimalControl
using NLPModelsIpopt
using Plots

Optimal control problem

Let us define the problem with the @def macro:

t0 = 0; tf = 1; x0 = [-1, 0]; xf = [0, 0]

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

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

    ∂(q)(t) == v(t)
    ∂(v)(t) == u(t)

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

Solve and plot

Direct method

We can solve it simply with:

direct_sol = solve(ocp)

▫ This is OptimalControl 1.3.3-beta, solving with: collocation → adnlp → ipopt (cpu)

  📦 Configuration:
   ├─ Discretizer: collocation
   ├─ Modeler: adnlp
   └─ Solver: ipopt

▫ This is Ipopt version 3.14.19, running with linear solver MUMPS 5.8.2.

Number of nonzeros in equality constraint Jacobian...:     1754
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:      250

Total number of variables............................:      752
                     variables with only lower bounds:        0
                variables with lower and upper bounds:        0
                     variables with only upper bounds:        0
Total number of equality constraints.................:      504
Total number of inequality constraints...............:        0
        inequality constraints with only lower bounds:        0
   inequality constraints with lower and upper bounds:        0
        inequality constraints with only upper bounds:        0

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
   0  5.0000000e-03 1.10e+00 2.03e-14   0.0 0.00e+00    -  0.00e+00 0.00e+00   0
   1  6.0000960e+00 2.22e-16 1.78e-15 -11.0 6.08e+00    -  1.00e+00 1.00e+00h  1

Number of Iterations....: 1

                                   (scaled)                 (unscaled)
Objective...............:   6.0000960015360247e+00    6.0000960015360247e+00
Dual infeasibility......:   1.7763568394002505e-15    1.7763568394002505e-15
Constraint violation....:   2.2204460492503131e-16    2.2204460492503131e-16
Variable bound violation:   0.0000000000000000e+00    0.0000000000000000e+00
Complementarity.........:   0.0000000000000000e+00    0.0000000000000000e+00
Overall NLP error.......:   1.7763568394002505e-15    1.7763568394002505e-15


Number of objective function evaluations             = 2
Number of objective gradient evaluations             = 2
Number of equality constraint evaluations            = 2
Number of inequality constraint evaluations          = 0
Number of equality constraint Jacobian evaluations   = 2
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 1
Total seconds in IPOPT                               = 5.163

EXIT: Optimal Solution Found.

And plot the solution with:

plot(direct_sol)

Indirect method

The first solution was obtained using the so-called direct method.^[1] Another approach is to use an indirect simple shooting method. We begin by importing the necessary packages.

using OrdinaryDiffEq # Ordinary Differential Equations (ODE) solver
using NonlinearSolve # Nonlinear Equations (NLE) solver

To define the shooting function, we must provide the maximising control in feedback form:

# maximising control, H(x, p, u) = p₁v + p₂u - u²/2
u(x, p) = p[2]

# Hamiltonian flow
f = Flow(ocp, u)

# state projection, p being the costate
π((x, p)) = x

# shooting function
S(p0) = π( f(t0, x0, p0, tf) ) - xf

We are now ready to solve the shooting equations.

# auxiliary in-place NLE function
nle!(s, p0, _) = s[:] = S(p0)

# initial guess for the Newton solver
p0_guess = [1, 1]

# NLE problem with initial guess
prob = NonlinearProblem(nle!, p0_guess)

# resolution of S(p0) = 0
shooting_sol = solve(prob; show_trace=Val(true))
p0_sol = shooting_sol.u # costate solution

# print the costate solution and the shooting function evaluation
println("\ncostate: p0 = ", p0_sol)
println("shoot: S(p0) = ", S(p0_sol), "\n")

Algorithm: NewtonRaphson(
    descent = NewtonDescent(),
    autodiff = AutoForwardDiff(),
    vjp_autodiff = AutoReverseDiff(
        compile = false
    ),
    jvp_autodiff = AutoForwardDiff(),
    concrete_jac = Val{false}()
)

----    	-------------       	-----------
Iter    	f(u) inf-norm       	Step 2-norm
----    	-------------       	-----------
0       	6.66666667e-01      	0.00000000e+00
1       	2.31244296e-14      	1.20830460e+01
Final   	2.31244296e-14
----------------------

costate: p0 = [12.000000000000192, 6.000000000000071]
shoot: S(p0) = [1.7868366116270543e-15, -2.3124429567206217e-14]

To plot the solution obtained by the indirect method, we need to build the solution of the optimal control problem. This is done using the costate solution and the flow function.

indirect_sol = f((t0, tf), x0, p0_sol; saveat=range(t0, tf, 100))
plot(indirect_sol)

State constraint

The following example illustrates both direct and indirect solution approaches for the energy minimization problem with a state constraint on the maximal velocity. The workflow demonstrates a practical strategy: a direct method on a coarse grid first identifies the problem structure and provides an initial guess for the indirect method, which then computes a precise solution via shooting based on Pontryagin's Maximum Principle.

Direct method: constrained case

We add the path constraint

\[ v(t) \le 1.2.\]

Let us model, solve and plot the optimal control problem with this constraint.

# the upper bound for v
v_max = 1.2

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

    v(t) ≤ v_max    # state constraint

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

    ∂(q)(t) == v(t)
    ∂(v)(t) == u(t)

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

# solve with a direct method
direct_sol = solve(ocp; grid_size=50)

# plot the solution
plt = plot(direct_sol; label="Direct", size=(800, 600))

The solution has three phases (unconstrained-constrained-unconstrained arcs), requiring definition of Hamiltonian flows for each phase and a shooting function to enforce boundary and switching conditions.

Indirect method: constrained case

Under the normal case, the pseudo-Hamiltonian reads:

\[H(x, p, u, \mu) = p_1 v + p_2 u - \frac{u^2}{2} + \mu\, g(x),\]

where $g(x) = v_{\max} - v$. Along a boundary arc we have $g(x(t)) = 0$; differentiating gives:

\[ \frac{\mathrm{d}}{\mathrm{d}t}g(x(t)) = -\dot{v}(t) = -u(t) = 0.\]

The zero control maximises the Hamiltonian, so $p_2(t) = 0$ along that arc. From the adjoint equation we then have

\[ \dot{p}_2(t) = -p_1(t) + \mu(t) = 0 \quad \Rightarrow \mu(t) = p_1(t).\]

Because the adjoint vector is continuous at both the entry time $t_1$ and the exit time $t_2$, the unknowns are $p_0 \in \mathbb{R}^2$ together with $t_1$ and $t_2$. The target condition supplies two equations, $g(x(t_1)) = 0$ enforces the state constraint, and $p_2(t_1) = 0$ encodes the switching condition.

# flow for unconstrained extremals
f_interior = Flow(ocp, (x, p) -> p[2])

ub = 0              # boundary control
g(x) = v_max - x[2] # constraint: g(x) ≥ 0
μ(p) = p[1]         # dual variable

# flow for boundary extremals
f_boundary = Flow(ocp, (x, p) -> ub, (x, u) -> g(x), (x, p) -> μ(p))

# shooting function
function shoot!(s, p0, t1, t2)
    x_t0, p_t0 = x0, p0
    x_t1, p_t1 = f_interior(t0, x_t0, p_t0, t1)
    x_t2, p_t2 = f_boundary(t1, x_t1, p_t1, t2)
    x_tf, p_tf = f_interior(t2, x_t2, p_t2, tf)
    s[1:2] = x_tf - xf
    s[3] = g(x_t1)
    s[4] = p_t1[2]
end

We can derive an initial guess for the costate and the entry/exit times from the direct solution:

t = time_grid(direct_sol) # the time grid as a vector
x = state(direct_sol)     # the state as a function of time
p = costate(direct_sol)   # the costate as a function of time

# initial costate
p0 = p(t0)

# times where constraint is active
t12 = t[ 0 .≤ (g ∘ x).(t) .≤ 1e-3 ]

# entry and exit times
t1 = minimum(t12) # entry time
t2 = maximum(t12) # exit time

We can now solve the shooting equations.

# auxiliary in-place NLE function
nle!(s, ξ, _) = shoot!(s, ξ[1:2], ξ[3], ξ[4])

# initial guess for the Newton solver
ξ_guess = [p0..., t1, t2]

# NLE problem with initial guess
prob = NonlinearProblem(nle!, ξ_guess)

# resolution of the shooting equations
shooting_sol = solve(prob; show_trace=Val(true))
p0, t1, t2 = shooting_sol.u[1:2], shooting_sol.u[3], shooting_sol.u[4]

# print the costate solution and the entry and exit times
println("\np0 = ", p0, "\nt1 = ", t1, "\nt2 = ", t2)

Algorithm: NewtonRaphson(
    descent = NewtonDescent(),
    autodiff = AutoForwardDiff(),
    vjp_autodiff = AutoReverseDiff(
        compile = false
    ),
    jvp_autodiff = AutoForwardDiff(),
    concrete_jac = Val{false}()
)

----    	-------------       	-----------
Iter    	f(u) inf-norm       	Step 2-norm
----    	-------------       	-----------
0       	9.23204318e-02      	0.00000000e+00
1       	3.69427252e-03      	6.09759937e-01
2       	6.59807546e-04      	4.98311337e-01
3       	1.28891676e-08      	4.83741523e-03
4       	5.86152803e-15      	2.73341876e-08
Final   	5.86152803e-15
----------------------

p0 = [38.40000000000023, 9.60000000000002]
t1 = 0.249999999999999
t2 = 0.7500000000000007

To reconstruct the constrained trajectory, concatenate the flows as follows: an unconstrained arc until $t_1$, a boundary arc from $t_1$ to $t_2$, and a final unconstrained arc from $t_2$ to $t_f$. This composition yields the full solution (state, costate, and control), which we then plot alongside the direct method for comparison.

# concatenation of the flows
φ = f_interior * (t1, f_boundary) * (t2, f_interior)

# compute the solution: state, costate, control...
indirect_sol = φ((t0, tf), x0, p0; saveat=range(t0, tf, 100))

# plot the solution on the previous plot
plot!(plt, indirect_sol; label="Indirect", color=2, linestyle=:dash)