From 1b432373d2b4500c430a7e6b373ea7fdbb41aaa6 Mon Sep 17 00:00:00 2001 From: Olivier Cots Date: Sun, 25 Aug 2024 11:35:52 +0200 Subject: [PATCH] fix merge --- ext/CTDirectExt.jl | 11 ++- src/solution.jl | 146 +++++++++++++++++++++++++------ src/utils.jl | 8 +- test/suite/test_constraints.jl | 81 +++++++++-------- test/suite/test_initial_guess.jl | 49 ++++++++--- test/suite/test_misc.jl | 2 +- test/suite/test_nlp.jl | 16 ++-- 7 files changed, 225 insertions(+), 88 deletions(-) diff --git a/ext/CTDirectExt.jl b/ext/CTDirectExt.jl index 52ba94f..b1b08d6 100644 --- a/ext/CTDirectExt.jl +++ b/ext/CTDirectExt.jl @@ -32,8 +32,11 @@ $(TYPEDSIGNATURES) Export OCP solution in JSON format """ -function CTDirect.export_ocp_solution(sol::OptimalControlSolution; filename_prefix="solution") -# +++ redo this, start with basics +function CTDirect.export_ocp_solution( + sol::OptimalControlSolution; + filename_prefix = "solution", +) + # +++ redo this, start with basics open(filename_prefix * ".json", "w") do io #JSON3.pretty(io, CTDirect.OCPDiscreteSolution(sol)) end @@ -45,8 +48,8 @@ $(TYPEDSIGNATURES) Read OCP solution in JSON format """ -function CTDirect.import_ocp_solution(filename_prefix="solution") -# +++ add constructor from json blob +function CTDirect.import_ocp_solution(filename_prefix = "solution") + # +++ add constructor from json blob json_string = read(filename_prefix * ".json", String) return OptimalControlSolution(JSON3.read(json_string)) end diff --git a/src/solution.jl b/src/solution.jl index 0ccc36a..f5f6559 100644 --- a/src/solution.jl +++ b/src/solution.jl @@ -262,11 +262,23 @@ $(TYPEDSIGNATURES) Build OCP functional solution from DOCP vector solution (given as raw variables and multipliers plus some optional infos) """ -function CTBase.OptimalControlSolution(docp, T, X, U, v, P; - objective=0, iterations=0, constraints_violation=0, message="No msg", stopping=nothing, success=nothing, - constraints_types=(nothing,nothing,nothing,nothing,nothing), - constraints_mult=(nothing,nothing,nothing,nothing,nothing), - box_multipliers=((nothing,nothing),(nothing,nothing),(nothing,nothing))) +function CTBase.OptimalControlSolution( + docp, + T, + X, + U, + v, + P; + objective = 0, + iterations = 0, + constraints_violation = 0, + message = "No msg", + stopping = nothing, + success = nothing, + constraints_types = (nothing, nothing, nothing, nothing, nothing), + constraints_mult = (nothing, nothing, nothing, nothing, nothing), + box_multipliers = ((nothing, nothing), (nothing, nothing), (nothing, nothing)), +) ocp = docp.ocp dim_x = state_dimension(ocp) @@ -300,29 +312,86 @@ function CTBase.OptimalControlSolution(docp, T, X, U, v, P; # +++ put interpolations here directly and reuse vectors ? # nonlinear constraints and multipliers - (control_constraints, state_constraints, mixed_constraints, boundary_constraints, variable_constraints, mult_control_constraints, mult_state_constraints, mult_mixed_constraints, mult_boundary_constraints, mult_variable_constraints) = set_constraints_and_multipliers(T, constraints_types, constraints_mult) + ( + control_constraints, + state_constraints, + mixed_constraints, + boundary_constraints, + variable_constraints, + mult_control_constraints, + mult_state_constraints, + mult_mixed_constraints, + mult_boundary_constraints, + mult_variable_constraints, + ) = set_constraints_and_multipliers(T, constraints_types, constraints_mult) # box constraints multipliers - (mult_state_box_lower, mult_state_box_upper, mult_control_box_lower, mult_control_box_upper, mult_variable_box_lower, mult_variable_box_upper) = set_box_multipliers(T, box_multipliers, dim_x, dim_u) + ( + mult_state_box_lower, + mult_state_box_upper, + mult_control_box_lower, + mult_control_box_upper, + mult_variable_box_lower, + mult_variable_box_upper, + ) = set_box_multipliers(T, box_multipliers, dim_x, dim_u) # build and return solution if docp.has_variable - return OptimalControlSolution(ocp; - state=fx, control=fu, objective=objective, costate=fp, time_grid=T, variable=var, iterations=iterations, stopping=stopping, message=message, success=success, infos=infos, control_constraints=control_constraints, state_constraints=state_constraints, mixed_constraints=mixed_constraints, boundary_constraints=boundary_constraints, variable_constraints=variable_constraints, - mult_control_constraints=mult_control_constraints, mult_state_constraints=mult_state_constraints, mult_mixed_constraints=mult_mixed_constraints, mult_boundary_constraints=mult_boundary_constraints, mult_variable_constraints=mult_variable_constraints, - mult_state_box_lower=mult_state_box_lower, - mult_state_box_upper=mult_state_box_upper, - mult_control_box_lower=mult_control_box_lower, - mult_control_box_upper=mult_control_box_upper, - mult_variable_box_lower=mult_variable_box_lower, - mult_variable_box_upper=mult_variable_box_upper) + return OptimalControlSolution( + ocp; + state = fx, + control = fu, + objective = objective, + costate = fp, + time_grid = T, + variable = var, + iterations = iterations, + stopping = stopping, + message = message, + success = success, + infos = infos, + control_constraints = control_constraints, + state_constraints = state_constraints, + mixed_constraints = mixed_constraints, + boundary_constraints = boundary_constraints, + variable_constraints = variable_constraints, + mult_control_constraints = mult_control_constraints, + mult_state_constraints = mult_state_constraints, + mult_mixed_constraints = mult_mixed_constraints, + mult_boundary_constraints = mult_boundary_constraints, + mult_variable_constraints = mult_variable_constraints, + mult_state_box_lower = mult_state_box_lower, + mult_state_box_upper = mult_state_box_upper, + mult_control_box_lower = mult_control_box_lower, + mult_control_box_upper = mult_control_box_upper, + mult_variable_box_lower = mult_variable_box_lower, + mult_variable_box_upper = mult_variable_box_upper, + ) else - return OptimalControlSolution(ocp; - state=fx, control=fu, objective=objective, costate=fp, time_grid=T, iterations=iterations, stopping=stopping, message=message, success=success, infos=infos, control_constraints=control_constraints, state_constraints=state_constraints, mixed_constraints=mixed_constraints, boundary_constraints=boundary_constraints, - mult_control_constraints=mult_control_constraints, mult_state_constraints=mult_state_constraints, mult_mixed_constraints=mult_mixed_constraints, mult_boundary_constraints=mult_boundary_constraints, - mult_state_box_lower=mult_state_box_lower, - mult_state_box_upper=mult_state_box_upper, - mult_control_box_lower=mult_control_box_lower, - mult_control_box_upper=mult_control_box_upper) + return OptimalControlSolution( + ocp; + state = fx, + control = fu, + objective = objective, + costate = fp, + time_grid = T, + iterations = iterations, + stopping = stopping, + message = message, + success = success, + infos = infos, + control_constraints = control_constraints, + state_constraints = state_constraints, + mixed_constraints = mixed_constraints, + boundary_constraints = boundary_constraints, + mult_control_constraints = mult_control_constraints, + mult_state_constraints = mult_state_constraints, + mult_mixed_constraints = mult_mixed_constraints, + mult_boundary_constraints = mult_boundary_constraints, + mult_state_box_lower = mult_state_box_lower, + mult_state_box_upper = mult_state_box_upper, + mult_control_box_lower = mult_control_box_lower, + mult_control_box_upper = mult_control_box_upper, + ) end end @@ -351,7 +420,18 @@ function set_constraints_and_multipliers(T, constraints_types, constraints_mult) variable_constraints = constraints_types[5] mult_variable_constraints = constraints_mult[5] - return (control_constraints, state_constraints, mixed_constraints, boundary_constraints, variable_constraints, mult_control_constraints, mult_state_constraints, mult_mixed_constraints, mult_boundary_constraints, mult_variable_constraints) + return ( + control_constraints, + state_constraints, + mixed_constraints, + boundary_constraints, + variable_constraints, + mult_control_constraints, + mult_state_constraints, + mult_mixed_constraints, + mult_boundary_constraints, + mult_variable_constraints, + ) end @@ -365,11 +445,19 @@ function set_box_multipliers(T, box_multipliers, dim_x, dim_u) # state box mult_state_box_lower, mult_state_box_upper = set_box_block(T, box_multipliers[1], dim_x) # control box - mult_control_box_lower, mult_control_box_upper = set_box_block(T, box_multipliers[2], dim_u) + mult_control_box_lower, mult_control_box_upper = + set_box_block(T, box_multipliers[2], dim_u) # variable box mult_variable_box_lower, mult_variable_box_upper = box_multipliers[3] - return (mult_state_box_lower, mult_state_box_upper, mult_control_box_lower, mult_control_box_upper, mult_variable_box_lower, mult_variable_box_upper) + return ( + mult_state_box_lower, + mult_state_box_upper, + mult_control_box_lower, + mult_control_box_upper, + mult_variable_box_lower, + mult_variable_box_upper, + ) end """ @@ -381,10 +469,10 @@ Process data related to a box type for solution building function set_box_block(T, mults, dim) mult_l, mult_u = mults if !isnothing(mult_l) && !isnothing(mult_u) - m_l = ctinterpolate(T, matrix2vec(mult_l[:,1:dim], 1)) - m_u = ctinterpolate(T, matrix2vec(mult_u[:,1:dim], 1)) + m_l = ctinterpolate(T, matrix2vec(mult_l[:, 1:dim], 1)) + m_u = ctinterpolate(T, matrix2vec(mult_u[:, 1:dim], 1)) end - return t -> m_l(t), t -> m_u(t) + return t -> m_l(t), t -> m_u(t) end diff --git a/src/utils.jl b/src/utils.jl index e50eed6..7c870b3 100644 --- a/src/utils.jl +++ b/src/utils.jl @@ -270,7 +270,13 @@ function DOCP_initial_guess(docp, init::OptimalControlInit = OptimalControlInit( # set state / control variables if provided for i = 0:docp.dim_NLP_steps ti = get_time_at_time_step(NLP_X, docp, i) - set_variables_at_time_step!(NLP_X, init.state_init(ti), init.control_init(ti), docp, i) + set_variables_at_time_step!( + NLP_X, + init.state_init(ti), + init.control_init(ti), + docp, + i, + ) end return NLP_X diff --git a/test/suite/test_constraints.jl b/test/suite/test_constraints.jl index dcc8c0f..405ff9e 100644 --- a/test/suite/test_constraints.jl +++ b/test/suite/test_constraints.jl @@ -31,7 +31,12 @@ if check_constraint_mult # variable constraints / box println("\nVariable constraints: ", sol.variable_constraints) println("multipliers: ", sol.mult_variable_constraints) - println("\nVariable box multipliers LB: ", sol.mult_variable_box_lower, " UB ", sol.mult_variable_box_upper) + println( + "\nVariable box multipliers LB: ", + sol.mult_variable_box_lower, + " UB ", + sol.mult_variable_box_upper, + ) # PATH CONSTRAINTS # state box @@ -55,9 +60,9 @@ if check_constraint_mult u = sol.control.(T) u_box_lb = flatten(sol.mult_control_box_lower.(T)) u_box_ub = flatten(sol.mult_control_box_upper.(T)) - p4_a = plot(T, u, label="u") - p4_b = plot(T, [u_box_lb u_box_ub], label=["LB" "UB"]) - p4 = plot(p4_a, p4_b, layout=(2,1)) + p4_a = plot(T, u, label = "u") + p4_b = plot(T, [u_box_lb u_box_ub], label = ["LB" "UB"]) + p4 = plot(p4_a, p4_b, layout = (2, 1)) p_box = plot( p1, @@ -74,47 +79,53 @@ if check_constraint_mult # control constraints c_u = flatten(sol.control_constraints.(T)) m_c_u = flatten(sol.mult_control_constraints.(T)) - p5_a = plot(T, c_u, label="c_u") - p5_b = plot(T, m_c_u, label="mul") - p5 = plot(p5_a, p5_b, layout=(2,1)) + p5_a = plot(T, c_u, label = "c_u") + p5_b = plot(T, m_c_u, label = "mul") + p5 = plot(p5_a, p5_b, layout = (2, 1)) # state constraints c_x = flatten(sol.state_constraints.(T)) m_c_x = flatten(sol.mult_state_constraints.(T)) - p6_a = plot(T, c_x, label="c_x") - p6_b = plot(T, m_c_x, label="mul") - p6 = plot(p6_a, p6_b, layout=(2,1)) + p6_a = plot(T, c_x, label = "c_x") + p6_b = plot(T, m_c_x, label = "mul") + p6 = plot(p6_a, p6_b, layout = (2, 1)) # mixed constraints c_xu = flatten(sol.mixed_constraints.(T)) m_c_xu = flatten(sol.mult_mixed_constraints.(T)) - p7_a = plot(T, c_xu, label="c_xu") - p7_b = plot(T, m_c_xu, label="mul") - p7 = plot(p7_a, p7_b, layout=(2,1)) - - p_cons = plot(p5, p6, p7, layout=(1,3), title=["control cons" "" "state cons" "" "mixed cons" ""]) + p7_a = plot(T, c_xu, label = "c_xu") + p7_b = plot(T, m_c_xu, label = "mul") + p7 = plot(p7_a, p7_b, layout = (2, 1)) + + p_cons = plot( + p5, + p6, + p7, + layout = (1, 3), + title = ["control cons" "" "state cons" "" "mixed cons" ""], + ) else -# box constraints -@testset verbose = true showtiming = true ":goddard :box_constraints" begin - ocp = goddard() - sol = direct_solve(ocp.ocp, display=false) - @test sol.objective ≈ ocp.obj rtol=1e-2 -end - -# functional constraints -@testset verbose = true showtiming = true ":goddard :functional_constraints" begin - ocp = goddard(functional_constraints=true) - sol = direct_solve(ocp.ocp, display=false, init=ocp.init) - @test sol.objective ≈ ocp.obj rtol=1e-2 -end + # box constraints + @testset verbose = true showtiming = true ":goddard :box_constraints" begin + ocp = goddard() + sol = direct_solve(ocp.ocp, display = false) + @test sol.objective ≈ ocp.obj rtol = 1e-2 + end + + # functional constraints + @testset verbose = true showtiming = true ":goddard :functional_constraints" begin + ocp = goddard(functional_constraints = true) + sol = direct_solve(ocp.ocp, display = false, init = ocp.init) + @test sol.objective ≈ ocp.obj rtol = 1e-2 + end + + # all constraints + @testset verbose = true showtiming = true ":goddard :all_constraints" begin + ocp = goddard_all() + sol = direct_solve(ocp.ocp, display = false, init = ocp.init) + @test sol.objective ≈ ocp.obj rtol = 1e-2 + end -# all constraints -@testset verbose = true showtiming = true ":goddard :all_constraints" begin - ocp = goddard_all() - sol = direct_solve(ocp.ocp, display=false, init=ocp.init) - @test sol.objective ≈ ocp.obj rtol=1e-2 end - -end \ No newline at end of file diff --git a/test/suite/test_initial_guess.jl b/test/suite/test_initial_guess.jl index a74d762..e25fe13 100644 --- a/test/suite/test_initial_guess.jl +++ b/test/suite/test_initial_guess.jl @@ -105,16 +105,24 @@ end # 1. functional initial guess @testset verbose = true showtiming = true ":functional_x" begin - sol = direct_solve(ocp, display=false, init=(state=x_func,), max_iter=maxiter) + sol = direct_solve(ocp, display = false, init = (state = x_func,), max_iter = maxiter) @test(check_xf(sol, x_func(sol.time_grid[end]))) end @testset verbose = true showtiming = true ":functional_u" begin - sol = direct_solve(ocp, display=false, init=(control=u_func,), max_iter=maxiter) + sol = direct_solve(ocp, display = false, init = (control = u_func,), max_iter = maxiter) @test(check_uf(sol, u_func(sol.time_grid[end]))) end @testset verbose = true showtiming = true ":functional_xu" begin - sol = direct_solve(ocp, display=false, init=(state=x_func, control=u_func), max_iter=maxiter) - @test(check_xf(sol, x_func(sol.time_grid[end])) && check_uf(sol, u_func(sol.time_grid[end]))) + sol = direct_solve( + ocp, + display = false, + init = (state = x_func, control = u_func), + max_iter = maxiter, + ) + @test( + check_xf(sol, x_func(sol.time_grid[end])) && + check_uf(sol, u_func(sol.time_grid[end])) + ) end # 1.d interpolated initial guess @@ -150,14 +158,27 @@ end # 1.e mixed initial guess @testset verbose = true showtiming = true ":vector_tx :functional_u :constant_v" begin - sol = direct_solve(ocp, display=false, init=(time=t_vec, state=x_vec, control=u_func, variable=v_const), max_iter=maxiter) - @test(check_xf(sol, x_vec[end]) && check_uf(sol, u_func(sol.time_grid[end])) && check_v(sol, v_const)) + sol = direct_solve( + ocp, + display = false, + init = (time = t_vec, state = x_vec, control = u_func, variable = v_const), + max_iter = maxiter, + ) + @test( + check_xf(sol, x_vec[end]) && + check_uf(sol, u_func(sol.time_grid[end])) && + check_v(sol, v_const) + ) end # 1.f warm start @testset verbose = true showtiming = true ":warm_start" begin - sol = direct_solve(ocp, display=false, init=sol0, max_iter=maxiter) - @test(check_xf(sol, sol.state(sol.time_grid[end])) && check_uf(sol, sol.control(sol.time_grid[end])) && check_v(sol, sol.variable)) + sol = direct_solve(ocp, display = false, init = sol0, max_iter = maxiter) + @test( + check_xf(sol, sol.state(sol.time_grid[end])) && + check_uf(sol, sol.control(sol.time_grid[end])) && + check_v(sol, sol.variable) + ) end ################################################# @@ -173,12 +194,20 @@ tag = CTDirect.IpoptTag() ) dsol = CTDirect.solve_docp(tag, docp, nlp, display = false, max_iter = maxiter) sol = OptimalControlSolution(docp, dsol) - @test(check_xf(sol, x_vec[end]) && check_uf(sol, u_func(sol.time_grid[end])) && check_v(sol, v_const)) + @test( + check_xf(sol, x_vec[end]) && + check_uf(sol, u_func(sol.time_grid[end])) && + check_v(sol, v_const) + ) end # warm start @testset verbose = true showtiming = true ":docp_warm_start" begin set_initial_guess(docp, nlp, sol0) dsol = CTDirect.solve_docp(tag, docp, nlp, display = false, max_iter = maxiter) sol = OptimalControlSolution(docp, dsol) - @test(check_xf(sol, sol.state(sol.time_grid[end])) && check_uf(sol, sol.control(sol.time_grid[end])) && check_v(sol, sol.variable)) + @test( + check_xf(sol, sol.state(sol.time_grid[end])) && + check_uf(sol, sol.control(sol.time_grid[end])) && + check_v(sol, sol.variable) + ) end diff --git a/test/suite/test_misc.jl b/test/suite/test_misc.jl index 4b8e613..b430b63 100644 --- a/test/suite/test_misc.jl +++ b/test/suite/test_misc.jl @@ -36,4 +36,4 @@ end sol_reloaded = import_ocp_solution("solution_test") @test sol0.objective == sol_reloaded.objective end -=# \ No newline at end of file +=# diff --git a/test/suite/test_nlp.jl b/test/suite/test_nlp.jl index 474b775..884aa80 100644 --- a/test/suite/test_nlp.jl +++ b/test/suite/test_nlp.jl @@ -45,17 +45,17 @@ u_opt = t -> 6 - 12 * t p_opt = t -> [24, 12 - 24 * t] @testset verbose = true showtiming = true ":analytic_solution :ipopt" begin - sol = direct_solve(ocp, display=false) + sol = direct_solve(ocp, display = false) T = sol.time_grid - @test isapprox(x_opt.(T), sol.state.(T), rtol=1e-2) - @test isapprox(u_opt.(T), sol.control.(T), rtol=1e-2) - @test isapprox(p_opt.(T), sol.costate.(T), rtol=1e-2) + @test isapprox(x_opt.(T), sol.state.(T), rtol = 1e-2) + @test isapprox(u_opt.(T), sol.control.(T), rtol = 1e-2) + @test isapprox(p_opt.(T), sol.costate.(T), rtol = 1e-2) end @testset verbose = true showtiming = true ":analytic_solution :madnlp" begin - sol = direct_solve(ocp, :madnlp, display=false) + sol = direct_solve(ocp, :madnlp, display = false) T = sol.time_grid - @test isapprox(x_opt.(T), sol.state.(T), rtol=1e-2) - @test isapprox(u_opt.(T), sol.control.(T), rtol=1e-2) - @test isapprox(p_opt.(T), sol.costate.(T), rtol=1e-2) + @test isapprox(x_opt.(T), sol.state.(T), rtol = 1e-2) + @test isapprox(u_opt.(T), sol.control.(T), rtol = 1e-2) + @test isapprox(p_opt.(T), sol.costate.(T), rtol = 1e-2) end