Warm starting an Optimal Control Problem

[DarioSlaifsteinSk]

Inspired by some past discussions:

• Why Is this Infeasible? #335
• MPC implementation #336
  I started digging into the warm-starting solvers. I couldn't help to notice 2 things.

1. In JuMP you can use discrete arrays to load guess trajectories as:

@variable(model, x[i=1:2], start=initval[i])

also using Primal and dual warm-starts
2. In InifiteOpt you use functions as in #335, which are then transcribed by the package. The issue is that the only valid arg is t

But imagine you are solving consecutive problems in a rolling horizon fashion. How would I define a function with only one argument that does the following:

function getYaT(t::Float64, y::Vector{Float64}, timeArray::Vector{Float64})
    # Find the index corresponding to the given time t
    idx = searchsortedlast(timeArray, t)
    
    # If t is outside the range of timeArray, return an error
    if idx == 0 || idx > length(y)
        error("Time value is outside the range of the provided time array.")
    end
    
    # Return the corresponding y value
    return y[idx]
end

Should I implement a @parameter_function ? How do I handle times outside of the original array? How's that going to be continuos?

[pulsipher]

Hi @DarioSlaifsteinSk,

Good question.

To warm start MPC problems, I typically reuse the previous solution. Since, InfiniteOpt models in continuous time, we have to map the discrete solution back to a continuous function. I use interpolation to accomplish this:

using InfiniteOpt, Ipopt, Interpolations

# define the model
model = InfiniteModel(Ipopt.Optimizer)
@infinite_parameter(model, t in [0, 10], num_supports = 11)
@variable(model, y, Infinite(t), start = sin) # y(t) with some initial trajectory to get things going

# some model defined here....

# solve the first time
optimize!(model)
y_opt = value(y)

# now resolve as many times as we want
for i in 1:n
   y_opt_interp = linear_interpolation(value(t), y_opt)
   set_start_value_function(y, t -> y_opt_interp(t))

   # update the model for as makes sense (i.e., update the initial condition and the setpoint trajectory)

   optimize!(model)
   y_opt = value(y)
end

Currently, InfiniteOpt doesn't support specifying guesses for constraint primals/duals. This is on the todo list. However, you can always work directly with the underlying JuMP model once it is made (after optimize! is called):

using InfiniteOpt, Ipopt

# define the model
model = InfiniteModel(Ipopt.Optimizer)
@infinite_parameter(model, t in [0, 10], num_supports = 11)
@variable(model, y, Infinite(t), start = sin) # y(t) with some initial trajectory to get things going

# some model defined here....
@constraint(model, my_constr, y^2 <= 2)

# solve the first time (builds a JuMP model and solves it)
optimize!(model)

# get the solved JuMP model
discretized_model = optimizer_model(model)
discretized_y = transcription_variable(y)
discretized_my_constr = transcription_constraint(my_constr)

# update and resolve the JuMP model
for i in 1:n
   # do some updates

   # do the warmstart
   for yt in discretized_y
      set_start_value(yt, value(yt))
   end
   for ct in discretized_my_constr
      set_start_value(ct, value(ct))
      set_dual_start_value(ct, dual(ct))
   end
end

The API to work with the underlying JuMP model is described at https://infiniteopt.github.io/InfiniteOpt.jl/stable/guide/transcribe/. You can also use the set_optimal_start_values function detailed in the JuMP link you detailed above.

For your getYaT function, why not just use interpolation instead:

using Interpolations, InfiniteOpt

function interpY(y::Vector{Float64}, timeArray::Vector{Float64})
    return t -> linear_interpolation(timeArray, y)(t)
end

model = InfiniteModel(Ipopt.Optimizer)
@infinite_parameter(model, t in [0, 10], supports = my_ts)
@variable(model, y, Infinite(t), start = interpY(my_ys, my_ts))

As for handling times outside the array, this will vary on a case by case basis. Naturally, you might want to shift the previous solution forward in time which means adding on a new time point at the end. Choosing what value to use for this new time point will depend on the nature of the problem.

[pulsipher]

Cool, note that my code had a typo, but I fixed it.

[DarioSlaifsteinSk]

okay, I tried 2 things.
First I tried if I could pass warm-starts to the problem, i did a mockup using the Hovercraft control problem.

xw = [1 3 6 1; 1 2 0.5 1] # positions
tw = [0, 25, 50, 60];    # times
m = InfiniteModel(Ipopt.Optimizer);
@infinite_parameter(m, t in [0, 60], num_supports = 61)
@variables(m, begin
    # state variables
    x[1:2], Infinite(t)
    v[1:2], Infinite(t)
    # control variables
    u[1:2], Infinite(t), (start = 0)
end)
@objective(m, Min, ∫(u[1]^2 + u[2]^2, t))
@constraint(m, [i = 1:2], v[i](0) == 0)
@constraint(m, [i = 1:2], ∂(x[i], t) == v[i])
@constraint(m, [i = 1:2], ∂(v[i], t) == u[i])
m[:traj] = @constraint(m, [i = 1:2, j = eachindex(tw)], x[i](tw[j]) == xw[i, j])
optimize!(m)
x_opt = value.(x)
sols = []; push!(sols, value.(x));
# now resolve as many times as we want
for st in 1:1
   x_opt_interp = [linear_interpolation(value.(t), x_opt[i]) for i ∈ 1:2]
   [set_start_value_function(x[i], t -> x_opt_interp[i](t)) for i ∈ 1:2]

   # update the model for as makes sense (i.e., update the initial condition and the setpoint trajectory)
   xw = [1 4 6 1; 1 3 0 1] # positions
   tw = [0, 25, 50, 60];    # times
   [delete(m, m[:traj][i,j]) for i in 1:2, j in eachindex(tw)];
   # [unregister(m, m[:traj][i]) for i in 1:2];
   m[:traj] = @constraint(m, [i = 1:2, j = eachindex(tw)], x[i](tw[j]) == xw[i, j])
   optimize!(m)
   x_opt = value.(x)
   push!(sols, value.(x))
end

Here's the output:

Second thing that I tried was also moving the @infinite_parameter t and do a shrinking horizon MPC, but only for the first setpoint.

# Define the initial model
xw = [1 3; 1 2] # positions
tw = [0, 25];    # times
m = InfiniteModel(Ipopt.Optimizer);
t0 = 0; tend = 25; ns = 50; # number of supports
fs = ns/(tend-t0); # sampling frequency
@infinite_parameter(m, t in [t0, tend], num_supports = ns)
@variables(m, begin
    # state variables
    x[1:2], Infinite(t)
    v[1:2], Infinite(t)
    # control variables
    u[1:2], Infinite(t), (start = 0)
end)
# @objective(m, Min, ∫(u[1]^2 + u[2]^2 + (x[1](tw[2])-xw[1,2])^2 + (x[2](tw[2])-xw[2,2])^2, t))
@objective(m, Min, ∫(u[1]^2 + u[2]^2, t))
m[:initV] = @constraint(m, [i = 1:2], v[i](0) == 0)
@constraint(m, [i = 1:2], ∂(x[i], t) == v[i])
@constraint(m, [i = 1:2], ∂(v[i], t) == u[i])
m[:traj] = @constraint(m, [i = 1:2, j = eachindex(tw)], x[i](tw[j]) == xw[i, j])

# Optimize the model
optimize!(m)
# Store the optimal solution
x_opt = []; push!(x_opt, value.(x));
v_opt = []; push!(v_opt, value.(v));

# now resolve as many times as we want
# for ts ∈ 1:ns
for ts ∈ 1:3
    x_opt_interp = [linear_interpolation(value.(t), x_opt[ts][i], extrapolation_bc = Line()) for i ∈ 1:2]
    [set_start_value_function(x[i], t -> x_opt_interp[i](t)) for i ∈ 1:2]

   # update the model for as makes sense (i.e., update the initial condition and the setpoint trajectory)
    delete(m, objective_function(m)) # <--- necessary because you can't modify measures containing infinite_parameters
    t0 = t0 + 1/fs; tw = [t0, tend];
    set_infinite_domain(t, IntervalDomain(t0, tend))
    # remove old constraints
    [delete(m, m[:initV][i]) for i in 1:2]; # delete the old initial condition
    # delete the old trajectory in the first step, otherwise delete the old initial condition of X
    ts == 1 ? [delete(m, m[:traj][i,1]) for i in 1:2] : [delete(m, m[:initX][i]) for i in 1:2];
    # add new constraints,  update initial condition
    m[:initV]=@constraint(m, [i = 1:2], v[i](t0) == v_opt[ts][i][2])
    m[:initX]=@constraint(m, [i = 1:2], x[i](t0) == x_opt[ts][i][2])
    
    @objective(m, Min, ∫(u[1]^2 + u[2]^2, t))
    println(latex_formulation(m))
    optimize!(m)

    push!(x_opt, value.(x))
    push!(v_opt, value.(v))
end

I'm still using m[:traj] but probably i'd put all the initial conditions together and not in the waypoints but I maintained it to stay in line with the example.
From this second one there's a catch in the transcription of x_opt_interp. Here's the error stacktrace:

This is Ipopt version 3.14.14, running with linear solver MUMPS 5.6.2.

Number of nonzeros in equality constraint Jacobian...:      994
Number of nonzeros in inequality constraint Jacobian.:        0
Number of nonzeros in Lagrangian Hessian.............:      100

Total number of variables............................:      500
                     variables with only lower bounds:        0
                variables with lower and upper bounds:        0
                     variables with only upper bounds:        0
Total number of equality constraints.................:      402
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  0.0000000e+00 3.00e+00 0.00e+00  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
   1  9.3127393e-04 3.47e-16 3.47e-18  -1.0 3.00e+00    -  1.00e+00 1.00e+00h  1

Number of Iterations....: 1

                                   (scaled)                 (unscaled)
Objective...............:   9.3127393261760634e-04    9.3127393261760634e-04
Dual infeasibility......:   3.4694469519536142e-18    3.4694469519536142e-18
Constraint violation....:   3.4694469519536142e-16    3.4694469519536142e-16
Variable bound violation:   0.0000000000000000e+00    0.0000000000000000e+00
Complementarity.........:   0.0000000000000000e+00    0.0000000000000000e+00
Overall NLP error.......:   3.4694469519536142e-16    3.4694469519536142e-16


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   = 1
Number of inequality constraint Jacobian evaluations = 0
Number of Lagrangian Hessian evaluations             = 1
Total seconds in IPOPT                               = 0.002

EXIT: Optimal Solution Found.
Unable to locate transcription variable by support, consider rebuilding the infinite model with less significant digits. Note this might be due to partially evaluating dependent parameters which is not supported by TranscriptionOpt. Such is the case with derivatives/measures that dependent on single dependent parameters.

Stacktrace:

  [1] error(::String, ::String, ::String, ::String, ::String, ::String)
  [2] _supp_error()
  [3] get
  [4] lookup_by_support(model::Model, vref::GeneralVariableRef, index_type::Type{InfiniteVariableIndex}, support::Vector{Float64})
  [5] lookup_by_support(model::Model, vref::GeneralVariableRef, support::Vector{Float64})
  [6] transcribe_point_variables!(trans_model::Model, inf_model::InfiniteModel)
  [7] build_transcription_model!(trans_model::Model, inf_model::InfiniteModel; check_support_dims::Bool)
  [8] build_transcription_model!
  [9] #build_optimizer_model!#103
 [10] build_optimizer_model!
 [11] build_optimizer_model!(model::InfiniteModel; kwargs::@Kwargs{})
 [12] build_optimizer_model!
 [13] #optimize!#475
 [14] optimize!(model::InfiniteModel)

How do I modify the significant digits? shouldn't be something related to the extrapolated values?

[pulsipher]

Hi @DarioSlaifsteinSk,

The problem with the receding horizon approach actually does not have to do with the significant digits as suggested, but rather that InfiniteOpt isn't really designed to resolve a model with a modified infinite domain. In this case, there are left over point variables from the previous solve with supports that violate the domain bounds. This an edge case, we didn't consider.

Hence, a better way to tackle this would be to rebuild the model on each iteration (which is a minor expense) and set the new start value based on the previous solution. I would strongly recommend not modifying the infinite domain of model after it has already been built.

[DarioSlaifsteinSk]

And.... here it is!
This is the implementation (up to the first waypoint) for the Hovercraft model:

function build_model(t0, tend, ns, xw, tw, v0)
    fs = ns/(tend-t0); # sampling frequency
    m = InfiniteModel(Ipopt.Optimizer);
    @infinite_parameter(m, t in [t0, tend], num_supports = ns)
    @variables(m, begin
        # state variables
        x[1:2], Infinite(t)
        v[1:2], Infinite(t)
        # control variables
        u[1:2], Infinite(t), (start = 0)
    end)
    # @objective(m, Min, ∫(u[1]^2 + u[2]^2 + (x[1](tw[2])-xw[1,2])^2 + (x[2](tw[2])-xw[2,2])^2, t))
    @objective(m, Min, ∫(u[1]^2 + u[2]^2, t))
    @constraint(m, [i = 1:2], v[i](t0) == v0[i][2])
    @constraint(m, [i = 1:2], ∂(x[i], t) == v[i])
    @constraint(m, [i = 1:2], ∂(v[i], t) == u[i])
    @constraint(m, [i = 1:2, j = eachindex(tw)], x[i](tw[j]) == xw[i, j])
    set_silent(m)
    return m
end

xw = [1. 3. 6. 1.; 1. 2. 0.5 1.] # positions
tw = [0., 25., 50., 60.];    # times
t0 = 0; tend = 60; ns = 50; # number of supports
fs = ns/(tend-t0); # sampling frequency
m = build_model(t0, tend, ns, xw, tw, [[0 0], [0 0]])
optimize!(m)
# Store the optimal solution
x_opt = []; push!(x_opt, value.(m[:x]));
v_opt = []; push!(v_opt, value.(m[:v]));
for ts ∈ 2:10
    t0 = t0 + 1/fs; tw[1] = copy(t0);
    [xw[i,1] = copy(x_opt[ts-1][i][2]) for i ∈ 1:2]

    m = build_model(t0, tend, ns, xw, tw, v_opt[ts-1])
    x = m[:x]; # extract the state variables
    t = m[:t]; # extract the time variable
    # check
    x_opt_interp = [linear_interpolation(value.(t), x_opt[ts-1][i], extrapolation_bc = Line()) for i ∈ 1:2]
    # set warm start
    [set_start_value_function(x[i], t -> x_opt_interp[i](t)) for i ∈ 1:2]
    optimize!(m)
    solution_summary(optimizer_model(m))
    push!(x_opt, value.(m[:x]))
    push!(v_opt, value.(m[:v]))
end

After you pass each waypoint you have to check and erase an element of xw but it wasn't necessary to test the warmstarting.

[pulsipher]

Awesome, glad you got it to work!