From 7f8c11bc5a952277f1ec3acaa325e759a6362f43 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 19 Dec 2022 12:19:01 +0100 Subject: [PATCH] Index refactor (#26) * remove ind_total * WIP: cleaning up index handling via function * WIP fixed some syntax issues and duplicate X_ki * fixing prints * fixing more typos * optional index parameter * more typos, and remaining problem variables * quieter prints and remove virtual forces indexes * even quieter prints * remove index from _end * Added constraint adding util, rename var * using add_constraint * fixing issues * fix typo, add dims check * fixing more typos * dims test * comments, todos * more todos --- src/matlab/add_constraint.m | 15 + src/matlab/add_variable.m | 18 + src/matlab/create_nlp_nosnoc.m | 674 +++++++++++++-------------------- 3 files changed, 304 insertions(+), 403 deletions(-) create mode 100644 src/matlab/add_constraint.m create mode 100644 src/matlab/add_variable.m diff --git a/src/matlab/add_constraint.m b/src/matlab/add_constraint.m new file mode 100644 index 00000000..3b0f5575 --- /dev/null +++ b/src/matlab/add_constraint.m @@ -0,0 +1,15 @@ +function [problem] = add_constraint(problem, g, lbg, ubg, idx) + %ADD_CONSTRAINT Adds a general constraint with upper and lower bounds to the problem. + dims = [length(g), length(lbg), length(ubg)]; + if(~all(dims == dims(1))) + error("dimension mismatch, with dims: g: %d, lbg: %d, ubg: %d", dims(1), dims(2), dims(3)); + end + problem.g = {problem.g{:}, g}; + problem.lbg = [problem.lbg; lbg]; + problem.ubg = [problem.ubg; ubg]; + % This is a bit of a hack, to avoid a bunch of if statements (maybe, currently would only be one) + if exist('idx','var') + ind_set = problem.(strcat('ind_', idx)); + problem.(strcat('ind_', idx)) = [ind_set, length(problem.lbg)]; + end +end diff --git a/src/matlab/add_variable.m b/src/matlab/add_variable.m new file mode 100644 index 00000000..f9568c0f --- /dev/null +++ b/src/matlab/add_variable.m @@ -0,0 +1,18 @@ +function [problem] = add_variable(problem, w, w0, lbw, ubw, idx) + %ADD_VARIABLE Adds a primal variable with upper and lower bounds and initialization to the problem. + dims = [length(w), length(w0), length(lbw), length(ubw)]; + if(~all(dims == dims(1))) + error("dimension mismatch, with dims: w: %d, w0: %d, lbw: %d, ubw: %d", dims(1), dims(2), dims(3), dims(4)); + end + problem.w = {problem.w{:}, w}; + % This is a bit of a hack, to avoid a bunch of if statements. + % TODO Maybe separate these out as a "per variable" cell array like done in python. + if exist('idx','var') + ind_set = problem.(strcat('ind_', idx)); + problem.(strcat('ind_', idx)) = [ind_set, length(problem.w0)+1:length(problem.w0)+length(w)]; + end + problem.w0 = [problem.w0; w0]; + problem.lbw = [problem.lbw; lbw]; + problem.ubw = [problem.ubw; ubw]; +end + diff --git a/src/matlab/create_nlp_nosnoc.m b/src/matlab/create_nlp_nosnoc.m index 6e0c4ff1..b2a8cfc8 100644 --- a/src/matlab/create_nlp_nosnoc.m +++ b/src/matlab/create_nlp_nosnoc.m @@ -140,13 +140,33 @@ J_comp_fesd = 0; J_regularize_h = 0; J_regularize_sot = 0; -% constraints -g = {}; -lbg = []; -ubg = []; + +% Initialize problem struct +% TODO This may one day be a class! +problem = struct(); +% Primal vars +problem.w = {}; +problem.w0 = []; +problem.lbw = []; +problem.ubw = []; +% Index vectors +problem.ind_x = []; +problem.ind_u = []; +problem.ind_v = []; +problem.ind_z = []; +problem.ind_h = []; +problem.ind_elastic = []; +problem.ind_sot = []; % index for speed of time variable +problem.ind_boundary = []; % index of bundary value lambda and mu +problem.ind_t_final = []; % Time-optimal problems: define auxilairy variable for the final time. +% Constraints +problem.g = {}; +problem.lbg = []; +problem.ubg = []; +problem.ind_g_clock_state = []; + X_ki = define_casadi_symbolic(casadi_symbolic_mode,'X0',n_x); -w = {w{:}, X_ki}; if there_exist_free_x0 x0_ub = x0; @@ -156,28 +176,12 @@ x0_ub(ind_free_x0) = ubx(ind_free_x0); x0_lb(ind_free_x0) = lbx(ind_free_x0); - lbw = [lbw; x0_lb]; - ubw = [ubw; x0_ub]; + + problem = add_variable(problem, X_ki, x0, x0_lb, x0_ub, 'x'); else - lbw = [lbw; x0]; - ubw = [ubw; x0]; + problem = add_variable(problem, X_ki, x0, x0, x0, 'x'); end -w0 = [w0; x0]; - -% Index vectors -ind_x = [1:n_x]; -ind_u = []; -ind_v = []; -ind_z = []; -ind_h = []; -ind_elastic = []; -ind_g_clock_state = []; -ind_vf = []; -ind_sot = []; % index for speed of time variable -ind_boundary = []; % index of bundary value lambda and mu -ind_total = ind_x; - % Integral of the clock state if no time rescaling is present. sum_h_ki_control_interval_k = 0; sum_h_ki_all = 0; @@ -201,18 +205,15 @@ comp_var_current_fe.cross_comp_all = 0; %% Formulate the NLP / Main Discretization loop +% TODO cleanup steps: +% - Create primal variables all at once. +% - Separate sections into separate functions operating on the `problem` struct/class +% - time variables should probably not just be lumped into the state, for readability. for k=0:N_stages-1 % control variables if n_u > 0 Uk = define_casadi_symbolic(casadi_symbolic_mode,['U_' num2str(k)],n_u); - w = {w{:}, Uk}; - % intialize contros, lower and upper bounds - w0 = [w0; u0]; - lbw = [lbw;lbu]; - ubw = [ubw;ubu]; - % index colector for contorl variables - ind_u = [ind_u,ind_total(end)+1:ind_total(end)+n_u]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+n_u]; + problem = add_variable(problem, Uk, u0, lbu, ubu, 'u'); end %% Time rescaling of the stages (speed of time) to acchieve e.g., a desired final time in Time-Freezing or to solve time optimal control problems. @@ -222,28 +223,13 @@ if local_speed_of_time_variable % at every stage s_sot_k = define_casadi_symbolic(casadi_symbolic_mode,['s_sot_' num2str(k)],1); - w = {w{:}, s_sot_k}; - % intialize speed of time (sot), lower and upper bounds - w0 = [w0; s_sot0]; - ubw = [ubw;s_sot_max]; - lbw = [lbw;s_sot_min]; - % index colector for sot variables - ind_sot = [ind_sot,ind_total(end)+1:ind_total(end)+1]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+1]; + problem = add_variable(problem, s_sot_k, s_sot0, s_sot_min, s_sot_max, 'sot'); J_regularize_sot = J_regularize_sot+(s_sot_k-1)^2; else if k == 0 % only once s_sot_k = define_casadi_symbolic(casadi_symbolic_mode,['s_sot_' num2str(k+1)],1); - w = {w{:}, s_sot_k}; - % intialize speed of time (sot), lower and upper bounds - w0 = [w0; s_sot0]; - lbw = [lbw;s_sot_min]; - ubw = [ubw;s_sot_max]; - % index colector for sot variables - ind_sot = [ind_sot,ind_total(end)+1:ind_total(end)+1]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+1]; - % enforce gradient steps towward smaller values of s_sot to aid convergence (large variaties of s_sot = high nonlinearity) + problem = add_variable(problem, s_sot_k, s_sot0, s_sot_min, s_sot_max, 'sot'); J_regularize_sot = J_regularize_sot+(s_sot_k)^2; end end @@ -254,17 +240,11 @@ %% General Nonlinear constraint (on control interval boundary) % The CasADi function g_ineq_fun and its lower and upper bound are provieded in model. if g_ineq_constraint - g_ineq_k = g_ineq_fun(X_ki,Uk); - g = {g{:}, g_ineq_k}; - lbg = [lbg; g_ineq_lb]; - ubg = [ubg; g_ineq_ub]; + problem = add_constraint(problem, g_ineq_fun(X_ki,Uk), g_ineq_lb, g_ineq_ub); end % path complementarity constraints if g_comp_path_constraint - g_comp_path_k = g_comp_path_fun(X_ki,Uk)-sigma_p; - g = {g{:}, g_comp_path_k}; - lbg = [lbg; g_comp_path_lb]; - ubg = [ubg; g_comp_path_ub]; + problem = add_constraint(problem, g_comp_path_fun(X_ki,Uk)-sigma_p, g_comp_path_lb, g_comp_path_ub); end sum_h_ki_control_interval_k = 0; % sum of all h_ki at current control interval @@ -293,15 +273,7 @@ end % define step-size h_ki = define_casadi_symbolic(casadi_symbolic_mode,['h_' num2str(k) '_' num2str(i)],1); - w = {w{:}, h_ki}; - w0 = [w0; h0_k(k+1)]; - ubw = [ubw;ubh(k+1)]; - lbw = [lbw;lbh(k+1)]; - % index sets for step-size variables - ind_h = [ind_h,ind_total(end)+1:ind_total(end)+1]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+1]; - - + problem = add_variable(problem, h_ki, h0_k(k+1), lbh(k+1), ubh(k+1), 'h'); if i > 0 delta_h_ki = h_ki - h_ki_previous; else @@ -332,45 +304,26 @@ % loop over all stage points to carry out defintions, initializations and bounds for j=1:n_s switch irk_representation - case 'integral' - % define symbolic variables for values of diff. state a stage points - X_ki_stages{j} = define_casadi_symbolic(casadi_symbolic_mode,['X_' num2str(k) '_' num2str(i) '_' num2str(j) ],n_x); - w = {w{:}, X_ki_stages{j}}; - w0 = [w0; x0]; - ind_x = [ind_x,ind_total(end)+1:ind_total(end)+n_x]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+n_x]; + case 'integral' + % define symbolic variables for values of diff. state a stage points + X_ki_stages{j} = define_casadi_symbolic(casadi_symbolic_mode,['X_' num2str(k) '_' num2str(i) '_' num2str(j) ],n_x); + if x_box_at_stg + problem = add_variable(problem, X_ki_stages{j}, x0, lbx, ubx, 'x'); + else + problem = add_variable(problem, X_ki_stages{j}, x0, -inf*ones(n_x,1), inf*ones(n_x,1), 'x'); + end + case 'differential' + V_ki_stages{j} = define_casadi_symbolic(casadi_symbolic_mode,['V_' num2str(k) '_' num2str(i) '_' num2str(j) ],n_x); + problem = add_variable(problem, V_ki_stages{j}, v0, -inf*ones(n_x,1), inf*ones(n_x,1), 'v'); + + if lift_irk_differential + X_ki_stages{j} = define_casadi_symbolic(casadi_symbolic_mode,['X_' num2str(k) '_' num2str(i) '_' num2str(j)],n_x); if x_box_at_stg - lbw = [lbw; lbx]; - ubw = [ubw; ubx]; + problem = add_variable(problem, X_ki_stages{j}, x0, lbx, ubx, 'x'); else - lbw = [lbw; -inf*ones(n_x,1)]; - ubw = [ubw; inf*ones(n_x,1)]; - end - case 'differential' - V_ki_stages{j} = define_casadi_symbolic(casadi_symbolic_mode,['V_' num2str(k) '_' num2str(i) '_' num2str(j) ],n_x); - w = {w{:}, V_ki_stages{j}}; - w0 = [w0; v0]; - ind_v = [ind_v,ind_total(end)+1:ind_total(end)+n_x]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+n_x]; - lbw = [lbw; -inf*ones(n_x,1)]; - ubw = [ubw; inf*ones(n_x,1)]; - - if lift_irk_differential - X_ki_stages{j} = define_casadi_symbolic(casadi_symbolic_mode,['X_' num2str(k) '_' num2str(i) '_' num2str(j)],n_x); - - w = {w{:}, X_ki_stages{j}}; - w0 = [w0; x0]; - ind_x = [ind_x,ind_total(end)+1:ind_total(end)+n_x]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+n_x]; - % Bounds - if x_box_at_stg - lbw = [lbw; lbx]; - ubw = [ubw; ubx]; - else - lbw = [lbw; -inf*ones(n_x,1)]; - ubw = [ubw; +inf*ones(n_x,1)]; - end + problem = add_variable(problem, X_ki_stages{j}, x0, -inf*ones(n_x,1), inf*ones(n_x,1), 'x'); end + end end % Note that the algebraic variablies are treated the same way in both irk representation modes. if strcmp(casadi_symbolic_mode, 'SX') || strcmp(casadi_symbolic_mode, 'casadi.SX') % TODO: remove this or @@ -383,14 +336,7 @@ Z_ki_stages{j} = define_casadi_symbolic(casadi_symbolic_mode,['Z_' num2str(k) '_' num2str(i) '_' num2str(j)],n_z); end - w = {w{:}, Z_ki_stages{j}}; - % index sets - ind_z = [ind_z,ind_total(end)+1:ind_total(end)+n_z]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+n_z]; - % bounds and initialization - lbw = [lbw; lbz]; - ubw = [ubw; ubz]; - w0 = [w0; z0]; + problem = add_variable(problem, Z_ki_stages{j}, z0, lbz, ubz, 'z'); % collection of all lambda and theta for current finite element, they are used for cross complementarites and step equilibration switch pss_mode @@ -439,26 +385,24 @@ case 'Stewart' Lambda_ki_end = define_casadi_symbolic(casadi_symbolic_mode,['Lambda_' num2str(k) '_' num2str(i) '_end'],n_theta); Mu_ki_end = define_casadi_symbolic(casadi_symbolic_mode,['Mu_' num2str(k) '_' num2str(i) '_end'],n_simplex); - w = {w{:}, Lambda_ki_end}; - w = {w{:}, Mu_ki_end}; - % bounds and index sets - w0 = [w0; z0(n_theta+1:end)]; - lbw = [lbw; lbz(n_theta+1:end)]; - ubw = [ubw; ubz(n_theta+1:end)]; - ind_boundary = [ind_boundary,ind_total(end)+1:(ind_total(end)+n_z-n_theta)]; - ind_total = [ind_total,ind_total(end)+1:(ind_total(end)+n_z-n_theta)]; + % TODO: remove z0 indexing + problem = add_variable(problem,... + vertcat(Lambda_ki_end, Mu_ki_end),... + z0(n_theta+1:end),... + lbz(n_theta+1:end),... + ubz(n_theta+1:end),... + 'boundary'); Lambda_ki = {Lambda_ki{:}, Lambda_ki_end}; Mu_ki = {Mu_ki{:}, Mu_ki_end}; case 'Step' Lambda_ki_end = define_casadi_symbolic(casadi_symbolic_mode,['Lambda_' num2str(k) '_' num2str(i) '_end'],2*n_alpha); Mu_ki_end = []; - w = {w{:}, Lambda_ki_end}; - % bounds and index sets - w0 = [w0; z0(n_alpha+1:3*n_alpha)]; - lbw = [lbw; lbz(n_alpha+1:3*n_alpha)]; - ubw = [ubw; ubz(n_alpha+1:3*n_alpha)]; - ind_boundary = [ind_boundary,ind_total(end)+1:(ind_total(end)+2*n_alpha)]; - ind_total = [ind_total,ind_total(end)+1:(ind_total(end)+2*n_alpha)]; + problem = add_variable(problem,... + vertcat(Lambda_ki_end, Mu_ki_end),... + z0(n_alpha+1:3*n_alpha),... + lbz(n_alpha+1:3*n_alpha),... + ubz(n_alpha+1:3*n_alpha),... + 'boundary'); Lambda_ki = {Lambda_ki{:}, Lambda_ki_end}; Mu_ki = {Mu_ki{:}, []}; end @@ -512,95 +456,83 @@ for j=1:n_s switch irk_representation - case 'integral' - % Expression for the state derivative at the stage point - xp = C(1,j+1)*X_ki; - % Lagrange polynomial with values at state - for r=1:n_s - xp = xp + C(r+1,j+1)*X_ki_stages{r}; - end - % Evaluate Differential and Algebraic Equations at stage points - if n_u > 0 - [fj, qj] = f_x_fun(X_ki_stages{j},Z_ki_stages{j},Uk); - gj = g_z_all_fun(X_ki_stages{j},Z_ki_stages{j},Uk); - else - [fj, qj] = f_x_fun(X_ki_stages{j},Z_ki_stages{j}); - gj = g_z_all_fun(X_ki_stages{j},Z_ki_stages{j}); - end - if time_rescaling && use_speed_of_time_variables - % rescale equations - fj = s_sot_k*fj; - qj = s_sot_k*qj; - gj = s_sot_k*gj; - end - % Add contribution to the end state, Attention qj changes with time scaling! - Xk_end = Xk_end + D(j+1)*X_ki_stages{j}; - % Add contribution to quadrature function - J = J + B(j+1)*qj*h_ki; - % Append IRK equations to NLP constraint - g = {g{:}, h_ki*fj - xp}; - - case 'differential' - % Evaluate Differential and Algebraic Equations at stage points - if n_u > 0 - [fj, qj] = f_x_fun(X_ki_stages{j},Z_ki_stages{j},Uk); - gj = g_z_all_fun(X_ki_stages{j},Z_ki_stages{j},Uk); - else - [fj, qj] = f_x_fun(X_ki_stages{j},Z_ki_stages{j}); - gj = g_z_all_fun(X_ki_stages{j},Z_ki_stages{j}); - end + case 'integral' + % Expression for the state derivative at the stage point + xp = C(1,j+1)*X_ki; + % Lagrange polynomial with values at state + for r=1:n_s + xp = xp + C(r+1,j+1)*X_ki_stages{r}; + end + % Evaluate Differential and Algebraic Equations at stage points + if n_u > 0 + [fj, qj] = f_x_fun(X_ki_stages{j},Z_ki_stages{j},Uk); + gj = g_z_all_fun(X_ki_stages{j},Z_ki_stages{j},Uk); + else + [fj, qj] = f_x_fun(X_ki_stages{j},Z_ki_stages{j}); + gj = g_z_all_fun(X_ki_stages{j},Z_ki_stages{j}); + end + if time_rescaling && use_speed_of_time_variables + % rescale equations + fj = s_sot_k*fj; + qj = s_sot_k*qj; + gj = s_sot_k*gj; + end + % Add contribution to the end state, Attention qj changes with time scaling! + Xk_end = Xk_end + D(j+1)*X_ki_stages{j}; + % Add contribution to quadrature function + J = J + B(j+1)*qj*h_ki; + % Append IRK equations to NLP constraint + problem = add_constraint(problem, h_ki*fj - xp, zeros(n_x, 1), zeros(n_x, 1)); + + case 'differential' + % Evaluate Differential and Algebraic Equations at stage points + if n_u > 0 + [fj, qj] = f_x_fun(X_ki_stages{j},Z_ki_stages{j},Uk); + gj = g_z_all_fun(X_ki_stages{j},Z_ki_stages{j},Uk); + else + [fj, qj] = f_x_fun(X_ki_stages{j},Z_ki_stages{j}); + gj = g_z_all_fun(X_ki_stages{j},Z_ki_stages{j}); + end + + if time_rescaling && use_speed_of_time_variables + % rescale equations + fj = s_sot_k*fj; + qj = s_sot_k*qj; + gj = s_sot_k*gj; + end + % Add contribution to the end state and quadrature term + if use_fesd + Xk_end = Xk_end + h_ki*b_irk(j)*V_ki_stages{j}; + J = J + h_ki*b_irk(j)*qj; + else + Xk_end = Xk_end + h_k(k+1)*b_irk(j)*V_ki_stages{j}; + J = J + h_k(k+1)*b_irk(j)*qj; + end + % Append IRK equations (differential part) to NLP constraint, note that we dont distingiush if use_fesd is on/off + % since it was done in the defintion of X_ki_stages which enters f_j + problem = add_constraint(problem, fj - V_ki_stages{j}, zeros(n_x, 1), zeros(n_x, 1)); - if time_rescaling && use_speed_of_time_variables - % rescale equations - fj = s_sot_k*fj; - qj = s_sot_k*qj; - gj = s_sot_k*gj; - end - % Add contribution to the end state and quadrature term - if use_fesd - Xk_end = Xk_end + h_ki*b_irk(j)*V_ki_stages{j}; - J = J + h_ki*b_irk(j)*qj; - else - Xk_end = Xk_end + h_k(k+1)*b_irk(j)*V_ki_stages{j}; - J = J + h_k(k+1)*b_irk(j)*qj; - end - % Append IRK equations (differential part) to NLP constraint, note that we dont distingiush if use_fesd is on/off - % since it was done in the defintion of X_ki_stages which enters f_j - g = {g{:}, fj - V_ki_stages{j}}; - - % lifting considerations - if lift_irk_differential - g = {g{:}, X_ki_lift{j}}; - lbg = [lbg; zeros(n_x,1)]; - ubg = [ubg; zeros(n_x,1)]; - end - if x_box_at_stg && ~lift_irk_differential - g = {g{:};X_ki_stages{j}}; - lbg = [lbg; lbx]; - ubg = [ubg; ubx]; - end + % lifting considerations + if lift_irk_differential + problem = add_constraint(problem, X_ki_lift{j}, zeros(n_x, 1), zeros(n_x, 1)); + end + if x_box_at_stg && ~lift_irk_differential + problem = add_constraint(problem, X_ki_lift{j}, lbx, ubx); + end end % Append IRK equations (algebraic part) to NLP constraint (same for both representations) - g = {g{:}, gj}; - lbg = [lbg; zeros(n_x,1); zeros(n_algebraic_constraints,1)]; - ubg = [ubg; zeros(n_x,1); zeros(n_algebraic_constraints,1)]; + problem = add_constraint(problem, gj, zeros(n_algebraic_constraints,1), zeros(n_algebraic_constraints,1)); %% General nonlinear constraint at stage points if g_ineq_constraint && g_ineq_at_stg % indepednet of the fact is it lifter od not in the % differential case - g_ineq_k = g_ineq_fun(X_ki_stages{j},Uk); - g = {g{:}, g_ineq_k}; - lbg = [lbg; g_ineq_lb]; - ubg = [ubg; g_ineq_ub]; + problem = add_constraint(problem, g_ineq_fun(X_ki_stages{j},Uk), g_ineq_lb, g_ineq_ub); end - + % path complementarity constraints if g_comp_path_constraint && g_ineq_at_stg - g_comp_path_k = g_comp_path_fun(X_ki_stages{j},Uk)-p(1); - g = {g{:}, g_comp_path_k}; - lbg = [lbg; g_comp_path_lb]; - ubg = [ubg; g_comp_path_ub]; + problem = add_constraint(problem, g_comp_path_fun(X_ki_stages{j},Uk)-p(1), g_comp_path_lb, g_comp_path_ub); end %% Complementarity constraints (standard and cross) @@ -636,12 +568,12 @@ end if s_ell_1_elastic_exists s_elastic_ell_1 = define_casadi_symbolic(casadi_symbolic_mode,['s_elastic_' num2str(k) '_' num2str(i) '_' num2str(j)], n_all_comp_j); - w = {w{:}, s_elastic_ell_1}; - ind_elastic = [ind_elastic,ind_total(end)+1:ind_total(end)+n_all_comp_j]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+n_all_comp_j]; - lbw = [lbw; s_elastic_min*ones(n_all_comp_j,1)]; - ubw = [ubw; s_elastic_max*ones(n_all_comp_j,1)]; - w0 = [w0;s_elastic_0*ones(n_all_comp_j,1)]; + problem = add_variable(problem,... + s_elastic_ell_1,... + s_elastic_0*ones(n_all_comp_j,1),... + s_elastic_min*ones(n_all_comp_j,1),... + s_elastic_max*ones(n_all_comp_j,1),... + 'elastic'); % sum of all elastic variables, to be passed penalized sum_s_elastic = sum_s_elastic+ sum(s_elastic_ell_1); % pass to constraint creation @@ -649,14 +581,12 @@ end [J,g_comp,g_comp_lb,g_comp_ub] = reformulate_mpcc_constraints(objective_scaling_direct,mpcc_mode,mpcc_var_current_fe,dimensions,current_index); - g = {g{:}, g_comp}; - lbg = [lbg; g_comp_lb]; - ubg = [ubg; g_comp_ub]; + problem = add_constraint(problem, g_comp, g_comp_lb, g_comp_ub); end %% Step equilibration if use_fesd -% step_equilibration_constrains; + % step_equilibration_constrains; % define the switching indicator function for previous node/finite element boundary if (k > 0 || i > 0) % backward sums at current stage k are equal to the forward sums at stage previous stage (k-1) @@ -687,23 +617,19 @@ J_regularize_h = J_regularize_h + (nu_ki)*delta_h_ki^2; elseif strcmpi(step_equilibration,'direct') % step equilbiration as hard equality constraint - g = {g{:}, nu_ki_scaled*delta_h_ki}; - lbg = [lbg; 0]; - ubg = [ubg; 0]; + problem = add_constraint(problem, nu_ki_scaled*delta_h_ki, 0, 0); elseif strcmpi(step_equilibration,'direct_homotopy') - g = {g{:}, [nu_ki*delta_h_ki-sigma_p;-nu_ki*delta_h_ki-sigma_p]}; - lbg = [lbg; -inf;-inf]; - ubg = [ubg; 0;0]; + problem = add_constraint(problem,... + [nu_ki*delta_h_ki-sigma_p;-nu_ki*delta_h_ki-sigma_p],... + [-inf;-inf],... + [0;0]); elseif strcmpi(step_equilibration,'direct_homotopy_lift') - nu_ki_lift = define_casadi_symbolic(casadi_symbolic_mode,'nu_ki_lift ',1); - w = {w{:}, nu_ki_lift }; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+1]; - w0 = [w0;1]; - lbw = [lbw; -inf]; - ubw = [ubw; inf]; - g = {g{:}, [nu_ki-nu_ki_lift;nu_ki_lift*delta_h_ki-sigma_p;-nu_ki_lift*delta_h_ki-sigma_p]}; - lbg = [lbg;0;-inf;-inf]; - ubg = [ubg;0;0;0]; + nu_ki_lift = define_casadi_symbolic(casadi_symbolic_mode,'nu_ki_lift ',1); + problem = add_variable(problem, nu_ki_lift, 1, -inf, inf); + problem = add_constraint(problem,... + [nu_ki-nu_ki_lift;nu_ki_lift*delta_h_ki-sigma_p;-nu_ki_lift*delta_h_ki-sigma_p],... + [0;-inf;-inf],... + [0;0;0]); else error('Invalid step_equlibration mode, please pick a valid option, e.g., ''l2_relaxed_scaled'' or ''heuristic_mean'''); end @@ -712,43 +638,28 @@ %% Continuity condition - new NLP variable for state at end of a finite element % Conntinuity conditions for differential state X_ki = define_casadi_symbolic(casadi_symbolic_mode,['X_' num2str(k+1) '_' num2str(i+1) ],n_x); - w = {w{:}, X_ki}; - w0 = [w0; x0]; - - ind_x = [ind_x,ind_total(end)+1:ind_total(end)+n_x]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+n_x]; % box constraint always done at control interval boundary if x_box_at_fe - lbw = [lbw; lbx]; - ubw = [ubw; ubx]; + problem = add_variable(problem, X_ki, x0, lbx, ubx, 'x'); else - lbw = [lbw; -inf*ones(n_x,1)]; - ubw = [ubw; -inf*ones(n_x,1)]; + problem = add_variable(problem, X_ki, x0, -inf*ones(n_x,1), inf*ones(n_x,1), 'x'); end % Add equality constraint - g = {g{:}, Xk_end-X_ki}; - lbg = [lbg; zeros(n_x,1)]; - ubg = [ubg; zeros(n_x,1)]; + problem = add_constraint(problem, Xk_end-X_ki, zeros(n_x,1), zeros(n_x,1)); %% Evaluate inequality constraints at finite elements boundaries % TODO?: This should be removed? the left boundary point is treated % after control defintion, the very last right point should be treated in the terminal constraint if g_ineq_constraint && g_ineq_at_fe && i= 5 && mpcc_mode < 8 % add elastic variable to the vector of unknowns and add objective contribution - w = {w{:}, s_elastic}; - ind_elastic = [ind_elastic,ind_total(end)+1:ind_total(end)+1]; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+1]; - lbw = [lbw; s_elastic_min]; - ubw = [ubw; s_elastic_max]; - w0 = [w0;s_elastic_0]; + problem = add_variable(problem, s_elastic, s_elastic_0, s_elastic_min, s_elastic_max, 'elastic'); if objective_scaling_direct J = J + (1/sigma_p)*s_elastic; @@ -1029,9 +909,6 @@ if mpcc_mode >= 8 && mpcc_mode <= 10 rho_elastic = define_casadi_symbolic(casadi_symbolic_mode,'rho_elastic',1); - w = {w{:}, rho_elastic}; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+1]; - s_elastic_min = 1e-16; s_elastic_max = inf; rho_0 = max(rho_min,0.5); @@ -1043,28 +920,19 @@ end rho_max = (log(rho_scale)-log(s_elastic_min))/rho_lambda; - lbw = [lbw; rho_min]; - ubw = [ubw; rho_max]; - w0 = [w0;rho_0]; + problem = add_variable(problem, rho_elastic, rho_0, rho_min, rho_max); if nonlinear_sigma_rho_constraint if convex_sigma_rho_constraint - g = {g{:}, -rho_scale*exp(-rho_lambda*rho_elastic)+s_elastic}; + problem = add_constraint(problem, -rho_scale*exp(-rho_lambda*rho_elastic)+s_elastic, 0, inf); else - g = {g{:}, rho_scale*exp(-rho_lambda*rho_elastic)-s_elastic}; + problem = add_constraint(problem, rho_scale*exp(-rho_lambda*rho_elastic)-s_elastic, 0, inf); end else - g = {g{:}, -(rho_elastic-rho_max)-s_elastic}; + problem = add_constraint(problem, -(rho_elastic-rho_max)-s_elastic, 0, inf); end - lbg = [lbg; 0]; - ubg = [ubg; inf]; - % add elastic variable to the vector of unknowns and add objective contribution - w = {w{:}, s_elastic}; - ind_total = [ind_total,ind_total(end)+1:ind_total(end)+1]; - lbw = [lbw; s_elastic_min]; - ubw = [ubw; s_elastic_max]; - w0 = [w0;s_elastic_0]; + problem = add_variable(problem, s_elastic, s_elastic_0, s_elastic_min, s_elastic_max); J = J-rho_penalty*(rho_elastic^2)+sigma_penalty*s_elastic; end %% Elastic mode variable for \ell_1 reformulations @@ -1080,8 +948,8 @@ J = J + rho_h_p*J_regularize_h; %% CasADi Functions for objective complementarity residual -w = vertcat(w{:}); % vectorize all variables -g = vertcat(g{:}); % vectorize all constraint functions +w = vertcat(problem.w{:}); % vectorize all variables, TODO: again, further cleanup necessary +g = vertcat(problem.g{:}); % vectorize all constraint functions J_fun = Function('J_fun', {w} ,{J_objective}); comp_res = Function('comp_res',{w, p},{J_comp}); comp_res_fesd = Function('comp_res_fesd',{w},{J_comp_fesd}); @@ -1121,43 +989,43 @@ disp("g") print_casadi_vector(g) disp('lbg, ubg') - disp([lbg, ubg]) + disp([length(problem.lbg), length(problem.ubg)]) + disp([problem.lbg, problem.ubg]) disp("w") print_casadi_vector(w) disp('lbw, ubw') - disp([lbw, ubw]) + disp([problem.lbw, problem.ubw]) disp('w0') - disp(w0) + disp(problem.w0) disp('objective') disp(J) end %% Model update: all index sets and dimensions -model.ind_x = ind_x; -model.ind_elastic = ind_elastic; -model.ind_v = ind_v; -model.ind_z = ind_z; -model.ind_u = ind_u; -model.ind_h = ind_h; -model.ind_vf = ind_vf; -model.ind_g_clock_state = ind_g_clock_state; -model.ind_sot = ind_sot; -model.ind_t_final = ind_t_final; -model.ind_boundary = ind_boundary; +% TODO: Maybe just return the problem, currently trying not to break compatibility for now. +model.ind_x = problem.ind_x; +model.ind_elastic = problem.ind_elastic; +model.ind_v = problem.ind_v; +model.ind_z = problem.ind_z; +model.ind_u = problem.ind_u; +model.ind_h = problem.ind_h; +model.ind_g_clock_state = problem.ind_g_clock_state; +model.ind_sot = problem.ind_sot; +model.ind_t_final = problem.ind_t_final; +model.ind_boundary = problem.ind_boundary; model.n_cross_comp = n_cross_comp; -model.ind_total = ind_total; model.h = h; model.h_k = h_k; model.p_val = p_val; model.n_cross_comp_total = sum(n_cross_comp(:)); %% Store solver initialization data -solver_initialization.w0 = w0; -solver_initialization.lbw = lbw; -solver_initialization.ubw = ubw; -solver_initialization.lbg = lbg; -solver_initialization.ubg = ubg; +solver_initialization.w0 = problem.w0; +solver_initialization.lbw = problem.lbw; +solver_initialization.ubw = problem.ubw; +solver_initialization.lbg = problem.lbg; +solver_initialization.ubg = problem.ubg; %% Output varargout{1} = solver; varargout{2} = solver_initialization;