diff --git a/Project.toml b/Project.toml index 0fbe2554..7379eaa2 100644 --- a/Project.toml +++ b/Project.toml @@ -27,7 +27,7 @@ ADNLPModels = "0.8" CTBase = "0.13" DocStringExtensions = "0.9" HSL = "0.4" -JLD2 = "0.5" +JLD2 = "0.4" # 0.5 has ugly warnings for functions JSON3 = "1" MadNLP = "0.8" NLPModels = "0.21" diff --git a/docs/Project.toml b/docs/Project.toml index 098fd971..7d603782 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -15,7 +15,7 @@ CTDirect = "0.12" Documenter = "1" DocumenterMermaid = "0.1" HSL = "0.4" -JLD2 = "0.5" +JLD2 = "0.4" # 0.5 has ugly warnings for functions JSON3 = "1" NLPModelsIpopt = "0.10" Plots = "1" diff --git a/src/midpoint.jl b/src/midpoint.jl index 0ce639d5..a6100c6a 100644 --- a/src/midpoint.jl +++ b/src/midpoint.jl @@ -5,11 +5,17 @@ with the convention u([t_i,t_i+1[) = U_i and u(tf) = U_N-1 =# # +++ TODO: use args - +# NB. could be defined as a generic IRK struct Midpoint <: Discretization + stage::Int additional_controls::Int - Midpoint() = new(1, 0) + butcher_a::Matrix{Float64} + butcher_b::Vector{Float64} + butcher_c::Vector{Float64} + info::String + + Midpoint() = new(1, 0, hcat(0.5), [1], [0.5], "Implicit Midpoint aka Gauss-Legendre collocation for s=1, 2nd order, symplectic") end """ @@ -40,7 +46,7 @@ function get_variables_at_time_step(xu, docp::DOCP{Midpoint}, i) if i < N offset_u = offset else - offset_u = (nx * 2 + m) * (i - 1) + offset_u = (nx * (1 + docp.discretization.stage) + m) * (i - 1) end if m == 1 ui = xu[offset_u + nx + 1] @@ -66,7 +72,7 @@ function get_NLP_variables_at_time_step(xu, docp, i, disc::Midpoint) nx = docp.dim_NLP_x m = docp.dim_NLP_u N = docp.dim_NLP_steps - offset = (nx * 2 + m) * i + offset = (nx * (1 + docp.discretization.stage) + m) * i # state xi = xu[(offset + 1):(offset + nx)] @@ -74,7 +80,7 @@ function get_NLP_variables_at_time_step(xu, docp, i, disc::Midpoint) if i < N offset_u = offset else - offset_u = (nx * 2 + m) * (i - 1) + offset_u = (nx * (1 + docp.discretization.stage) + m) * (i - 1) end ui = xu[(offset_u + nx + 1):(offset_u + nx + m)] # stage @@ -92,7 +98,7 @@ function set_variables_at_time_step!(xu, x_init, u_init, docp, i, disc::Midpoint n = docp.dim_OCP_x m = docp.dim_NLP_u N = docp.dim_NLP_steps - offset = (nx * 2 + m) * i + offset = (nx * (1 + docp.discretization.stage) + m) * i # NB. only set the actual state variables from the OCP # - skip the possible additional state for lagrange cost @@ -155,9 +161,9 @@ $(TYPEDSIGNATURES) Set the constraints corresponding to the state equation """ function setStateEquation!(docp::DOCP{Midpoint}, c, index::Int, args, v, i) + ocp = docp.ocp - - # +++ later use butcher table in struct ? + disc = docp.discretization # variables ti = args.time @@ -171,16 +177,21 @@ function setStateEquation!(docp::DOCP{Midpoint}, c, index::Int, args, v, i) hi = tip1 - ti # midpoint rule - @. c[index:(index + docp.dim_OCP_x - 1)] = xip1 - (xi + hi * ki[1:(docp.dim_OCP_x)]) + h_sum_bk = hi * disc.butcher_b[1] * ki[1:docp.dim_NLP_x] + c[index:(index + docp.dim_OCP_x - 1)] .= xip1 .- (xi .+ h_sum_bk[1:docp.dim_OCP_x]) # +++ just define extended dynamics ! if docp.has_lagrange - c[index + docp.dim_OCP_x] = xlip1 - (xli + hi * ki[end]) + c[index + docp.dim_OCP_x] = xlip1 - (xli + h_sum_bk[end]) end index += docp.dim_NLP_x # stage equation at mid-step - t_s = 0.5 * (ti + tip1) - x_s = 0.5 * (xi + xip1) + t_s = ti + hi * disc.butcher_c[1] + if docp.dim_OCP_x == 1 + x_s = xi + hi * disc.butcher_a[1][1] * ki[1] #FFS + else + x_s = xi .+ hi .* (disc.butcher_a[1][1] .* ki[1:docp.dim_OCP_x]) + end c[index:(index + docp.dim_OCP_x - 1)] .= ki[1:(docp.dim_OCP_x)] .- ocp.dynamics(t_s, x_s, ui, v) # +++ just define extended dynamics ! if docp.has_lagrange diff --git a/src/trapeze.jl b/src/trapeze.jl index 18878300..f2b5c8ef 100644 --- a/src/trapeze.jl +++ b/src/trapeze.jl @@ -3,11 +3,14 @@ Internal layout for NLP variables: [X_0,U_0, X_1,U_1, .., X_N,U_N, V] =# +# NB. could be defined as a generic IRK struct Trapeze <: Discretization + stage::Int - additional_controls::Int - # add control at tf - Trapeze() = new(0, 1) + additional_controls::Int # add control at tf + info::String + + Trapeze() = new(0, 1, "Implicit Trapeze aka Crank-Nicolson, 2nd order, A-stable") end """ diff --git a/test/Project.toml b/test/Project.toml index ed0bdd43..ca19863c 100644 --- a/test/Project.toml +++ b/test/Project.toml @@ -13,7 +13,7 @@ Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" [compat] CTBase = "0.13" HSL = "0.4" -JLD2 = "0.5" +JLD2 = "0.4" # 0.5 has ugly warnings for functions JSON3 = "1" MadNLP = "0.8" NLPModelsIpopt = "0.10"