diff --git a/Project.toml b/Project.toml index 02afdb4..cec5197 100644 --- a/Project.toml +++ b/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" diff --git a/src/CTDirect.jl b/src/CTDirect.jl index ffce075..f3791e9 100644 --- a/src/CTDirect.jl +++ b/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 diff --git a/src/problem.jl b/src/problem.jl index 7e300dd..8866c0f 100644 --- a/src/problem.jl +++ b/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 \ No newline at end of file diff --git a/src/solution.jl b/src/solution.jl index 45c89ae..0f34f97 100644 --- a/src/solution.jl +++ b/src/solution.jl @@ -1,26 +1,44 @@ -# 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)) @@ -28,15 +46,16 @@ function OCPSolutionFromDOCP(ipopt_solution, docp) 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 diff --git a/src/solve.jl b/src/solve.jl index 674bed8..d90f523 100644 --- a/src/solve.jl +++ b/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 diff --git a/test/test_basic.jl b/test/test_basic.jl index 886e211..7711f9e 100644 --- a/test/test_basic.jl +++ b/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)