cleanup of DOCP struct, more generic version of OCPsolutionFromDOCP (#77

)

* raw version of OCPsolutionFromDOCP

* todo: debug recomputation of constraints post optimization

* cleaned DOCP and solution generation

* bugfix

* set objective and constraint violation if issing

Project.toml

@@ -7,6 +7,7 @@ version = "0.4.5"
 ADNLPModels = "54578032-b7ea-4c30-94aa-7cbd1cce6c9a"
 CTBase = "54762871-cc72-4466-b8e8-f6c8b58076cd"
 DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae"
+LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 NLPModels = "a4795742-8479-5a88-8948-cc11e1c8c1a6"
 NLPModelsIpopt = "f4238b75-b362-5c4c-b852-0801c9a21d71"
 Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7"
@@ -15,4 +16,4 @@ Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7"
 ADNLPModels = "0.7"
 CTBase = "0.7"
 DocStringExtensions = "0.9"
-julia = "1.8"
+julia = "1.9"

src/CTDirect.jl

@@ -4,6 +4,7 @@ using CTBase
 using DocStringExtensions
 using Symbolics                   # for optimized auto diff
 using NLPModelsIpopt, ADNLPModels # docp model and solver
+using LinearAlgebra               # norm
 
 # Other declarations
 const nlp_constraints = CTBase.nlp_constraints

src/problem.jl

@@ -56,17 +56,11 @@ mutable struct DOCP
     dim_NLP_variables::Int64
     dim_NLP_steps::Int64
 
-    # initialization
-    #NLP_init
-
-    # NLP solution
-    NLP_solution
-    NLP_objective
-    NLP_sol_constraints
-    NLP_constraints_violation
-    NLP_iterations     
-    # remove later ? type is https://juliasmoothoptimizers.github.io/SolverCore.jl/stable/reference/#SolverCore.GenericExecutionStats
-    NLP_stats 
+    # lower and upper bounds for variables and constraints
+    var_l
+    var_u
+    con_l
+    con_u
 
     # NLP model for solver
     nlp
@@ -114,7 +108,6 @@ mutable struct DOCP
 
         ## Non Linear Programming NLP
         docp.dim_NLP_steps = N
-        #docp.NLP_init = init
 
         # Mayer to Lagrange reformulation: 
         # additional state with Lagrange cost as dynamics and null initial condition
@@ -269,8 +262,8 @@ function variables_bounds(docp)
 end
 
 
-# IPOPT objective
-function ipopt_objective(xu, docp)
+# DOCP objective
+function DOCP_objective(xu, docp)
 
     #t0 = get_initial_time(xu, docp)
     #tf = get_final_time(xu, docp)
@@ -296,8 +289,8 @@ function ipopt_objective(xu, docp)
 end
 
 
-# IPOPT constraints  (add bounds computation here at first call ?)
-function ipopt_constraint!(c, xu, docp)    
+# DOCP constraints  (add bounds computation here at first call ?)
+function DOCP_constraints!(c, xu, docp)    
     """
     compute the constraints for the NLP : 
         - discretization of the dynamics via the trapeze method
@@ -357,7 +350,8 @@ function ipopt_constraint!(c, xu, docp)
         end
         index = index + docp.dim_NLP_state
 
-        # path constraints
+        # path constraints 
+        # +++use aux function for block, see solution also
         if docp.has_control_constraints
             c[index:index+docp.dim_control_constraints-1] = docp.control_constraints[2](ti, ui, v)
             index = index + docp.dim_control_constraints
@@ -409,6 +403,35 @@ function ipopt_constraint!(c, xu, docp)
         c[index] = get_lagrange_cost_at_time_step(xu, docp, 0)
         index = index + 1
     end
-
     return c # needed even for inplace version, AD error otherwise oO
 end
+
+# +++ todo unify in a single utils function check_bounds(v,lb,ub) that returns the error vector
+function DOCP_constraints_check!(cb, constraints, docp)
+
+    # check constraints vs bounds
+    # by construction only one of the two can be active
+    for i in 1:docp.dim_NLP_constraints
+        if constraints[i] < docp.con_l[i]
+            cb[i] = constraints[i] - docp.con_l[i]
+        end
+        if constraints[i] > docp.con_u[i]
+            cb[i] = constraints[i] - docp.con_u[i]
+        end
+    end
+    return nothing
+end
+
+function DOCP_variables_check!(vb, variables, docp)
+    # check variables vs bounds
+    # by construction only one of the two can be active
+    for i in 1:docp.dim_NLP_variables
+        if variables[i] < docp.var_l[i]
+            vb[i] = solution[i] - docp.var_l[i]
+        end
+        if variables[i] > docp.var_u[i]
+            vb[i] = solution[i] - docp.var_u[i]
+        end
+    end
+    return nothing
+end

src/solution.jl

@@ -1,42 +1,61 @@
-# build OCP solution from DOCP solution
-function OCPSolutionFromDOCP(ipopt_solution, docp)
-
-    # save general solution data
-    docp.NLP_stats = ipopt_solution
-    if is_min(docp.ocp)
-        docp.NLP_objective = ipopt_solution.objective
-    else
-        docp.NLP_objective = - ipopt_solution.objective
-    end
-    docp.NLP_constraints_violation = ipopt_solution.primal_feas
-    docp.NLP_iterations = ipopt_solution.iter
-    docp.NLP_solution = ipopt_solution.solution
-    docp.NLP_sol_constraints = zeros(docp.dim_NLP_constraints)
-    ipopt_constraint!(docp.NLP_sol_constraints, ipopt_solution.solution, docp)
+# build OCP solution from DOCP solution (GenericExecutionStats)
+# ipopt solution is a GenericExecutionStats
+# https://jso.dev/SolverCore.jl/dev/reference/#SolverCore.get_status-Tuple{NLPModels.AbstractNLPModel}
+function OCPSolutionFromDOCP(docp, docp_solution_ipopt)
 
+    # could pass some status info too (get_status ?)
+    return OCPSolutionFromDOCP_raw(docp, docp_solution_ipopt.solution, objective=docp_solution_ipopt.objective, constraints_violation=docp_solution_ipopt.primal_feas, iterations=docp_solution_ipopt.iter,multipliers_constraints=docp_solution_ipopt.multipliers, multipliers_LB=docp_solution_ipopt.multipliers_L, multipliers_UB=docp_solution_ipopt.multipliers_U, message=docp_solution_ipopt.solver_specific[:internal_msg])
+end
+
+# still missing: stopping and success info...
+function OCPSolutionFromDOCP_raw(docp, solution; objective=nothing, constraints_violation=nothing, iterations=0, multipliers_constraints=nothing, multipliers_LB=nothing, multipliers_UB=nothing, message=nothing)
+
+    # set objective if needed
+    if objective==nothing
+        objective = DOCP_objective(solution, docp)
+    end
+    # adjust objective sign for maximization problems
+    if !is_min(docp.ocp)
+        objective = - objective
+    end
+
+    # recompute value of constraints at solution
+    # NB. the constraint formulation is LB <= C <= UB
+    constraints = zeros(docp.dim_NLP_constraints)
+    DOCP_constraints!(constraints, solution, docp)
+    # set constraint violation if needed
+    if constraints_violation==nothing
+        constraints_check = zeros(docp.dim_NLP_constraints)
+        DOCP_constraints_check!(constraints_check, constraints, docp)
+        variables_check = zeros(docp.dim_NLP_variables)
+        DOCP_variables_check!(variables_check, solution, docp)
+        constraints_violation = norm(append!(variables_check, constraints_check), Inf)
+    end
+
     # parse NLP variables, constraints and multipliers 
-    X, U, v, P, sol_control_constraints, sol_state_constraints, sol_mixed_constraints, sol_variable_constraints, mult_control_constraints, mult_state_constraints, mult_mixed_constraints, mult_variable_constraints, mult_state_box_lower, mult_state_box_upper, mult_control_box_lower, mult_control_box_upper, mult_variable_box_lower, mult_variable_box_upper = parse_ipopt_sol(docp)
+    X, U, v, P, sol_control_constraints, sol_state_constraints, sol_mixed_constraints, sol_variable_constraints, mult_control_constraints, mult_state_constraints, mult_mixed_constraints, mult_variable_constraints, mult_state_box_lower, mult_state_box_upper, mult_control_box_lower, mult_control_box_upper, mult_variable_box_lower, mult_variable_box_upper = parse_DOCP_solution(docp, solution, multipliers_constraints, multipliers_LB, multipliers_UB, constraints)
 
     # variables and misc infos
     N = docp.dim_NLP_steps
-    t0 = get_initial_time(docp.NLP_solution, docp)
-    tf = max(get_final_time(docp.NLP_solution, docp), t0 + 1e-9)
+    t0 = get_initial_time(solution, docp)
+    tf = max(get_final_time(solution, docp), t0 + 1e-9)
     T = collect(LinRange(t0, tf, N+1))
     x = ctinterpolate(T, matrix2vec(X, 1))
     u = ctinterpolate(T, matrix2vec(U, 1))
     p = ctinterpolate(T[1:end-1], matrix2vec(P, 1))
     sol = OptimalControlSolution() # +++ constructor with ocp as argument ?
     copy!(sol, docp.ocp)
     sol.times      = T
-    sol.state      = (sol.state_dimension==1)    ? deepcopy(t -> x(t)[1]) : deepcopy(t -> x(t)) # scalar output if dim=1
-    sol.costate    = (sol.state_dimension==1)    ? deepcopy(t -> p(t)[1]) : deepcopy(t -> p(t)) # scalar output if dim=1
-    sol.control    = (sol.control_dimension==1)  ? deepcopy(t -> u(t)[1]) : deepcopy(t -> u(t)) # scalar output if dim=1
-    sol.variable   = (sol.variable_dimension==1) ? v[1] : v # scalar output if dim=1
-    sol.objective  = docp.NLP_objective
-    sol.iterations = docp.NLP_iterations
-    sol.stopping   = :dummy
-    sol.message    = "no message"
-    sol.success    = false #
+    # use scalar output for x,u,v,p if dim=1
+    sol.state      = (sol.state_dimension==1)    ? deepcopy(t -> x(t)[1]) : deepcopy(t -> x(t)) 
+    sol.costate    = (sol.state_dimension==1)    ? deepcopy(t -> p(t)[1]) : deepcopy(t -> p(t))
+    sol.control    = (sol.control_dimension==1)  ? deepcopy(t -> u(t)[1]) : deepcopy(t -> u(t))
+    sol.variable   = (sol.variable_dimension==1) ? v[1] : v
+    sol.objective  = objective
+    sol.iterations = iterations
+    sol.stopping   = nothing
+    sol.message    = String(message)
+    sol.success    = nothing
 
     # nonlinear constraints and multipliers
     if docp.has_state_constraints
@@ -89,18 +108,16 @@ function OCPSolutionFromDOCP(ipopt_solution, docp)
 end
 
 
-# parse NLP solution from ipopt into OCP variables, constraints and multipliers
-function parse_ipopt_sol(docp)
+# parse DOCP solution into OCP variables, constraints and multipliers
+function parse_DOCP_solution(docp, solution, multipliers_constraints, multipliers_LB, multipliers_UB, constraints)
 
-    N = docp.dim_NLP_steps
-
     # states and controls variables, with box multipliers
-    xu = docp.NLP_solution
-    mult_L = docp.NLP_stats.multipliers_L
-    mult_U = docp.NLP_stats.multipliers_U
+    N = docp.dim_NLP_steps
+    mult_L = multipliers_LB
+    mult_U = multipliers_UB
     X = zeros(N+1,docp.dim_NLP_state)
     U = zeros(N+1,docp.ocp.control_dimension)
-    v = get_variable(xu, docp)
+    v = get_variable(solution, docp)
     mult_state_box_lower = zeros(N+1,docp.dim_NLP_state)
     mult_state_box_upper = zeros(N+1,docp.dim_NLP_state)
     mult_control_box_lower = zeros(N+1,docp.ocp.control_dimension)
@@ -110,8 +127,8 @@ function parse_ipopt_sol(docp)
 
     for i in 1:N+1
         # variables
-        X[i,:] = vget_state_at_time_step(xu, docp, i-1)
-        U[i,:] = vget_control_at_time_step(xu, docp, i-1)
+        X[i,:] = vget_state_at_time_step(solution, docp, i-1)
+        U[i,:] = vget_control_at_time_step(solution, docp, i-1)
         # box multipliers (same layout as variables !)
         # +++ fix mult vectors instead to always be full size (todo in return from solve)
         if length(mult_L) > 0
@@ -134,8 +151,7 @@ function parse_ipopt_sol(docp)
 
     # constraints, costate and constraints multipliers
     P = zeros(N, docp.dim_NLP_state)
-    lambda = docp.NLP_stats.multipliers
-    c = docp.NLP_sol_constraints
+    lambda = multipliers_constraints
     sol_control_constraints = zeros(N+1,docp.dim_control_constraints)
     sol_state_constraints = zeros(N+1,docp.dim_state_constraints)
     sol_mixed_constraints = zeros(N+1,docp.dim_mixed_constraints) 
@@ -151,49 +167,51 @@ function parse_ipopt_sol(docp)
         P[i,:] = lambda[index:index+docp.dim_NLP_state-1]
         index = index + docp.dim_NLP_state
         # path constraints
+        # +++ use aux function for the 3 blocks, see eval c also
         if docp.has_control_constraints
-            sol_control_constraints[i,:] = c[index:index+docp.dim_control_constraints-1]
+            sol_control_constraints[i,:] = constraints[index:index+docp.dim_control_constraints-1]
             mult_control_constraints[i,:] = lambda[index:index+docp.dim_control_constraints-1]
             index = index + docp.dim_control_constraints
         end
         if docp.has_state_constraints
-            sol_state_constraints[i,:] = c[index:index+docp.dim_state_constraints-1]
+            sol_state_constraints[i,:] = constraints[index:index+docp.dim_state_constraints-1]
             mult_state_constraints[i,:] = lambda[index:index+docp.dim_state_constraints-1]
             index = index + docp.dim_state_constraints
         end
         if docp.has_mixed_constraints
-            sol_mixed_constraints[i,:] = c[index:index+docp.dim_mixed_constraints-1]
+            sol_mixed_constraints[i,:] = constraints[index:index+docp.dim_mixed_constraints-1]
             mult_mixed_constraints[i,:] = lambda[index:index+docp.dim_mixed_constraints-1]
             index = index + docp.dim_mixed_constraints
         end
     end
     # path constraints at final time
+    # +++ use aux function for the 3 blocks, see eval c also
     if docp.has_control_constraints
-        sol_control_constraints[N+1,:] = c[index:index+docp.dim_control_constraints-1]
+        sol_control_constraints[N+1,:] = constraints[index:index+docp.dim_control_constraints-1]
         mult_control_constraints[N+1,:] = lambda[index:index+docp.dim_control_constraints-1]
         index = index + docp.dim_control_constraints
     end
     if docp.has_state_constraints
-        sol_state_constraints[N+1,:] = c[index:index+docp.dim_state_constraints-1] 
+        sol_state_constraints[N+1,:] = constraints[index:index+docp.dim_state_constraints-1] 
         mult_state_constraints[N+1,:] = lambda[index:index+docp.dim_state_constraints-1]
         index = index + docp.dim_state_constraints
     end
     if docp.has_mixed_constraints
-        sol_mixed_constraints[N+1,:] = c[index:index+docp.dim_mixed_constraints-1]        
+        sol_mixed_constraints[N+1,:] = constraints[index:index+docp.dim_mixed_constraints-1]        
         mult_mixed_constraints[N+1,:] =  lambda[index:index+docp.dim_mixed_constraints-1]
         index = index + docp.dim_mixed_constraints
     end
 
     # boundary conditions and multipliers
     if docp.has_boundary_conditions
-        sol_boundary_conditions = c[index:index+docp.dim_boundary_conditions-1]
+        sol_boundary_conditions = constraints[index:index+docp.dim_boundary_conditions-1]
         mult_boundary_conditions = lambda[index:index+docp.dim_boundary_conditions-1]
         index = index + docp.dim_boundary_conditions
     end
 
     # variable constraints and multipliers
     if docp.has_variable_constraints
-        sol_variable_constraints = c[index:index+docp.dim_variable_constraints-1]
+        sol_variable_constraints = constraints[index:index+docp.dim_variable_constraints-1]
         mult_variable_constraints = lambda[index:index+docp.dim_variable_constraints-1]
         index = index + docp.dim_variable_constraints
     end

src/solve.jl

@@ -1,7 +1,7 @@
 # TODO
 # add function to set the intial guess for a docp: need to rebuild the nlp model completely ? 
 
-# availble methods by order of preference: from top to bottom
+# available methods by order of preference: from top to bottom
 algorithmes = ()
 algorithmes = add(algorithmes, (:adnlp, :ipopt))
 
@@ -29,13 +29,13 @@ function directTranscription(ocp::OptimalControlModel,
     # initialization is optional
     docp = DOCP(ocp, grid_size)
     x0 = initial_guess(docp, init)
-    l_var, u_var = variables_bounds(docp)
-    lb, ub = constraints_bounds(docp)
-    docp.nlp = ADNLPModel!(x -> ipopt_objective(x, docp), 
+    docp.var_l, docp.var_u = variables_bounds(docp)
+    docp.con_l, docp.con_u = constraints_bounds(docp)
+    docp.nlp = ADNLPModel!(x -> DOCP_objective(x, docp), 
                     x0,
-                    l_var, u_var, 
-                    (c, x) -> ipopt_constraint!(c, x, docp), 
-                    lb, ub, 
+                    docp.var_l, docp.var_u, 
+                    (c, x) -> DOCP_constraints!(c, x, docp), 
+                    docp.con_l, docp.con_u, 
                     backend = :optimized)
 
 return docp
@@ -72,7 +72,7 @@ function solveDOCP(docp::DOCP;
     end
 
     # return solution for original OCP
-    return OCPSolutionFromDOCP(docp_solution, docp)
+    return OCPSolutionFromDOCP(docp, docp_solution)
 end
 
 

test/test_basic.jl

@@ -23,27 +23,3 @@ 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)
-
-# 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)