Merge pull request #107 from control-toolbox/init_for_direct

Init for direct

Author: ocots

src/CTDirect/problem.jl

@@ -1,4 +1,4 @@
-function ADNLProblem(ocp::OptimalControlModel, N::Integer)
+function ADNLProblem(ocp::OptimalControlModel, N::Integer, init=nothing)
 
     # direct_infos
     t0, tf_, n_x, m, f, ξ, ψ, ϕ, dim_ξ, dim_ψ, dim_ϕ, 
@@ -94,7 +94,7 @@ function ADNLProblem(ocp::OptimalControlModel, N::Integer)
     end
 
     # bounds for the constraints
-    function  ipopt_l_u_b()
+    function  constraints_bounds()
         lb = zeros(nc)
         ub = zeros(nc)
         index = 1 # counter for the constraints
@@ -134,17 +134,75 @@ function ADNLProblem(ocp::OptimalControlModel, N::Integer)
         return lb, ub
     end
 
-    # todo: init a changer
-    xu0 = 1.1*ones(dim_xu)
+    # todo: retrieve optional bounds from ocp parsed constraints
+    function variables_bounds()
+        # unbounded case
+        l_var = -Inf*ones(dim_xu)
+        u_var = Inf*ones(dim_xu)
+        return l_var, u_var
+    end
+
+    # generate initial guess
+    function set_state_at_time_step!(x, i, dim_x, N, xu)
+        if i > N
+            error("trying to set x(t_i) for i > N")
+        else
+            xu[1+i*dim_x:(i+1)*dim_x] = x[1:dim_x]
+        end
+    end
+    
+    function set_control_at_time_step!(u, i, dim_x, N, m, xu)
+        if i > N
+            error("trying to set (t_i) for i > N")
+        else
+            xu[1+(N+1)*dim_x+i*m:m+(N+1)*dim_x+i*m] = u[1:m]
+        end
+    end
+
+    function initial_guess()
+        #println("Initialization: ", init)
+
+        if init === nothing
+            # default initialization
+            xu0 = 1.1*ones(dim_xu)
+        else
+            if length(init) != (n_x + m)
+                error("vector for initialization should be of size n+m",n_x+m)
+            end
+            # split state / control
+            x_init = zeros(dim_x)
+            x_init[1:n_x] = init[1:n_x]
+            u_init = zeros(m)
+            u_init[1:m] = init[n_x+1:n_x+m]
+            
+            # mayer -> lagrange additional state
+            if hasLagrangeCost
+                x_init[dim_x] = 0.1
+            end
+
+            # constant initialization
+            xu0 = zeros(dim_xu)
+            for i in 0:N
+                set_state_at_time_step!(x_init, i, dim_x, N, xu0)
+                set_control_at_time_step!(u_init, i, dim_x, N, m, xu0)
+            end
+        end
+        return xu0
+    end
+
+    # variables bounds   
+    l_var, u_var = variables_bounds()
+
+    # initial guess
+    xu0 = initial_guess()
 
-    l_var = -Inf*ones(dim_xu)
-    u_var = Inf*ones(dim_xu)
+    # free final time case
     if has_free_final_time
       xu0[end] = 1.0
       l_var[end] = 1.e-3
     end
 
-    lb, ub = ipopt_l_u_b()
+    lb, ub = constraints_bounds()
 
     nlp = ADNLPModel(ipopt_objective, xu0, l_var, u_var, ipopt_constraint, lb, ub)    
 

src/CTDirect/solve.jl

@@ -5,6 +5,7 @@ function direct_solve(ocp::OptimalControlModel,
   print_level::Integer=__print_level_ipopt(),
   mu_strategy::String=__mu_strategy_ipopt(),
   display::Bool=__display(),
+  init=nothing,  #NB. for now, can be nothing or (n+m) vector
   kwargs...)
   """
     Solve the optimal control problem
@@ -23,7 +24,8 @@ function direct_solve(ocp::OptimalControlModel,
   print_level = display ?  print_level : 0
 
   # from OCP to NLP
-  nlp = ADNLProblem(ocp, grid_size)
+  nlp = ADNLProblem(ocp, grid_size, init)
+  #println("nlp x0:", nlp.meta.x0)
 
   # solve by IPOPT: more info at 
   # https://github.com/JuliaSmoothOptimizers/NLPModelsIpopt.jl/blob/main/src/NLPModelsIpopt.jl#L119

src/CTDirect/utils.jl

@@ -4,7 +4,7 @@ function get_state_at_time_step(xu, i, dim_x, N)
         x(t_i)
     """
     if i > N
-        error("trying to access at x(t_i) for i > N")
+        error("trying to get x(t_i) for i > N")
     end  
     return xu[1+i*dim_x:(i+1)*dim_x]
 end
@@ -15,7 +15,7 @@ function get_control_at_time_step(xu, i, dim_x, N, m)
         u(t_i)
     """
     if i > N
-        error("trying to access at (t_i) for i > N")
+        error("trying to get (t_i) for i > N")
     end
     return xu[1+(N+1)*dim_x+i*m:m+(N+1)*dim_x+i*m]
 end

test/test_basic_manual.jl renamed to test/manual_test_basic.jl

@@ -19,8 +19,13 @@ B = [ 0.0
 constraint!(ocp, :dynamics, (x, u) -> A*x + B*u[1])
 objective!(ocp, :lagrange, (x, u) -> 0.5u[1]^2) # default is to minimise
 
+# initial guess (constant state and control functions)
+init = [1., 0.5, 0.3]
+
 # solve
-sol = solve(ocp, grid_size=30)
+#sol = solve(ocp, grid_size=10, print_level=5)
+sol = solve(ocp, grid_size=10, print_level=5, init=init)
+
 
 # plot
 plot(sol)

test/test_goddard_manual.jl renamed to test/manual_test_goddard.jl

@@ -47,6 +47,10 @@ end
 
 constraint!(ocp, :dynamics, f)
 
-sol = solve(ocp, 20)
+# initial guess (constant state and control functions)
+init = [1.01, 0.25, 0.5, 0.4]
+
+#sol = solve(ocp, grid_size=20, print_level=5)
+sol = solve(ocp, grid_size=20, print_level=5, init=init)
 
 plot(sol)

test/runtests.jl

@@ -21,11 +21,11 @@ const __display = OptimalControl.CTBase.__display
 #
 @testset verbose = true showtiming = true "Optimal control tests" begin
     for name in (
-        #"utils", 
-        #"direct_shooting_CTOptimization", # unconstrained direct simple shooting
+        "utils", 
+        "direct_shooting_CTOptimization", # unconstrained direct simple shooting
         "basic",
-        #"goddard_direct",
-        #"goddard_indirect",
+        "goddard_direct",
+        "goddard_indirect",
         )
         @testset "$name" begin
             include("test_$name.jl")

test/test_goddard_direct.jl

@@ -47,7 +47,8 @@ end
 
 constraint!(ocp, :dynamics, f)
 
-sol = solve(ocp, grid_size=10, print_level=0)
+init = [1.01, 0.25, 0.5, 0.4]
+sol = solve(ocp, grid_size=10, print_level=0, init=init)
 
 @test objective(sol) ≈ -1.0 atol=1e-1
 @test constraints_violation(sol) < 1e-6