added failsafe in solution generation to ensure that t0 > tf (#76)

* added failsafe in solution generation to ensure that t0 > tf

* some reorg in test scripts

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))

test/suite/abstract_ocp.jl

@@ -0,0 +1,48 @@
+using CTDirect
+
+println("Test: abstract OCP 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=0, tol=1e-12)
+    @test sol2.objective ≈ 5.46 rtol=1e-2
+end

test/suite/goddard_all_constraints.jl → 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)
@@ -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

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

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)

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)