@@ -20,17 +20,32 @@ struct CasADiDynamicOptProblem{uType, tType, isinplace, P, F, K} <:
2020 end
2121end
2222
23+ # Default linear interpolation for MX objects, likely to change down the line when we support interpolation with the collocation polynomial.
24+ struct MXLinearInterpolation
25+ u:: MX
26+ t:: Vector{Float64}
27+ dt:: Float64
28+ end
29+
2330struct CasADiModel
2431 opti:: Opti
25- U:: AbstractInterpolation
26- V:: AbstractInterpolation
32+ U:: MXLinearInterpolation
33+ V:: MXLinearInterpolation
2734 tₛ:: Union{Number, MX}
2835end
2936
37+ function (M:: MXLinearInterpolation )(τ)
38+ nt = (τ - M. t) / M. dt
39+ i = 1 + floor (Int, nt)
40+ Δ = nt - i + 1
41+
42+ (i > length (M. t) || i < 1 ) && error (" Cannot extrapolate past the tspan." )
43+ M. u[i] + Δ* (M. u[i + 1 ] - M. u[i])
44+ end
45+
3046function MTK. CasADiDynamicOptProblem (sys:: ODESystem , u0map, tspan, pmap;
3147 dt = nothing ,
3248 steps = nothing ,
33- interpolation_method:: AbstractInterpolation = LinearInterpolation,
3449 guesses = Dict (), kwargs... )
3550 MTK. warn_overdetermined (sys, u0map)
3651 _u0map = has_alg_eqs (sys) ? u0map : merge (Dict (u0map), Dict (guesses))
@@ -39,10 +54,10 @@ function MTK.CasADiDynamicOptProblem(sys::ODESystem, u0map, tspan, pmap;
3954
4055 pmap = Dict {Any, Any} (pmap)
4156 steps, is_free_t = MTK. process_tspan (tspan, dt, steps)
42- model = init_model ()
57+ model = init_model (sys, tspan, steps, u0map, pmap, u0; is_free_t )
4358end
4459
45- function init_model (sys, tspan, steps, u0map, pmap, u0; is_free_t = false , interp_type :: AbstractInterpolation )
60+ function init_model (sys, tspan, steps, u0map, pmap, u0; is_free_t = false )
4661 ctrls = MTK. unbound_inputs (sys)
4762 states = unknowns (sys)
4863 opti = CasADi. Opti ()
@@ -61,8 +76,8 @@ function init_model(sys, tspan, steps, u0map, pmap, u0; is_free_t = false, inter
6176 U = CasADi. variable! (opti, length (states), steps)
6277 V = CasADi. variable! (opti, length (ctrls), steps)
6378
64- U_interp = interp_type ( Matrix (U) , tsteps)
65- V_interp = interp_type ( Matrix (V) , tsteps)
79+ U_interp = MXLinearInterpolation (U , tsteps, tsteps[ 2 ] - tsteps[ 1 ] )
80+ V_interp = MXLinearInterpolation (V , tsteps, tsteps[ 2 ] - tsteps[ 1 ] )
6681
6782 CasADiModel (opti, U_interp, V_interp, tₛ)
6883end
@@ -220,9 +235,8 @@ function add_solve_constraints!(prob, tableau; is_free_t)
220235 end
221236end
222237
223-
224238"""
225- solve(prob::CasADiDynamicOptProblem, casadi_solver, ode_solver; plugin_options, solver_options)
239+ solve(prob::CasADiDynamicOptProblem, casadi_solver, ode_solver; plugin_options, solver_options, silent)
226240
227241`plugin_options` and `solver_options` get propagated to the Opti object in CasADi.
228242"""
@@ -247,7 +261,6 @@ function DiffEqBase.solve(prob::CasADiDynamicOptProblem, solver::Union{String, S
247261 catch ErrorException
248262 value_getter = x -> CasADi. debug_value (opti, x)
249263 failed = true
250- continue
251264 end
252265
253266 ts = value_getter (tₛ) * U. t
@@ -270,5 +283,4 @@ function DiffEqBase.solve(prob::CasADiDynamicOptProblem, solver::Union{String, S
270283
271284 DynamicOptSolution (cmodel, sol, input_sol)
272285end
273-
274286end
0 commit comments