set objective and constraint violation if issing

src/problem.jl

@@ -56,6 +56,12 @@ mutable struct DOCP
     dim_NLP_variables::Int64
     dim_NLP_steps::Int64
 
+    # lower and upper bounds for variables and constraints
+    var_l
+    var_u
+    con_l
+    con_u
+
     # NLP model for solver
     nlp
 
@@ -284,7 +290,7 @@ end
 
 
 # DOCP constraints  (add bounds computation here at first call ?)
-function DOCP_constraint!(c, xu, docp)    
+function DOCP_constraints!(c, xu, docp)    
     """
     compute the constraints for the NLP : 
         - discretization of the dynamics via the trapeze method
@@ -397,6 +403,35 @@ function DOCP_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

@@ -10,16 +10,28 @@ 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)
 
-    # adjust objective sign for maximization problem
+    # 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_constraint!(constraints, solution, docp)
-
+    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_DOCP_solution(docp, solution, multipliers_constraints, multipliers_LB, multipliers_UB, constraints)
 

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