From cd8c80d2d0c732c2ba72167c5a50dd4c438fa870 Mon Sep 17 00:00:00 2001 From: Pierre Martinon Date: Tue, 9 Apr 2024 15:32:26 +0200 Subject: [PATCH 1/2] added failsafe in solution generation to ensure that t0 > tf --- src/solution.jl | 2 +- test/suite/abstract_ocp.jl | 48 ++++++++++++++++++++++++++ test/suite/double_integrator.jl | 20 ----------- test/suite/goddard_all_constraints.jl | 24 ++++++------- test/test_basic.jl | 46 ++++++++++++------------- test/test_init.jl | 49 +++++++++++++++++++++++++++ 6 files changed, 133 insertions(+), 56 deletions(-) create mode 100644 test/suite/abstract_ocp.jl create mode 100644 test/test_init.jl diff --git a/src/solution.jl b/src/solution.jl index 56b3f3d..45c89ae 100644 --- a/src/solution.jl +++ b/src/solution.jl @@ -20,7 +20,7 @@ function OCPSolutionFromDOCP(ipopt_solution, docp) # variables and misc infos N = docp.dim_NLP_steps t0 = get_initial_time(docp.NLP_solution, docp) - tf = get_final_time(docp.NLP_solution, docp) + tf = max(get_final_time(docp.NLP_solution, docp), t0 + 1e-9) T = collect(LinRange(t0, tf, N+1)) x = ctinterpolate(T, matrix2vec(X, 1)) u = ctinterpolate(T, matrix2vec(U, 1)) diff --git a/test/suite/abstract_ocp.jl b/test/suite/abstract_ocp.jl new file mode 100644 index 0000000..0869796 --- /dev/null +++ b/test/suite/abstract_ocp.jl @@ -0,0 +1,48 @@ +using CTDirect + +println("Test: abstract definition") + +# double integrator min tf, abstract definition +@def ocp1 begin + tf ∈ R, variable + t ∈ [ 0, tf ], time + x ∈ R², state + u ∈ R, control + -1 ≤ u(t) ≤ 1 + x(0) == [ 0, 0 ] + x(tf) == [ 1, 0 ] + 0.1 ≤ tf ≤ Inf + ẋ(t) == [ x₂(t), u(t) ] + tf → min +end + +@testset verbose = true showtiming = true ":double_integrator :min_tf :abstract" begin + sol1 = solveDirect(ocp1, grid_size=100, print_level=0, tol=1e-12) + @test sol1.objective ≈ 2.0 rtol=1e-2 +end + +# same with some random constraints +@def ocp2 begin + tf ∈ R, variable + t ∈ [ 0, tf ], time + x ∈ R², state + u ∈ R, control + tf ≥ 0.1 + -1 ≤ u(t) ≤ 1 + q = x₁ + v = x₂ + q(0) == 1 + v(0) == 2 + q(tf) == 0 + v(tf) == 0 + 0 ≤ q(t) ≤ 5 + -2 ≤ v(t) ≤ 3 + (u^2)(t) ≤ 100 + ẋ(t) == [ v(t), u(t) ] + tf → min +end + +@testset verbose = true showtiming = true ":double_integrator :min_tf :abstract :constraints" begin + sol2 = solveDirect(ocp2, grid_size=100, print_level=5, tol=1e-12) + @test sol2.objective ≈ 5.46 rtol=1e-2 +end \ No newline at end of file diff --git a/test/suite/double_integrator.jl b/test/suite/double_integrator.jl index 25d8a75..5cae709 100644 --- a/test/suite/double_integrator.jl +++ b/test/suite/double_integrator.jl @@ -92,23 +92,3 @@ objective!(ocp5, :lagrange, (x, u) -> u[1]*u[1] + u[2]*u[2]) sol5 = solveDirect(ocp5, grid_size=50, print_level=0, tol=1e-12) @test sol5.objective ≈ 9.6e-2 rtol=1e-2 end - -# min tf, abstract definition -@def ocp begin - tf ∈ R, variable - t ∈ [ 0, tf ], time - x ∈ R², state - u ∈ R, control - -1 ≤ u(t) ≤ 1 - x(0) == [ 0, 0 ] - x(tf) == [ 1, 0 ] - 0.1 ≤ tf ≤ Inf - ẋ(t) == [ x₂(t), u(t) ] - tf → min -end - -@testset verbose = true showtiming = true ":double_integrator :min_tf :abstract" begin - @test is_solvable(ocp) - sol = solveDirect(ocp, grid_size=100, print_level=0, tol=1e-12) - @test sol.objective ≈ 2.0 rtol=1e-2 -end \ No newline at end of file diff --git a/test/suite/goddard_all_constraints.jl b/test/suite/goddard_all_constraints.jl index 16a4185..848492f 100644 --- a/test/suite/goddard_all_constraints.jl +++ b/test/suite/goddard_all_constraints.jl @@ -50,7 +50,7 @@ function F1(x) end dynamics!(ocp, (x, u, v) -> F0(x) + u*F1(x) ) -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints" begin sol1 = solveDirect(ocp, grid_size=100, print_level=0, tol=1e-8) @test sol1.objective ≈ 1.0125 rtol=1e-2 end @@ -60,64 +60,64 @@ x_init = [1.05, 0.1, 0.8] u_init = 0.5 v_init = 0.1 -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_constant" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_constant" begin init_constant = OptimalControlInit(x_init=x_init, u_init=u_init, v_init=v_init) sol2 = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=init_constant) @test sol2.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_function_x" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_function_x" begin init_function_x = OptimalControlInit(x_init=t->x_init, u_init=u_init, v_init=v_init) sol = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=init_function_x) @test sol.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_function_u" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_function_u" begin init_function_u = OptimalControlInit(x_init=x_init, u_init=t->u_init, v_init=v_init) sol = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=init_function_u) @test sol.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_function_xu" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_function_xu" begin init_function_xu = OptimalControlInit(x_init=t->x_init, u_init=t->u_init, v_init=v_init) sol = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=init_function_xu) @test sol.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_constant (x)" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_constant (x)" begin sol3 = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=OptimalControlInit(x_init=x_init)) @test sol3.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_constant (u)" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_constant (u)" begin sol4 = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=OptimalControlInit(u_init=u_init)) @test sol4.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_constant (v)" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_constant (v)" begin sol5 = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=OptimalControlInit(v_init=v_init)) @test sol5.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_constant (x,u)" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_constant (x,u)" begin sol6 = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=OptimalControlInit(x_init=x_init, u_init=u_init)) @test sol6.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_constant (x,v)" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_constant (x,v)" begin sol7 = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=OptimalControlInit(x_init=x_init, v_init=v_init)) @test sol7.objective ≈ 1.0125 rtol=1e-2 end -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_constant (u,v)" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_constant (u,v)" begin sol8 = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=OptimalControlInit(u_init=u_init, v_init=v_init)) @test sol8.objective ≈ 1.0125 rtol=1e-2 end # with initial guess from solution sol = solveDirect(ocp, grid_size=100, print_level=0, tol=1e-8) -@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints_types :init_sol" begin +@testset verbose = true showtiming = true ":goddard :max_rf :all_constraints :init_sol" begin init_sol = OptimalControlInit(sol) sol9 = solveDirect(ocp, grid_size=30, print_level=0, tol=1e-8, init=init_sol) @test sol9.objective ≈ 1.0125 rtol=1e-2 diff --git a/test/test_basic.jl b/test/test_basic.jl index 1bf91ac..886e211 100644 --- a/test/test_basic.jl +++ b/test/test_basic.jl @@ -24,26 +24,26 @@ println("Solve discretized problem and retrieve solution") sol = solveDOCP(docp, print_level=5, tol=1e-12) println("Expected Objective 0.313, found ", sol.objective) -# different starting guess -println("with constant init x=0.5 and u=0") -init_constant = OptimalControlInit(x_init=[-0.5], u_init=0) -setDOCPInit(docp, init_constant) -sol = solveDOCP(docp, print_level=5, tol=1e-12) -println("Expected Objective 0.313, found ", sol.objective) - -# init from solution -init_sol = OptimalControlInit(sol) -setDOCPInit(docp, init_sol) -sol = solveDOCP(docp, print_level=5, tol=1e-12) -println("Expected Objective 0.313, found ", sol.objective) - -# pass init directly to solve call -setDOCPInit(docp, OptimalControlInit()) # reset init in docp -sol = solveDOCP(docp, init=init_sol, print_level=5, tol=1e-12) -println("Expected Objective 0.313, found ", sol.objective) -sol = solveDOCP(docp, print_level=5, tol=1e-12) -println("Expected Objective 0.313, found ", sol.objective) - - -# check types on objective and constraints functions -#@code_warntype ipopt_objective(xu, docp) +# fail test +#sol = solveDirect(ocp, grid_size=100, print_level=5, tol=1e-12, max_iter=1) ok + +@def ocp2 begin + tf ∈ R, variable + t ∈ [ 0, tf ], time + x ∈ R², state + u ∈ R, control + tf ≥ 0 + -1 ≤ u(t) ≤ 1 + q = x₁ + v = x₂ + q(0) == 1 + v(0) == 2 + q(tf) == 0 + v(tf) == 0 + 0 ≤ q(t) ≤ 5 + -2 ≤ v(t) ≤ 3 + (u^2)(t) ≤ 100 + ẋ(t) == [ v(t), u(t) ] + tf → min +end +sol = solveDirect(ocp2, grid_size=100, print_level=5, tol=1e-12) diff --git a/test/test_init.jl b/test/test_init.jl new file mode 100644 index 0000000..1bf91ac --- /dev/null +++ b/test/test_init.jl @@ -0,0 +1,49 @@ +using CTDirect + +# simple integrator min energy +ocp = Model() +state!(ocp, 1) +control!(ocp, 1) +time!(ocp, [0, 1]) +constraint!(ocp, :initial, -1, :initial_constraint) +constraint!(ocp, :final, 0, :final_constraint) +dynamics!(ocp, (x, u) -> -x + u) +objective!(ocp, :lagrange, (x, u) -> u^2) + +# all-in-one solve call +println("Test simple integrator: all in one solve call") +sol = solveDirect(ocp, grid_size=100, print_level=5, tol=1e-12) +println("Expected Objective 0.313, found ", sol.objective) + +# split calls +println("Test simple integrator: split calls") +println("Direct transcription") +docp = directTranscription(ocp, grid_size=100) +nlp = getNLP(docp) +println("Solve discretized problem and retrieve solution") +sol = solveDOCP(docp, print_level=5, tol=1e-12) +println("Expected Objective 0.313, found ", sol.objective) + +# different starting guess +println("with constant init x=0.5 and u=0") +init_constant = OptimalControlInit(x_init=[-0.5], u_init=0) +setDOCPInit(docp, init_constant) +sol = solveDOCP(docp, print_level=5, tol=1e-12) +println("Expected Objective 0.313, found ", sol.objective) + +# init from solution +init_sol = OptimalControlInit(sol) +setDOCPInit(docp, init_sol) +sol = solveDOCP(docp, print_level=5, tol=1e-12) +println("Expected Objective 0.313, found ", sol.objective) + +# pass init directly to solve call +setDOCPInit(docp, OptimalControlInit()) # reset init in docp +sol = solveDOCP(docp, init=init_sol, print_level=5, tol=1e-12) +println("Expected Objective 0.313, found ", sol.objective) +sol = solveDOCP(docp, print_level=5, tol=1e-12) +println("Expected Objective 0.313, found ", sol.objective) + + +# check types on objective and constraints functions +#@code_warntype ipopt_objective(xu, docp) From e674770cbc6a1f72468d16ad891e78ee1d5c197e Mon Sep 17 00:00:00 2001 From: Pierre Martinon Date: Wed, 10 Apr 2024 11:14:18 +0200 Subject: [PATCH 2/2] some reorg in test scripts --- test/suite/abstract_ocp.jl | 4 ++-- .../{goddard_all_constraints.jl => all_constraints_types.jl} | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) rename test/suite/{goddard_all_constraints.jl => all_constraints_types.jl} (99%) diff --git a/test/suite/abstract_ocp.jl b/test/suite/abstract_ocp.jl index 0869796..ecddb88 100644 --- a/test/suite/abstract_ocp.jl +++ b/test/suite/abstract_ocp.jl @@ -1,6 +1,6 @@ using CTDirect -println("Test: abstract definition") +println("Test: abstract OCP definition") # double integrator min tf, abstract definition @def ocp1 begin @@ -43,6 +43,6 @@ end end @testset verbose = true showtiming = true ":double_integrator :min_tf :abstract :constraints" begin - sol2 = solveDirect(ocp2, grid_size=100, print_level=5, tol=1e-12) + sol2 = solveDirect(ocp2, grid_size=100, print_level=0, tol=1e-12) @test sol2.objective ≈ 5.46 rtol=1e-2 end \ No newline at end of file diff --git a/test/suite/goddard_all_constraints.jl b/test/suite/all_constraints_types.jl similarity index 99% rename from test/suite/goddard_all_constraints.jl rename to test/suite/all_constraints_types.jl index 848492f..2793e0d 100644 --- a/test/suite/goddard_all_constraints.jl +++ b/test/suite/all_constraints_types.jl @@ -1,6 +1,6 @@ using CTDirect -println("Test: goddard all constraints") +println("Test: all constraint types") # goddard max final altitue (all constraint types formulation) ocp = Model(variable=true)