diff --git a/+nosnoc/+dcs/Base.m b/+nosnoc/+dcs/Base.m index fb1c2610..32665257 100644 --- a/+nosnoc/+dcs/Base.m +++ b/+nosnoc/+dcs/Base.m @@ -1,20 +1,28 @@ classdef Base < matlab.mixin.Scalar & handle & matlab.mixin.CustomDisplay +% Base class for Dynamic Complementarity Systems of the (most generic) form +% +% .. math:: +% :nowrap: +% +% \begin{align*} +% \dot{x}&= f(x,z) \\ +% 0 &\le h(x,z) \perp z \ge 0 +% \end{align*} properties model - f_x_fun % CasADi function - Related to DCS - f_q_fun % CasADi function - Related to DCS - g_z_fun % CasADi function - Related to DCS - g_alg_fun % CasADi function - Related to DCS - % No - g_path_fun % CasADi function - Only defined in an OCP - G_path_fun % CasADi function - Only defined in an OCP - H_path_fun % CasADi function - Only defined in an OCP - g_terminal_fun % CasADi function - Only defined in an OCP - f_q_T_fun % CasADi function - Only defined in an OCP - f_lsq_x_fun % CasADi function - Only defined in an OCP - f_lsq_u_fun % CasADi function - Only defined in an OCP - f_lsq_T_fun % CasADi function - Only defined in an OCP + f_x_fun % casadi.Function: Right hand side of the DCS. + f_q_fun % casadi.Function: Lagrange cost term function. + g_z_fun % casadi.Function: User algebraic constraints cost function. + g_alg_fun % casadi.Function: Constraints for algorithmically defined algebraics. + g_path_fun % casadi.Function: Function for non-box path constraints. Only defined in an OCP. + G_path_fun % casadi.Function: Function for one half of path complementarity. Only defined in an OCP. + H_path_fun % casadi.Function: Function for one half of path complementarity. Only defined in an OCP. + g_terminal_fun % casadi.Function: Function for terminal constraints. Only defined in an OCP. + f_q_T_fun % casadi.Function: Function for Mayer cost term. Only defined in an OCP. + f_lsq_x_fun % casadi.Function: Function for least squares cost on the differential state. Only defined in an OCP. + f_lsq_u_fun % casadi.Function: Function for least squares cost on the controls. Only defined in an OCP. + f_lsq_T_fun % casadi.Function: Function for least squares cost on the terminal state. Only defined in an OCP. end methods(Abstract) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m new file mode 100644 index 00000000..4efcbdbb --- /dev/null +++ b/+nosnoc/+dcs/Gcs.m @@ -0,0 +1,102 @@ +classdef Gcs < nosnoc.dcs.Base +% A particular class of dynamic complementarity system called a gradient complementarity system. +% It is defined by the dynamics: +% +% .. math:: +% :nowrap: +% +% \begin{align*} +% \dot{x} &= f(x) + \nabla c(x)\lambda \\ +% 0 &\le c(x) \perp \lambda \ge 0 +% \end{align*} + properties + lambda % casadi.SX|casadi.MX: Lagrange multipliers for the gradient complementarity system. + c_lift % casadi.SX|casadi.MX: Lift variables for the gap functions in :class:`nosnoc.model.Pds`. + + f_x % casadi.SX or casadi.MX: Right hand side of the gradient complementarity system + + dims % struct: dimensions struct TODO document members + + f_unconstrained_fun % casadi.Function: Unconstrained dynamics. + nabla_c_fun % casadi.Function: Gradient of the gap functions. + c_fun % casadi.Function: Gap Functions. + g_c_lift_fun % casadi.Function: Lifting constraints for the gap functions in :class:`nosnoc.model.Pds`. + end + + methods + function obj = Gcs(model) + obj.model = model; + obj.dims = model.dims; + end + + function generate_variables(obj, opts) + import casadi.* + dims = obj.dims; + % dimensions + dims.n_lambda = dims.n_c; + if opts.gcs_lift_gap_functions + dims.n_c_lift = dims.n_c; + else + dims.n_c_lift = 0; + end + obj.lambda = define_casadi_symbolic(opts.casadi_symbolic_mode,'lambda',dims.n_lambda); + obj.c_lift = define_casadi_symbolic(opts.casadi_symbolic_mode,'c_lift',dims.n_c_lift); + + obj.dims = dims; + end + + function generate_equations(obj, opts) + import casadi.* + model = obj.model; + dims = obj.dims; + + nabla_c = model.c.jacobian(model.x)'; + if opts.gcs_lift_gap_functions + g_c_lift = obj.c_lift - model.c; + c = obj.c_lift; + else + g_c_lift = []; + c = model.c; + end + + obj.f_x = model.f_x_unconstrained + model.E*nabla_c*obj.lambda; + + obj.f_x_fun = Function('f_x', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {obj.f_x}); + obj.f_unconstrained_fun = Function('f_unconstrained', {model.x, model.z, model.u, model.v_global, model.p}, {model.f_x_unconstrained}); + obj.f_q_fun = Function('f_q', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {model.f_q}); + obj.g_z_fun = Function('g_z', {model.x, model.z, model.u, model.v_global, model.p}, {model.g_z}); + obj.g_alg_fun = Function('g_alg', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {[]}); + obj.c_fun = Function('c_fun', {model.x, obj.c_lift, model.z, model.v_global, model.p}, {c}); + obj.g_c_lift_fun = Function('c_fun', {model.x, obj.c_lift, model.z, model.v_global, model.p}, {g_c_lift}); + obj.nabla_c_fun = Function('c_fun', {model.x, model.z, model.v_global, model.p}, {nabla_c}); + obj.g_path_fun = Function('g_path', {model.x, model.z, model.u, model.v_global, model.p}, {model.g_path}); % TODO(@anton) do dependence checking for spliting the path constriants + obj.G_path_fun = Function('G_path', {model.x, model.z, model.u, model.v_global, model.p}, {model.G_path}); + obj.H_path_fun = Function('H_path', {model.x, model.z, model.u, model.v_global, model.p}, {model.H_path}); + obj.g_terminal_fun = Function('g_terminal', {model.x, model.z, model.v_global, model.p_global}, {model.g_terminal}); + obj.f_q_T_fun = Function('f_q_T', {model.x, model.z, model.v_global, model.p}, {model.f_q_T}); + obj.f_lsq_x_fun = Function('f_lsq_x_fun',{model.x,model.x_ref,model.p},{model.f_lsq_x}); + obj.f_lsq_u_fun = Function('f_lsq_u_fun',{model.u,model.u_ref,model.p},{model.f_lsq_u}); + obj.f_lsq_T_fun = Function('f_lsq_T_fun',{model.x,model.x_ref_end,model.p_global},{model.f_lsq_T}); + end + end + + methods(Access=protected) + function propgrp = getPropertyGroups(obj) + propgrp = getPropertyGroups@nosnoc.dcs.Base(obj); + group_title = 'Variables'; + var_list = struct; + var_list.x = obj.model.x; + if ~any(size(obj.model.u) == 0) + var_list.u = obj.model.u; + end + if ~any(size(obj.model.z) == 0) + var_list.z = obj.model.z; + end + var_list.lambda = obj.lambda; + if ~any(size(obj.c_lift) == 0) + var_list.c_lift = obj.c_lift; + end + propgrp(2) = matlab.mixin.util.PropertyGroup(var_list, group_title); + end + end +end diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m new file mode 100644 index 00000000..97455b45 --- /dev/null +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -0,0 +1,895 @@ +classdef Gcs < vdx.problems.Mpcc + properties + model + dcs + opts + + populated = false + sorted = false + end + + methods + function obj = Gcs(dcs, opts) + obj = obj@vdx.problems.Mpcc(); + obj.model = dcs.model; + obj.dcs = dcs; + obj.opts = opts; + end + + function create_variables(obj) + dims = obj.dcs.dims; + dcs = obj.dcs; + model = obj.model; + opts = obj.opts; + + % Parameters + obj.p.rho_h_p = {{'rho_h_p',1}, 1}; + obj.p.rho_terminal_p = {{'rho_terminal_p',1}, 1}; + obj.p.T = {{'T',1}, opts.T}; + obj.p.p_global = {model.p_global, model.p_global_val}; + + + % 0d vars: Variables which only exist once globally. + obj.w.v_global = {{'v_global',dims.n_v_global}, model.lbv_global, model.ubv_global, model.v0_global}; + if opts.use_speed_of_time_variables && ~opts.local_speed_of_time_variable + obj.w.sot = {{'sot', 1}, opts.s_sot_min, opts.s_sot_max, opts.s_sot0}; + end + if opts.time_optimal_problem + obj.w.T_final = {{'T_final', 1}, opts.T_final_min, opts.T_final_max, opts.T}; + obj.f = obj.f + obj.w.T_final(); + end + + % 1d vars: Variables that are defined per control stage + obj.w.u(1:opts.N_stages) = {{'u', dims.n_u}, model.lbu, model.ubu, model.u0}; + obj.p.p_time_var(1:opts.N_stages) = {{'p_time_var', dims.n_p_time_var}, model.p_time_var_val}; + if opts.use_speed_of_time_variables && opts.local_speed_of_time_variable + obj.w.sot(1:opts.N_stages) = {{'sot', 1}, opts.s_sot_min, opts.s_sot_max, opts.s_sot0}; + end + + % TODO(@anton) This _severely_ hurts performance over the vectorized assignment by doing N_stages vertcats of + % casadi symbolics vs just a vectorized assignment which does one. As such there needs to be backend + % work done for vdx to cache vertcats of SX somehow. Current theory is one can simply keep a queue of + % symbolics to be added in a cell array until a read is done, at which point we call a single vertcat + % on the whole queue which is _significantly_ faster. + % 2d vars: Variables that are defined for each finite element. + for ii=1:opts.N_stages + % other derived values + h0 = opts.h_k(ii); + if obj.opts.use_fesd + ubh = (1 + opts.gamma_h) * h0; + lbh = (1 - opts.gamma_h) * h0; + if opts.time_rescaling && ~opts.use_speed_of_time_variables + % if only time_rescaling is true, speed of time and step size all lumped together, e.g., \hat{h}_{k,i} = s_n * h_{k,i}, hence the bounds need to be extended. + ubh = (1+opts.gamma_h)*h0*opts.s_sot_max; + lbh = (1-opts.gamma_h)*h0/opts.s_sot_min; + end + obj.w.h(ii,1:opts.N_finite_elements(ii)) = {{'h', 1}, lbh, ubh, h0}; + end + if obj.opts.step_equilibration == StepEquilibrationMode.linear_complementarity + obj.w.B_max(ii,2:opts.N_finite_elements(ii)) = {{'B_max', dims.n_lambda},-inf,inf}; + % forward sum of c(x) or backward sum of c(x). + obj.w.pi_c(ii,2:opts.N_finite_elements(ii)) = {{'pi_c', dims.n_c},-inf,inf}; + % forward sum of lambda or backward sum of lambda. + obj.w.pi_lambda(ii,2:opts.N_finite_elements(ii)) = {{'pi_lambda', dims.n_lambda},-inf,inf}; + % multipliers for c(x) in max kkt conditions. + obj.w.c_mult(ii,2:opts.N_finite_elements(ii)) = {{'c_mult', dims.n_c},0,inf}; + % multipliers for lambda in max kkt conditions. + obj.w.lambda_mult(ii,2:opts.N_finite_elements(ii)) = {{'lambda_mult', dims.n_lambda},0,inf}; + % pi_c and pi_lambda, i.e. vector of switch indicators. + obj.w.eta(ii,2:opts.N_finite_elements(ii)) = {{'eta', dims.n_lambda},0,inf}; + % an or of all the switch indicators. + obj.w.nu(ii,2:opts.N_finite_elements(ii)) = {{'nu', 1},0,inf}; + end + end + + % For c_n ~= 1 case + rbp = ~opts.right_boundary_point_explicit; + + % 3d vars: Variables defined on each rk stage + % some of which are also defined at the initial point: + obj.w.x(0,0,opts.n_s) = {{['x_0'], dims.n_x}, model.x0, model.x0, model.x0}; % differential state + obj.w.z(0,0,opts.n_s) = {{'z', dims.n_z}, model.lbz, model.ubz, model.z0}; % user algebraics + obj.w.lambda(0,0,opts.n_s) = {{['lambda'], dims.n_lambda},0,inf, opts.initial_lambda_gcs}; + obj.w.c_lift(0,0,opts.n_s) = {{['c_lift'], dims.n_c_lift},-inf,inf}; + for ii=1:opts.N_stages + obj.w.x(ii,1:opts.N_finite_elements(ii),1:(opts.n_s+rbp)) = {{'x', dims.n_x}, model.lbx, model.ubx, model.x0}; + if opts.rk_representation == RKRepresentation.differential_lift_x + obj.w.v(ii,1:opts.N_finite_elements(ii),1:opts.n_s) = {{'v', dims.n_x}}; % v = f_x + end + + obj.w.z(ii,1:opts.N_finite_elements(ii),1:(opts.n_s+rbp)) = {{'z', dims.n_z}, model.lbz, model.ubz, model.z0}; + obj.w.lambda(ii,1:opts.N_finite_elements(ii),1:(opts.n_s+rbp)) = {{'lambda', dims.n_lambda},0, inf, opts.initial_lambda_gcs}; + obj.w.c_lift(ii,1:opts.N_finite_elements(ii),1:(opts.n_s+rbp)) = {{'c_lift', dims.n_c_lift},-inf,inf}; + % Handle x_box settings + if ~opts.x_box_at_stg + obj.w.x(1:opts.N_stages,1:opts.N_finite_elements(ii),1:(opts.n_s+rbp-1)).lb = -inf*ones(dims.n_x, 1); + obj.w.x(1:opts.N_stages,1:opts.N_finite_elements(ii),1:(opts.n_s+rbp-1)).ub = inf*ones(dims.n_x, 1); + end + + if ~opts.x_box_at_fe + obj.w.x(1:opts.N_stages,1:(opts.N_finite_elements(ii)-1),opts.n_s+rbp).lb = -inf*ones(dims.n_x, 1); + obj.w.x(1:opts.N_stages,1:(opts.N_finite_elements(ii)-1),opts.n_s+rbp).ub = inf*ones(dims.n_x, 1); + end + end + end + + function generate_direct_transcription_constraints(obj) + import casadi.* + model = obj.model; + opts = obj.opts; + dcs = obj.dcs; + dims = obj.dcs.dims; + + rbp = ~opts.right_boundary_point_explicit; + + v_global = obj.w.v_global(); + p_global = obj.p.p_global(); + + % Recall that we treat (0,0,n_s) as the initial point. This is done for ergonomics in extracting results. + x_0 = obj.w.x(0,0,opts.n_s); + z_0 = obj.w.z(0,0,opts.n_s); + c_lift_0 = obj.w.c_lift(0,0,opts.n_s); + lambda_0 = obj.w.lambda(0,0,opts.n_s); + + obj.g.z(0,0,opts.n_s) = {dcs.g_z_fun(x_0, z_0, obj.w.u(1), v_global, [p_global;obj.p.p_time_var(1)])}; + obj.g.c_lb(0,0,opts.n_s) = {dcs.c_fun(x_0, c_lift_0, z_0, v_global, [p_global;obj.p.p_time_var(1)]), 0, inf}; + obj.g.c_lift(0,0,opts.n_s) = {dcs.g_c_lift_fun(x_0, c_lift_0, z_0, v_global, [p_global;obj.p.p_time_var(1)])}; + + x_prev = obj.w.x(0,0,opts.n_s); + for ii=1:opts.N_stages + h0 = obj.p.T().val/(opts.N_stages*opts.N_finite_elements(ii)); + + u_i = obj.w.u(ii); + p_stage = obj.p.p_time_var(ii); + p = [p_global;p_stage]; + if obj.opts.use_speed_of_time_variables && opts.local_speed_of_time_variable + s_sot = obj.w.sot(ii); + elseif obj.opts.use_speed_of_time_variables + s_sot = obj.w.sot(); + else + s_sot = 1; + end + if opts.time_optimal_problem && ~opts.use_speed_of_time_variables + t_stage = obj.w.T_final()/(opts.N_stages*opts.N_finite_elements(ii)); + elseif opts.time_optimal_problem + t_stage = s_sot*obj.p.T()/opts.N_stages; + else + t_stage = obj.p.T()/opts.N_stages; + end + + sum_h = 0; + for jj=1:opts.N_finite_elements(ii) + if obj.opts.use_fesd + h = obj.w.h(ii,jj); + sum_h = sum_h + h; + elseif opts.time_optimal_problem && ~opts.use_speed_of_time_variables + h = obj.w.T_final()/(opts.N_stages*opts.N_finite_elements(ii)); + else + h = h0; + end + switch opts.rk_representation + case RKRepresentation.integral + % In integral representation stage variables are states. + x_ij_end = opts.D_rk(1)*x_prev; + for kk=1:opts.n_s + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + lambda_ijk = obj.w.lambda(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + + f_ijk = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, u_i, v_global, p); + q_ijk = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, u_i, v_global, p); + xk = opts.C_rk(1, kk+1) * x_prev; + for rr=1:opts.n_s + x_ijr = obj.w.x(ii,jj,rr); + xk = xk + opts.C_rk(rr+1, kk+1) * x_ijr; + end + obj.g.dynamics(ii,jj,kk) = {h * f_ijk - xk}; + obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + obj.g.c_lb(ii,jj,kk) = {dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p), 0, inf}; + obj.g.c_lift(ii,jj,kk) = {dcs.g_c_lift_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p)}; + + x_ij_end = x_ij_end + opts.D_rk(kk+1)*x_ijk; + + if opts.g_path_at_stg + obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; + end + if size(model.G_path, 1) > 0 + G_path = dcs.G_path_fun(x_ijk, z_ijk, u_i, v_global, p); + H_path = dcs.H_path_fun(x_ijk, z_ijk, u_i, v_global, p); + obj.G.path(ii,jj,kk) = {G_path}; + obj.H.path(ii,jj,kk) = {H_path}; + end + if opts.cost_integration + % also integrate the objective + obj.f = obj.f + opts.B_rk(kk+1)*h*q_ijk; + end + end + if ~opts.right_boundary_point_explicit + error("not implemented") + end + if ~opts.g_path_at_stg && opts.g_path_at_fe + obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; + end + case RKRepresentation.differential + error("Differential representation without lifting is unsupported for gradient complementarity systems") + + case RKRepresentation.differential_lift_x + % In differential representation with lifted state stage variables are the state derivatives and we + % lift the states at each stage point as well. + for kk = 1:opts.n_s + x_ijk = obj.w.x(ii,jj,kk); + x_temp = x_prev; + for rr = 1:opts.n_s + x_temp = x_temp + h*opts.A_rk(kk,rr)*obj.w.v(ii,jj,rr); + end + obj.g.lift_x(ii,jj,kk) = {x_ijk - x_temp}; + end + x_ij_end = x_prev; + for kk=1:opts.n_s + x_ijk = obj.w.x(ii,jj,kk); + v_ijk = obj.w.v(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + lambda_ijk = obj.w.lambda(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + + f_ijk = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, u_i, v_global, p); + q_ijk = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, u_i, v_global, p); + + x_ij_end = x_ij_end + h*opts.b_rk(kk)*v_ijk; + obj.g.v(ii,jj,kk) = {f_ijk - v_ijk}; + obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + obj.g.c_lb(ii,jj,kk) = {dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p), 0, inf}; + obj.g.c_lift(ii,jj,kk) = {dcs.g_c_lift_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p)}; + + if opts.g_path_at_stg + obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; + end + if size(model.G_path, 1) > 0 + G_path = dcs.G_path_fun(x_ijk, z_ijk, u_i, v_global, p); + H_path = dcs.H_path_fun(x_ijk, z_ijk, u_i, v_global, p); + obj.G.path(ii,jj,kk) = {G_path}; + obj.H.path(ii,jj,kk) = {H_path}; + end + if opts.cost_integration + % also integrate the objective + obj.f = obj.f + opts.b_rk(kk)*h*q_ijk; + end + end + if ~opts.g_path_at_stg && opts.g_path_at_fe + obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; + end + end + x_prev = obj.w.x(ii,jj,opts.n_s+rbp); + end + if ~opts.g_path_at_stg && ~opts.g_path_at_fe + x_i = obj.w.x(ii, opts.N_finite_elements(ii), opts.n_s); + z_i = obj.w.z(ii, opts.N_finite_elements(ii), opts.n_s); + obj.g.path(ii) = {dcs.g_path_fun(x_i, z_i, u_i, v_global, p), model.lbg_path, model.ubg_path}; + end + + % Least Squares Costs + % TODO we should convert the refs to params + if ~isempty(model.x_ref_val) + obj.f = obj.f + t_stage*dcs.f_lsq_x_fun(obj.w.x(ii,opts.N_finite_elements(ii),opts.n_s),... + model.x_ref_val(:,ii),... + p); + end + if ~isempty(model.u_ref_val) + obj.f = obj.f + t_stage*dcs.f_lsq_u_fun(obj.w.u(ii),... + model.u_ref_val(:,ii),... + p); + end + if ~opts.cost_integration + obj.f = obj.f + dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, u_i, v_global, p); + end + + % Clock Constraints + if obj.opts.use_fesd && opts.equidistant_control_grid + if opts.time_optimal_problem + if opts.use_speed_of_time_variables + obj.g.equidistant_control_grid(ii) = {[sum_h - opts.h;sot*sum_h - obj.w.T_final()/opts.N_stages]}; + else + obj.g.equidistant_control_grid(ii) = {sum_h - obj.w.T_final()/opts.N_stages}; + end + else + if opts.relax_terminal_numerical_time + % TODO(@armin) why is this allowed to be negative? + obj.w.s_numerical_time(ii) = {{'s_numerical', 1}, -2*opts.h, 2*opts.h, opts.h/2}; + g_eq_grid = [sum_h - t_stage - obj.w.s_numerical_time(ii); + -(sum_h - t_stage) - obj.w.s_numerical_time(ii)]; + obj.g.equidistant_control_grid(ii) = {g_eq_grid, -inf, 0}; + obj.f = obj.f + opts.rho_terminal_numerical_time*obj.w.s_numerical_time(ii); + else + obj.g.equidistant_control_grid(ii) = {t_stage-sum_h}; + end + end + end + end + + x_end = obj.w.x(opts.N_stages,opts.N_finite_elements(opts.N_stages),opts.n_s+rbp); + z_end = obj.w.z(opts.N_stages,opts.N_finite_elements(opts.N_stages),opts.n_s+rbp); + % Terminal cost + obj.f = obj.f + dcs.f_q_T_fun(x_end, z_end, v_global, p_global); + + % Terminal_lsq_cost + if ~isempty(model.x_ref_end_val) + obj.f = obj.f + h0*opts.N_finite_elements(ii)*dcs.f_lsq_T_fun(x_end,... + model.x_ref_end_val,... + p_global); + end + + % Terminal constraint + if opts.relax_terminal_constraint_homotopy + error("Currently unsupported") + end + g_terminal = dcs.g_terminal_fun(x_end, z_end, v_global, p_global); + switch opts.relax_terminal_constraint + case ConstraintRelaxationMode.NONE % hard constraint + if opts.relax_terminal_constraint_from_above + obj.g.terminal = {g_terminal, model.lbg_terminal, inf*ones(dims.n_g_terminal,1)}; + else + obj.g.terminal = {g_terminal, model.lbg_terminal, model.ubg_terminal}; + end + case ConstraintRelaxationMode.ELL_1 % l_1 + obj.w.s_terminal_ell_1 = {{'s_terminal_ell_1', dims.n_g_terminal}, 0, inf, 10}; + + g_terminal = [g_terminal-model.lbg_terminal-obj.w.s_terminal_ell_1(); + -(g_terminal-model.ubg_terminal)-obj.w.s_terminal_ell_1()]; + obj.g.terminal = {g_terminal, -inf, 0}; + obj.f = obj.f + obj.p.rho_terminal_p()*sum(obj.w.s_terminal_ell_1()); + case ConstraintRelaxationMode.ELL_2 % l_2 + % TODO(@anton): this is as it was implemented before. should handle lb != ub? + obj.f = obj.f + obj.p.rho_terminal_p()*(g_terminal-model.lbg_terminal)'*(g_terminal-model.lbg_terminal); + case ConstraintRelaxationMode.ELL_INF % l_inf + obj.w.s_terminal_ell_inf = {{'s_terminal_ell_inf', 1}, 0, inf, 1e3}; + + g_terminal = [g_terminal-model.lbg_terminal-obj.w.s_terminal_ell_inf(); + -(g_terminal-model.ubg_terminal)-obj.w.s_terminal_ell_inf()]; + obj.g.terminal = {g_terminal, -inf, 0}; + obj.f = obj.f + obj.p.rho_terminal_p()*obj.w.s_terminal_ell_inf(); + end + end + + function generate_complementarity_constraints(obj) + import casadi.* + opts = obj.opts; + dcs = obj.dcs; + model = obj.model; + % Do Cross-Complementarity + + rbp = ~opts.right_boundary_point_explicit; + + v_global = obj.w.v_global(); + p_global = obj.p.p_global(); + + x_prev = obj.w.x(0,0,opts.n_s); + z_prev = obj.w.z(0,0,opts.n_s); + lambda_prev = obj.w.lambda(0,0,opts.n_s); + c_lift_prev = obj.w.c_lift(0,0,opts.n_s); + c_prev = dcs.c_fun(x_prev, c_lift_prev, z_prev, v_global, [p_global;obj.p.p_time_var(1)]); + + if opts.use_fesd + switch opts.cross_comp_mode + case CrossCompMode.STAGE_STAGE + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p = [p_global;p_stage]; + for jj=1:opts.N_finite_elements(ii); + Gij = {}; + Hij = {}; + for rr=1:(opts.n_s + rbp) + x_ijr = obj.w.x(ii,jj,rr); + z_ijr = obj.w.z(ii,jj,rr); + c_lift_ijr = obj.w.c_lift(ii,jj,rr); + c_ijr = dcs.c_fun(x_ijr, c_lift_ijr, z_ijr, v_global, p); + + Gij = vertcat(Gij, {lambda_prev}); + Hij = vertcat(Hij, {c_ijr}); + end + for rr=1:(opts.n_s + rbp) + lambda_ijr = obj.w.lambda(ii,jj,rr); + + Gij = vertcat(Gij, {lambda_ijr}); + Hij = vertcat(Hij, {c_prev}); + end + for kk=1:(opts.n_s + rbp) + lambda_ijk = obj.w.lambda(ii,jj,kk); + for rr=1:(opts.n_s + rbp) + x_ijr = obj.w.x(ii,jj,rr); + z_ijr = obj.w.z(ii,jj,rr); + c_lift_ijr = obj.w.c_lift(ii,jj,rr); + c_ijr = dcs.c_fun(x_ijr, c_lift_ijr, z_ijr, v_global, p); + + Gij = vertcat(Gij, {lambda_ijk}); + Hij = vertcat(Hij, {c_ijr}); + end + end + obj.G.cross_comp(ii,jj) = {vertcat(Gij{:})}; + obj.H.cross_comp(ii,jj) = {vertcat(Hij{:})}; + lambda_prev = obj.w.lambda(ii,jj,opts.n_s + rbp); + x_prev = obj.w.x(ii,jj,opts.n_s + rbp); + z_prev = obj.w.z(ii,jj,opts.n_s + rbp); + c_lift_prev = obj.w.c_lift(ii,jj,opts.n_s + rbp); + c_prev = dcs.c_fun(x_prev, c_lift_prev, z_prev, v_global, p); + end + end + case CrossCompMode.FE_STAGE + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p = [p_global;p_stage]; + for jj=1:opts.N_finite_elements(ii); + sum_lambda = lambda_prev + sum2(obj.w.lambda(ii,jj,:)); + Gij = {sum_lambda}; + Hij = {c_prev}; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + + Gij = vertcat(Gij, {sum_lambda}); + Hij = vertcat(Hij, {c_ijk}); + end + obj.G.cross_comp(ii,jj) = {vertcat(Gij{:})}; + obj.H.cross_comp(ii,jj) = {vertcat(Hij{:})}; + lambda_prev = obj.w.lambda(ii,jj,opts.n_s + rbp); + x_prev = obj.w.x(ii,jj,opts.n_s + rbp); + z_prev = obj.w.z(ii,jj,opts.n_s + rbp); + c_lift_prev = obj.w.c_lift(ii,jj,opts.n_s + rbp); + c_prev = dcs.c_fun(x_prev, c_lift_prev, z_prev, v_global, p); + end + end + case CrossCompMode.STAGE_FE + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p = [p_global;p_stage]; + for jj=1:opts.N_finite_elements(ii); + sum_c = dcs.c_fun(x_prev); + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + sum_c = sum_c + c_ijk; + end + Gij = {lambda_prev}; + Hij = {sum_c}; + for kk=1:(opts.n_s + rbp) + lambda_ijk = obj.w.lambda(ii,jj,kk); + + Gij = vertcat(Gij, {lambda_ijk}); + Hij = vertcat(Hij, {sum_c}); + end + obj.G.cross_comp(ii,jj) = {vertcat(Gij{:})}; + obj.H.cross_comp(ii,jj) = {vertcat(Hij{:})}; + lambda_prev = obj.w.lambda(ii,jj,opts.n_s + rbp); + x_prev = obj.w.x(ii,jj,opts.n_s + rbp); + z_prev = obj.w.z(ii,jj,opts.n_s + rbp); + c_lift_prev = obj.w.c_lift(ii,jj,opts.n_s + rbp); + c_prev = dcs.c_fun(x_prev, c_lift_prev, z_prev, v_global, p); + end + end + case CrossCompMode.FE_FE + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p = [p_global;p_stage]; + for jj=1:opts.N_finite_elements(ii); + sum_lambda = lambda_prev + sum2(obj.w.lambda(ii,jj,:)); + sum_c = c_prev; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + sum_c = sum_c + c_ijk; + end + obj.G.cross_comp(ii,jj) = {sum_lambda}; + obj.H.cross_comp(ii,jj) = {sum_c}; + lambda_prev = obj.w.lambda(ii,jj,opts.n_s + rbp); + x_prev = obj.w.x(ii,jj,opts.n_s + rbp); + z_prev = obj.w.z(ii,jj,opts.n_s + rbp); + c_lift_prev = obj.w.c_lift(ii,jj,opts.n_s + rbp); + c_prev = dcs.c_fun(x_prev, c_lift_prev, z_prev, v_global, p); + end + end + end + else + obj.G.standard_comp(0,0,opts.n_s) = {lambda_prev}; + obj.H.standard_comp(0,0,opts.n_s) = {c_prev}; + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p = [p_global;p_stage]; + for jj=1:opts.N_finite_elements(ii); + Gij = {}; + Hij = {}; + for kk=1:(opts.n_s + rbp) + lambda_ijk = obj.w.lambda(ii,jj,kk); + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + + obj.G.standard_comp(ii,jj, kk) = {lambda_ijk}; + obj.H.standard_comp(ii,jj, kk) = {c_ijk}; + end + end + end + end + end + + function generate_step_equilibration_constraints(obj) + import casadi.* + model = obj.model; + dcs = obj.dcs; + opts = obj.opts; + dims = obj.dcs.dims; + v_global = obj.w.v_global(); + p_global = obj.p.p_global(); + + if ~opts.use_fesd % do nothing + return + end + rbp = ~opts.right_boundary_point_explicit; + + switch obj.opts.step_equilibration + case StepEquilibrationMode.heuristic_mean + for ii=1:opts.N_stages + for jj=1:opts.N_finite_elements(ii) + h0 = obj.p.T()/(opts.N_stages*opts.N_finite_elements(ii)); + obj.f = obj.f + obj.p.rho_h_p()*(h0-obj.w.h(ii,jj))^2; + end + end + case StepEquilibrationMode.heuristic_diff + for ii=1:opts.N_stages + for jj=2:opts.N_finite_elements(ii) + h0 = obj.p.T()/(opts.N_stages*opts.N_finite_elements(ii)); + obj.f = obj.f + obj.p.rho_h_p()*(obj.w.h(ii,jj)-obj.w.h(ii,jj-1))^2; + end + end + case StepEquilibrationMode.l2_relaxed_scaled + eta_vec = []; + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p =[p_global;p_stage]; + for jj=2:opts.N_finite_elements(ii) + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj-1,kk); + z_ijk = obj.w.z(ii,jj-1,kk); + c_lift_ijk = obj.w.c_lift(ii,jj-1,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + sigma_c_B = sigma_c_B + c_ijk; + sigma_lambda_B = sigma_lambda_B + obj.w.lambda(ii,jj-1,kk); + end + sigma_c_F = 0; + sigma_lambda_F = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + + sigma_c_F = sigma_c_F + c_ijk; + sigma_lambda_F = sigma_lambda_F + obj.w.lambda(ii,jj,kk); + end + + pi_c = sigma_c_B .* sigma_c_F; + pi_lam = sigma_lambda_B .* sigma_lambda_F; + nu = pi_c + pi_lam; + eta = 1; + for jjj=1:length(nu) + eta = eta*nu(jjj); + end + eta_vec = [eta_vec;eta]; + delta_h = obj.w.h(ii,jj) - obj.w.h(ii,jj-1); + obj.f = obj.f + obj.p.rho_h_p() * tanh(eta/opts.step_equilibration_sigma) * delta_h.^2; + end + end + case StepEquilibrationMode.l2_relaxed + eta_vec = []; + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p =[p_global;p_stage]; + for jj=2:opts.N_finite_elements(ii) + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj-1,kk); + z_ijk = obj.w.z(ii,jj-1,kk); + c_lift_ijk = obj.w.c_lift(ii,jj-1,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + sigma_c_B = sigma_c_B + c_ijk; + sigma_lambda_B = sigma_lambda_B + obj.w.lambda(ii,jj-1,kk); + end + sigma_c_F = 0; + sigma_lambda_F = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + + sigma_c_F = sigma_c_F + c_ijk; + sigma_lambda_F = sigma_lambda_F + obj.w.lambda(ii,jj,kk); + end + + pi_c = sigma_c_B .* sigma_c_F; + pi_lam = sigma_lambda_B .* sigma_lambda_F; + nu = pi_c + pi_lam; + eta = 1; + for jjj=1:length(nu) + eta = eta*nu(jjj); + end + eta_vec = [eta_vec;eta]; + delta_h = obj.w.h(ii,jj) - obj.w.h(ii,jj-1); + obj.f = obj.f + obj.p.rho_h_p() * eta * delta_h.^2 + end + end + case StepEquilibrationMode.direct + eta_vec = []; + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p =[p_global;p_stage]; + for jj=2:opts.N_finite_elements(ii) + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj-1,kk); + z_ijk = obj.w.z(ii,jj-1,kk); + c_lift_ijk = obj.w.c_lift(ii,jj-1,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + sigma_c_B = sigma_c_B + c_ijk; + sigma_lambda_B = sigma_lambda_B + obj.w.lambda(ii,jj-1,kk); + end + sigma_c_F = 0; + sigma_lambda_F = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + + sigma_c_F = sigma_c_F + c_ijk; + sigma_lambda_F = sigma_lambda_F + obj.w.lambda(ii,jj,kk); + end + + pi_c = sigma_c_B .* sigma_c_F; + pi_lam = sigma_lambda_B .* sigma_lambda_F; + nu = pi_c + pi_lam; + + eta = 1; + for jjj=1:length(nu) + eta = eta*nu(jjj); + end + eta_vec = [eta_vec;eta]; + delta_h = obj.w.h(ii,jj) - obj.w.h(ii,jj-1); + obj.g.step_equilibration(ii,jj) = {eta*delta_h, 0, 0}; + end + end + %obj.eta_fun = Function('eta_fun', {obj.w.sym}, {eta_vec}); + case StepEquilibrationMode.direct_homotopy + error("not currently implemented") + eta_vec = []; + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p =[p_global;p_stage]; + for jj=2:opts.N_finite_elements(ii) + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj-1,kk); + z_ijk = obj.w.z(ii,jj-1,kk); + c_lift_ijk = obj.w.c_lift(ii,jj-1,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + sigma_c_B = sigma_c_B + c_ijk; + sigma_lambda_B = sigma_lambda_B + obj.w.lambda(ii,jj-1,kk); + end + sigma_c_F = 0; + sigma_lambda_F = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + + sigma_c_F = sigma_c_F + c_ijk; + sigma_lambda_F = sigma_lambda_F + obj.w.lambda(ii,jj,kk); + end + + pi_c = sigma_c_B .* sigma_c_F; + pi_lam = sigma_lambda_B .* sigma_lambda_F; + nu = pi_c + pi_lam; + eta = 1; + for jjj=1:length(nu) + eta = eta*nu(jjj); + end + eta_vec = [eta_vec;eta]; + obj.eta_vec = eta_vec; + delta_h = obj.w.h(ii,jj) - obj.w.h(ii,jj-1); + homotopy_eq = [eta*delta_h - sigma;eta*delta_h + sigma]; + obj.g.step_equilibration(ii,jj) = {homotopy_eq, [-inf;0], [0;inf]}; + end + end + %obj.eta_fun = Function('eta_fun', {obj.w.sym}, {eta_vec}); + case StepEquilibrationMode.linear_complementarity + for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); + p =[p_global;p_stage]; + h0 = obj.p.T()/(opts.N_stages*opts.N_finite_elements(ii)); + for jj=2:opts.N_finite_elements(ii) + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj-1,kk); + z_ijk = obj.w.z(ii,jj-1,kk); + c_lift_ijk = obj.w.c_lift(ii,jj-1,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + sigma_c_B = sigma_c_B + c_ijk; + sigma_lambda_B = sigma_lambda_B + obj.w.lambda(ii,jj-1,kk); + end + sigma_c_F = 0; + sigma_lambda_F = 0; + for kk=1:(opts.n_s + rbp) + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_lift_ijk = obj.w.c_lift(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk, c_lift_ijk, z_ijk, v_global, p); + + sigma_c_F = sigma_c_F + c_ijk; + sigma_lambda_F = sigma_lambda_F + obj.w.lambda(ii,jj,kk); + end + + lambda_mult = obj.w.lambda_mult(ii,jj); + c_mult = obj.w.c_mult(ii,jj); + B_max = obj.w.B_max(ii,jj); + pi_lambda = obj.w.pi_lambda(ii,jj); + pi_c = obj.w.pi_c(ii,jj); + eta = obj.w.eta(ii,jj); + nu = obj.w.nu(ii,jj); + + obj.g.pi_lambda_or(ii,jj) = {[pi_lambda-sigma_lambda_F;pi_lambda-sigma_lambda_B;sigma_lambda_F+sigma_lambda_B-pi_lambda],0,inf}; + obj.g.pi_c_or(ii,jj) = {[pi_c-sigma_c_F;pi_c-sigma_c_B;sigma_c_F+sigma_c_B-pi_c],0,inf}; + + % kkt conditions for min B, B>=sigmaB, B>=sigmaF + kkt_max = [1-c_mult-lambda_mult; + B_max-pi_lambda; + B_max-pi_c]; + obj.g.kkt_max(ii,jj) = {kkt_max, + [0*ones(dims.n_lambda,1);0*ones(dims.n_lambda,1);0*ones(dims.n_lambda,1)], + [0*ones(dims.n_lambda,1);inf*ones(dims.n_lambda,1);inf*ones(dims.n_lambda,1)]}; + + obj.G.step_eq_kkt_max(ii,jj) = {[(B_max-pi_lambda);(B_max-pi_c)]}; + obj.H.step_eq_kkt_max(ii,jj) = {[lambda_mult;c_mult]}; + + % eta calculation + eta_const = [eta-pi_c;eta-pi_lambda;eta-pi_c-pi_lambda+B_max]; + obj.g.eta_const(ii,jj) = {eta_const, + [-inf*ones(dims.n_lambda,1);-inf*ones(dims.n_lambda,1);zeros(dims.n_lambda,1)], + [zeros(dims.n_lambda,1);zeros(dims.n_lambda,1);inf*ones(dims.n_lambda,1)]}; + + obj.g.nu_or(ii,jj) = {[nu-eta;sum(eta)-nu],0,inf}; + + % the actual step eq conditions + M=opts.linear_complemtarity_M; + delta_h = obj.w.h(ii,jj) - obj.w.h(ii,jj-1); + step_equilibration = [delta_h + (1/h0)*nu*M; + delta_h - (1/h0)*nu*M]; + obj.g.step_equilibration(ii,jj) = {step_equilibration,[0;-inf],[inf;0]}; + end + end + end + end + + function populate_problem(obj) + obj.create_variables(); + obj.generate_direct_transcription_constraints(); + obj.generate_complementarity_constraints(); + obj.generate_step_equilibration_constraints(); + + obj.populated = true; + end + + function create_solver(obj, solver_options, plugin, regenerate) + if ~obj.populated + obj.populate_problem(); + end + + if ~exist('plugin') + plugin = 'scholtes_ineq'; + end + + if ~exist('regenerate') + regenerate = false; + end + + % Sort by indices to recover almost block-band structure. + % TODO: Maybe it would be useful to do a custom sorting for FESD to make the problem maximally block band. + % This is almost certainly necessary if we want to take advantage of e.g. FATROP. + if ~obj.sorted + obj.w.sort_by_index(); + obj.g.sort_by_index(); + end + solver_options.assume_lower_bounds = true; + + if regenerate || isempty(obj.solver) || (nosnoc.solver.MpccMethod(plugin) ~= obj.solver.relaxation_type) + obj.solver = nosnoc.solver.mpccsol('Mpcc solver', plugin, obj.to_casadi_struct(), solver_options); + end + end + + function stats = solve(obj) + opts = obj.opts; + T_val = obj.p.T().val; + + if opts.use_fesd + for ii=1:opts.N_stages + for jj=1:opts.N_finite_elements(ii) + % Recalculate ubh and lbh based on T_val + h0 = T_val/(opts.N_stages*opts.N_finite_elements(ii)); + ubh = (1 + opts.gamma_h) * h0; + lbh = (1 - opts.gamma_h) * h0; + if opts.time_rescaling && ~opts.use_speed_of_time_variables + % if only time_rescaling is true, speed of time and step size all lumped together, e.g., \hat{h}_{k,i} = s_n * h_{k,i}, hence the bounds need to be extended. + ubh = (1+opts.gamma_h)*h0*opts.s_sot_max; + lbh = (1-opts.gamma_h)*h0/opts.s_sot_min; + end + obj.w.h(ii,jj).lb = lbh; + obj.w.h(ii,jj).ub = ubh; + end + end + end + + stats = solve@vdx.problems.Mpcc(obj); + end + + function results = get_results_struct(obj) + opts = obj.opts; + model = obj.model; + + rbp = ~opts.right_boundary_point_explicit; + + if opts.right_boundary_point_explicit + results.x = obj.discrete_time_problem.w.x(:,:,obj.opts.n_s).res; + results.z = obj.discrete_time_problem.w.z(:,:,obj.opts.n_s).res; + results.lambda = obj.discrete_time_problem.w.lambda(:,:,obj.opts.n_s).res; + else + results.x = [obj.discrete_time_problem.w.x(0,0,obj.opts.n_s).res,... + obj.discrete_time_problem.w.x(1:opts.N_stages,:,obj.opts.n_s+1).res]; + results.z = [obj.discrete_time_problem.w.z(0,0,obj.opts.n_s).res,... + obj.discrete_time_problem.w.z(1:opts.N_stages,:,obj.opts.n_s+1).res]; + results.lambda = [obj.discrete_time_problem.w.lambda(0,0,obj.opts.n_s).res,... + obj.discrete_time_problem.w.lambda(1:opts.N_stages,:,obj.opts.n_s+1).res]; + end + results.u = obj.w.u.res; + + % TODO also speed of time/etc here. + if opts.use_fesd + results.h = obj.w.h.res; + else + results.h = []; + for ii=1:opts.N_stages + h = obj.p.T.val/(opts.N_stages*opts.N_finite_elements(ii)); + results.h = [results.h,h*ones(1, opts.N_finite_elements(ii))]; + end + end + results.t_grid = cumsum([0, results.h]); + if ~isempty(results.u) + if opts.use_fesd + t_grid_u = [0]; + for ii=1:opts.N_stages + h_sum = sum(obj.discrete_time_problem.w.h(ii,:).res); + t_grid_u = [t_grid_u, t_grid(end)+h_sum]; + end + results.t_grid_u = t_grid_u; + else + results.t_grid_u = linspace(0, obj.p.T.val, opts.N_stages+1); + end + end + + fields = fieldnames(S); + empty_fields = cellfun(@(field) isempty(results.(field)), fields); + results = rmfield(S, fields(empty_fields)); + end + end +end diff --git a/+nosnoc/+discrete_time_problem/Heaviside.m b/+nosnoc/+discrete_time_problem/Heaviside.m index b828dec7..230a5498 100644 --- a/+nosnoc/+discrete_time_problem/Heaviside.m +++ b/+nosnoc/+discrete_time_problem/Heaviside.m @@ -145,7 +145,7 @@ function generate_direct_transcription_constraints(obj) for ii=1:opts.N_stages h0 = obj.p.T().val/(opts.N_stages*opts.N_finite_elements(ii)); - ui = obj.w.u(ii); + u_i = obj.w.u(ii); p_stage = obj.p.p_time_var(ii); p = [p_global;p_stage]; if obj.opts.use_speed_of_time_variables && opts.local_speed_of_time_variable @@ -185,31 +185,31 @@ function generate_direct_transcription_constraints(obj) lambda_n_ijk = obj.w.lambda_n(ii,jj,kk); lambda_p_ijk = obj.w.lambda_p(ii,jj,kk); - fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p); - qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p); + f_ijk = s_sot*dcs.f_x_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p); + q_ijk = s_sot*dcs.f_q_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p); xk = opts.C_rk(1, kk+1) * x_prev; for rr=1:opts.n_s x_ijr = obj.w.x(ii,jj,rr); xk = xk + opts.C_rk(rr+1, kk+1) * x_ijr; end - obj.g.dynamics(ii,jj,kk) = {h * fj - xk}; - obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; - obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p)}; + obj.g.dynamics(ii,jj,kk) = {h * f_ijk - xk}; + obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p)}; x_ij_end = x_ij_end + opts.D_rk(kk+1)*x_ijk; if opts.g_path_at_stg - obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end if size(model.G_path, 1) > 0 - G_path = dcs.G_path_fun(x_ijk, z_ijk, ui, v_global, p); - H_path = dcs.H_path_fun(x_ijk, z_ijk, ui, v_global, p); + G_path = dcs.G_path_fun(x_ijk, z_ijk, u_i, v_global, p); + H_path = dcs.H_path_fun(x_ijk, z_ijk, u_i, v_global, p); obj.G.path(ii,jj,kk) = {G_path}; obj.H.path(ii,jj,kk) = {H_path}; end if opts.cost_integration % also integrate the objective - obj.f = obj.f + opts.B_rk(kk+1)*h*qj; + obj.f = obj.f + opts.B_rk(kk+1)*h*q_ijk; end end if ~opts.right_boundary_point_explicit @@ -220,10 +220,10 @@ function generate_direct_transcription_constraints(obj) obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.lp_stationarity(ii,jj,opts.n_s+1) = {dcs.g_lp_stationarity_fun(x_ijk, z_ijk, lambda_p_ijk, lambda_p_ijk, v_global, p)}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; end if ~opts.g_path_at_stg && opts.g_path_at_fe - obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end case RKRepresentation.differential % In differential representation stage variables are the state derivatives. @@ -245,25 +245,25 @@ function generate_direct_transcription_constraints(obj) lambda_n_ijk = obj.w.lambda_n(ii,jj,kk); lambda_p_ijk = obj.w.lambda_p(ii,jj,kk); - fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p); - qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p); + f_ijk = s_sot*dcs.f_x_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p); + q_ijk = s_sot*dcs.f_q_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p); x_ij_end = x_ij_end + h*opts.b_rk(kk)*v_ijk; - obj.g.v(ii,jj,kk) = {fj - v_ijk}; - obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; - obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p)}; + obj.g.v(ii,jj,kk) = {f_ijk - v_ijk}; + obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p)}; if opts.g_path_at_stg - obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end if size(model.G_path, 1) > 0 - G_path = dcs.G_path_fun(x_ijk, z_ijk, ui, v_global, p); - H_path = dcs.H_path_fun(x_ijk, z_ijk, ui, v_global, p); + G_path = dcs.G_path_fun(x_ijk, z_ijk, u_i, v_global, p); + H_path = dcs.H_path_fun(x_ijk, z_ijk, u_i, v_global, p); obj.G.path(ii,jj,kk) = {G_path}; obj.H.path(ii,jj,kk) = {H_path}; end if opts.cost_integration % also integrate the objective - obj.f = obj.f + opts.b_rk(kk)*h*qj; + obj.f = obj.f + opts.b_rk(kk)*h*q_ijk; end end if ~opts.right_boundary_point_explicit @@ -275,12 +275,12 @@ function generate_direct_transcription_constraints(obj) obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.lp_stationarity(ii,jj,opts.n_s+1) = {dcs.g_lp_stationarity_fun(x_ijk, z_ijk, lambda_n_ijk, lambda_p_ijk, v_global, p)}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; else obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ij_end - obj.w.x(ii,jj,opts.n_s)}; end if ~opts.g_path_at_stg && opts.g_path_at_fe - obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end case RKRepresentation.differential_lift_x % In differential representation with lifted state stage variables are the state derivatives and we @@ -302,25 +302,25 @@ function generate_direct_transcription_constraints(obj) lambda_n_ijk = obj.w.lambda_n(ii,jj,kk); lambda_p_ijk = obj.w.lambda_p(ii,jj,kk); - fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p); - qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p); + f_ijk = s_sot*dcs.f_x_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p); + q_ijk = s_sot*dcs.f_q_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p); x_ij_end = x_ij_end + h*opts.b_rk(kk)*v_ijk; - obj.g.v(ii,jj,kk) = {fj - v_ijk}; - obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; - obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, ui, v_global, p)}; + obj.g.v(ii,jj,kk) = {f_ijk - v_ijk}; + obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_p_ijk, u_i, v_global, p)}; if opts.g_path_at_stg - obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end if size(model.G_path, 1) > 0 - G_path = dcs.G_path_fun(x_ijk, z_ijk, ui, v_global, p); - H_path = dcs.H_path_fun(x_ijk, z_ijk, ui, v_global, p); + G_path = dcs.G_path_fun(x_ijk, z_ijk, u_i, v_global, p); + H_path = dcs.H_path_fun(x_ijk, z_ijk, u_i, v_global, p); obj.G.path(ii,jj,kk) = {G_path}; obj.H.path(ii,jj,kk) = {H_path}; end if opts.cost_integration % also integrate the objective - obj.f = obj.f + opts.b_rk(kk)*h*qj; + obj.f = obj.f + opts.b_rk(kk)*h*q_ijk; end end if ~opts.right_boundary_point_explicit @@ -331,12 +331,12 @@ function generate_direct_transcription_constraints(obj) obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.lp_stationarity(ii,jj,opts.n_s+1) = {dcs.g_lp_stationarity_fun(x_ijk, z_ijk, lambda_n_ijk, lambda_p.ijk, v_global, p)}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; else obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ij_end - obj.w.x(ii,jj,opts.n_s)}; end if ~opts.g_path_at_stg && opts.g_path_at_fe - obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end end x_prev = obj.w.x(ii,jj,opts.n_s+rbp); @@ -344,7 +344,7 @@ function generate_direct_transcription_constraints(obj) if ~opts.g_path_at_stg && ~opts.g_path_at_fe x_i = obj.w.x(ii, opts.N_finite_elements(ii), opts.n_s); z_i = obj.w.z(ii, opts.N_finite_elements(ii), opts.n_s); - obj.g.path(ii) = {dcs.g_path_fun(x_i, z_i, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii) = {dcs.g_path_fun(x_i, z_i, u_i, v_global, p), model.lbg_path, model.ubg_path}; end % Least Squares Costs @@ -360,7 +360,7 @@ function generate_direct_transcription_constraints(obj) p); end if ~opts.cost_integration - obj.f = obj.f + dcs.f_q_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_n_ijk, ui, v_global, p); + obj.f = obj.f + dcs.f_q_fun(x_ijk, z_ijk, alpha_ijk, lambda_n_ijk, lambda_n_ijk, u_i, v_global, p); end % Clock 0 - G_path = dcs.G_path_fun(x_ijk, z_ijk, ui, v_global, p); - H_path = dcs.H_path_fun(x_ijk, z_ijk, ui, v_global, p); + G_path = dcs.G_path_fun(x_ijk, z_ijk, u_i, v_global, p); + H_path = dcs.H_path_fun(x_ijk, z_ijk, u_i, v_global, p); obj.G.path(ii,jj,kk) = {G_path}; obj.H.path(ii,jj,kk) = {H_path}; end if opts.cost_integration % also integrate the objective - obj.f = obj.f + opts.B_rk(kk+1)*h*qj; + obj.f = obj.f + opts.B_rk(kk+1)*h*q_ijk; end end if ~opts.right_boundary_point_explicit @@ -232,10 +232,10 @@ function generate_direct_transcription_constraints(obj) obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.lp_stationarity(ii,jj,opts.n_s+1) = {dcs.g_lp_stationarity_fun(x_ijk, z_ijk, lambda_ijk, mu_ijk, v_global, p)}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; end if ~opts.g_path_at_stg && opts.g_path_at_fe - obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end case RKRepresentation.differential % In differential representation stage variables are the state derivatives. @@ -257,25 +257,25 @@ function generate_direct_transcription_constraints(obj) theta_ijk = obj.w.theta(ii,jj,kk); mu_ijk = obj.w.mu(ii,jj,kk); - fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, ui, v_global, p); - qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, ui, v_global, p); + f_ijk = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, u_i, v_global, p); + q_ijk = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, u_i, v_global, p); x_ij_end = x_ij_end + h*opts.b_rk(kk)*v_ijk; - obj.g.v(ii,jj,kk) = {fj - v_ijk}; - obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; - obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, ui, v_global, p)}; + obj.g.v(ii,jj,kk) = {f_ijk - v_ijk}; + obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, u_i, v_global, p)}; if opts.g_path_at_stg - obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end if size(model.G_path, 1) > 0 - G_path = dcs.G_path_fun(x_ijk, z_ijk, ui, v_global, p); - H_path = dcs.H_path_fun(x_ijk, z_ijk, ui, v_global, p); + G_path = dcs.G_path_fun(x_ijk, z_ijk, u_i, v_global, p); + H_path = dcs.H_path_fun(x_ijk, z_ijk, u_i, v_global, p); obj.G.path(ii,jj,kk) = {G_path}; obj.H.path(ii,jj,kk) = {H_path}; end if opts.cost_integration % also integrate the objective - obj.f = obj.f + opts.b_rk(kk)*h*qj; + obj.f = obj.f + opts.b_rk(kk)*h*q_ijk; end end if ~opts.right_boundary_point_explicit @@ -286,12 +286,12 @@ function generate_direct_transcription_constraints(obj) obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.lp_stationarity(ii,jj,opts.n_s+1) = {dcs.g_lp_stationarity_fun(x_ijk, z_ijk, lambda_ijk, mu_ijk, v_global, p)}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; else obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ij_end - obj.w.x(ii,jj,opts.n_s)}; end if ~opts.g_path_at_stg && opts.g_path_at_fe - obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end case RKRepresentation.differential_lift_x % In differential representation with lifted state stage variables are the state derivatives and we @@ -313,25 +313,25 @@ function generate_direct_transcription_constraints(obj) theta_ijk = obj.w.theta(ii,jj,kk); mu_ijk = obj.w.mu(ii,jj,kk); - fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, ui, v_global, p); - qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, ui, v_global, p); + f_ijk = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, u_i, v_global, p); + q_ijk = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, u_i, v_global, p); x_ij_end = x_ij_end + h*opts.b_rk(kk)*v_ijk; - obj.g.v(ii,jj,kk) = {fj - v_ijk}; - obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; - obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, ui, v_global, p)}; + obj.g.v(ii,jj,kk) = {f_ijk - v_ijk}; + obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, u_i, v_global, p)}; if opts.g_path_at_stg - obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end if size(model.G_path, 1) > 0 - G_path = dcs.G_path_fun(x_ijk, z_ijk, ui, v_global, p); - H_path = dcs.H_path_fun(x_ijk, z_ijk, ui, v_global, p); + G_path = dcs.G_path_fun(x_ijk, z_ijk, u_i, v_global, p); + H_path = dcs.H_path_fun(x_ijk, z_ijk, u_i, v_global, p); obj.G.path(ii,jj,kk) = {G_path}; obj.H.path(ii,jj,kk) = {H_path}; end if opts.cost_integration % also integrate the objective - obj.f = obj.f + opts.b_rk(kk)*h*qj; + obj.f = obj.f + opts.b_rk(kk)*h*q_ijk; end end if ~opts.right_boundary_point_explicit @@ -342,13 +342,13 @@ function generate_direct_transcription_constraints(obj) obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.lp_stationarity(ii,jj,opts.n_s+1) = {dcs.g_lp_stationarity_fun(x_ijk, z_ijk, lambda_ijk, mu_ijk, v_global, p)}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; else obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ij_end - obj.w.x(ii,jj,opts.n_s)}; end if ~opts.g_path_at_stg && opts.g_path_at_fe % if path constraints are evaluated at control and FE grid points - obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, u_i, v_global, p), model.lbg_path, model.ubg_path}; end end x_prev = obj.w.x(ii,jj,opts.n_s+rbp); @@ -357,7 +357,7 @@ function generate_direct_transcription_constraints(obj) % if path constraints are only evaluated at the control grid nodes x_i = obj.w.x(ii, opts.N_finite_elements(ii), opts.n_s); z_i = obj.w.z(ii, opts.N_finite_elements(ii), opts.n_s); - obj.g.path(ii) = {dcs.g_path_fun(x_i, z_i, ui, v_global, p), model.lbg_path, model.ubg_path}; + obj.g.path(ii) = {dcs.g_path_fun(x_i, z_i, u_i, v_global, p), model.lbg_path, model.ubg_path}; end % Least Squares Costs @@ -373,7 +373,7 @@ function generate_direct_transcription_constraints(obj) p); end if ~opts.cost_integration - obj.f = obj.f + dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, ui, v_global, p); + obj.f = obj.f + dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, u_i, v_global, p); end % Clock ` the project. .. note:: @@ -14,4 +14,6 @@ Contents .. toctree:: usage - api + options + models + dcs diff --git a/docs/source/models.rst b/docs/source/models.rst new file mode 100644 index 00000000..d0f8cff0 --- /dev/null +++ b/docs/source/models.rst @@ -0,0 +1,7 @@ +Models +====== +All ``nosnoc`` models are derived from :mat:class:`nosnoc.model.Base` and share its functionality. + +.. module:: +nosnoc.+model +.. autoclass:: Base +.. autoclass:: Pds diff --git a/docs/source/options.rst b/docs/source/options.rst new file mode 100644 index 00000000..384c3796 --- /dev/null +++ b/docs/source/options.rst @@ -0,0 +1,4 @@ +Options +======= +.. module:: +nosnoc +.. autoclass:: Options diff --git a/docs/source/usage.rst b/docs/source/usage.rst index 88f1434f..3c8175ae 100644 --- a/docs/source/usage.rst +++ b/docs/source/usage.rst @@ -6,4 +6,25 @@ Usage Installation ------------ +**nosnoc** requires ``CasADi`` version 3.5.5 and Matlab version R2021b or later. +Installation for MATLAB +^^^^^^^^^^^^^^^^^^^^^^^ + +1. Install ``CasADi`` and make sure it is added to your ``MATLAB`` path. + For ``CasADi`` installation instructions follow visit: https://web.casadi.org/get/ + +2. Clone this repository:: + + git clone --recursive https://github.com/nurkanovic/nosnoc.git + +3. Open the `nosnoc` folder in Matlab and run the `install_nosnoc` script:: + + >> install_nosnoc + +Note that ``IPOPT`` is shipped with ``CasADi``. More information including detailed documentation can be found on its `homepage `_. + +Installation for python +^^^^^^^^^^^^^^^^^^^^^^^ + +Go to the `nosnoc_py `_ repository for more info. diff --git a/examples/basic_pds/basic_pds.m b/examples/basic_pds/basic_pds.m new file mode 100644 index 00000000..fd8ff1f7 --- /dev/null +++ b/examples/basic_pds/basic_pds.m @@ -0,0 +1,66 @@ +clear all +clc +close all +import casadi.* +%% discretization parameters +N_sim = 31; +T_sim = 10; + +%% NOSNOC settings +problem_options = nosnoc.Options(); +solver_options = nosnoc.solver.Options(); +problem_options.n_s = 3; +problem_options.rk_scheme = RKSchemes.RADAU_IIA; +%problem_options.rk_representation= RKRepresentation.differential_lift_x; +problem_options.rk_representation = RKRepresentation.integral; +problem_options.cross_comp_mode = CrossCompMode.STAGE_STAGE; +%problem_options.cross_comp_mode = CrossCompMode.FE_FE; +problem_options.N_sim = N_sim; +problem_options.N_finite_elements = 2; +problem_options.T_sim = T_sim; +problem_options.rho_h = 1e-4; +problem_options.gcs_lift_gap_functions = true; +problem_options.step_equilibration = StepEquilibrationMode.l2_relaxed_scaled; +%solver_options.homotopy_steering_strategy = 'ELL_INF'; +solver_options.complementarity_tol = 1e-10; +solver_options.print_level = 3; + +model = nosnoc.model.Pds(); + +model.x0 = [sqrt(2)/2;sqrt(2)/2]; +x = SX.sym('x',2); +model.x = x; +model.c = x(2)+0.2; +model.f_x_unconstrained = [x(2);-x(1)]; + +model.x0 = [0;pi-2]; + +integrator = nosnoc.Integrator(model, problem_options, solver_options); +[t_grid, x_res] = integrator.simulate(); +% +figure +plot(x_res(1,:), x_res(2,:)) +grid on +xlabel('$x_1$','Interpreter','latex') +ylabel('$x_2$','Interpreter','latex') +grid on + +c_fun = casadi.Function('c', {model.x}, {model.c}); +c = full(c_fun(integrator.get_full('x')))'; +x_full = integrator.get_full('x'); +lambda = integrator.get('lambda'); + +% extened values over all rk stage points +t_grid_full = integrator.get_time_grid_full(); +lambda_full = integrator.get_full('lambda'); + +% + +figure +plot(t_grid,lambda,'LineWidth',2) +hold on +plot(t_grid,x_res,'LineWidth',1.5) +grid on +xlabel('$t$','Interpreter','latex') +legend({'$\lambda(t)$','$x_1(t)$','$x_2(t)$'},'interpreter','latex'); + diff --git a/examples/marine_surface_vehicle/msv.m b/examples/marine_surface_vehicle/msv.m new file mode 100644 index 00000000..cdd26b39 --- /dev/null +++ b/examples/marine_surface_vehicle/msv.m @@ -0,0 +1,99 @@ +clear all +close all +import casadi.* +import vdx.* + +R = 3.5; +%% Define projected system +problem_options = nosnoc.Options(); +solver_options = nosnoc.solver.Options(); +% problem_options.rk_scheme = RKSchemes.RADAU_I; +problem_options.rk_scheme = RKSchemes.RADAU_IIA; +% problem_options.rk_representation= RKRepresentation.differential_lift_x; +problem_options.rk_representation = RKRepresentation.integral; +problem_options.cross_comp_mode = CrossCompMode.STAGE_STAGE; +problem_options.cross_comp_mode = CrossCompMode.FE_FE; +problem_options.N_finite_elements = 2; +problem_options.n_s = 2; +problem_options.N_stages = 25; +problem_options.T = 1; +problem_options.rho_h = 1e-4; +problem_options.time_optimal_problem = true; +problem_options.use_fesd = true; +problem_options.gcs_lift_gap_functions = true; +%solver_options.homotopy_steering_strategy = 'ELL_INF'; +solver_options.complementarity_tol = 1e-10; +solver_options.print_level = 3; + +model = nosnoc.model.Pds(); +x1 = SX.sym('x1', 2); +x2 = SX.sym('x2', 2); +x = [x1;x2]; +x_target = [0;0;0;0]; +model.x = [x]; +model.lbx = [-inf;-inf;-inf;-inf]; +model.ubx = [inf;inf;inf;inf]; +x0 =[-25;-25;-15;-15]; +model.x0 = [x0]; +u1 = SX.sym('u1', 2); +u2 = SX.sym('u2', 2); +model.u = [u1;u2]; +model.lbu = [-100/sqrt(2);-100/sqrt(2);-60/sqrt(2);-60/sqrt(2)]; +model.ubu = [100/sqrt(2);100/sqrt(2);60/sqrt(2);60/sqrt(2)]; +model.u0 = model.ubu; +model.c = [norm_2(x2-x1)-2*R]; +model.f_x_unconstrained = [u1;u2]; + +% costs +model.f_q = 0; +model.f_q_T = 0.5*(x-x_target)'*(x-x_target); + +% Solve +ocp_solver = nosnoc.ocp.Solver(model, problem_options, solver_options); +ocp_solver.solve(); + +%% plot +fontsize = 12; +x_res = ocp_solver.get('x'); +u_res = ocp_solver.get('u'); +u_rep = kron(u_res, ones(1,problem_options.N_finite_elements(1))); +T_final = ocp_solver.get('T_final'); +lambda_res = ocp_solver.get('lambda'); +t_res = ocp_solver.get_time_grid(); +if ~problem_options.use_fesd + t_res = t_res*T_final; +end +c_fun = casadi.Function('nabla_c', {x}, {model.c}); +nabla_c_fun = casadi.Function('nabla_c', {x}, {model.c.jacobian(x)'}); +c_res = full(c_fun(x_res(1:4,:))); +nabla_c_res = full(nabla_c_fun(x_res(1:4,:))); +v_res = u_rep + repmat(lambda_res(2:end),4,1).*nabla_c_res(:, 2:end); + +figure +ax = subplot(3,1,1); +plot(t_res(1:end-1), v_res([1,3],:), "LineWidth", 2) +xlabel("$t$",'Interpreter','latex') +ylabel("$v_x$",'Interpreter','latex') +ylim([40, 75]) +xlim([0, T_final]) +grid on +ax.FontSize = fontsize; +ax.FontSize = fontsize; +ax = subplot(3,1,2); +plot(t_res(1:end-1), v_res([2,4],:), "LineWidth", 2) +xlabel("$t$",'Interpreter','latex') +ylabel("$v_y$",'Interpreter','latex') +ax.FontSize = fontsize; +ax.FontSize = fontsize; +ylim([40, 75]) +xlim([0, T_final]) +grid on +ax = subplot(3,1,3); +plot(t_res, c_res, "LineWidth", 2) +xlabel("$t$",'Interpreter','latex') +ylabel("$c(x)$",'Interpreter','latex') +ax.FontSize = fontsize; +ax.FontSize = fontsize; +ylim([-0.1, 10]) +xlim([0, T_final]) +grid on diff --git a/examples/new_design_examples/new_design_car.m b/examples/new_design_examples/new_design_car.m index 61258399..dc701351 100644 --- a/examples/new_design_examples/new_design_car.m +++ b/examples/new_design_examples/new_design_car.m @@ -8,8 +8,8 @@ solver_options = nosnoc.solver.Options(); %% set some options -problem_options.rk_scheme = RKSchemes.RADAU_IIA; -problem_options.rk_representation = RKRepresentation.integral; +problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; +problem_options.rk_representation = RKRepresentation.differential_lift_x; problem_options.n_s = 2; problem_options.N_stages = 50; % number of control intervals problem_options.N_finite_elements = 2; % number of finite element on every control interval (optionally a vector might be passed) diff --git a/examples/sweeping_set/plot_moving_set.m b/examples/sweeping_set/plot_moving_set.m new file mode 100644 index 00000000..c46ba951 --- /dev/null +++ b/examples/sweeping_set/plot_moving_set.m @@ -0,0 +1,106 @@ +function plot_moving_set(h,pos,r,type,fig,vidname,export_frames) + n = length(r); + if ~exist('type') + type = []; + for ii=1:n + type = [type, "circle"]; + end + end + if ~exist('export_frames') + export_frames = false; + end + if length(type) ~= n + error('not all types specified') + end + indices = {}; + curr_idx = 1; + for ii=1:n + switch type(ii) + case "circle" + indices{ii} = curr_idx:curr_idx+1; + curr_idx = curr_idx+2; + case "square" + indices{ii} = curr_idx:curr_idx+2; + curr_idx = curr_idx+3; + end + end + if ~exist('fig') + fig = figure; + else + figure(fig) + end + axis equal + box on; + ax = gca; + set(ax,'XTick',[-10,0,10], 'YTick', [0,5,10], 'TickLength', [0.025,0.025], 'LineWidth', 4.0); + xlim([-3,3]) + ylim([-3,3]) + ax.XAxis.FontSize = 36; + ax.YAxis.FontSize = 36; + xlabel("$c_x$", "fontsize", 32,'Interpreter','latex') + ylabel("$c_y$", "fontsize", 32,'Interpreter','latex') + + %axis off + discs = {}; + tau = linspace(0, 2*pi)'; + rot = @(theta) [cos(theta) -sin(theta);... + sin(theta) cos(theta)]; + circ = @(p,r) [r*cos(tau)+p(1),r*sin(tau)+p(2)]; + sqr = @(p,r) transpose(rot(-p(3))*([r,r;r,-r;-r,-r;-r,r]') + p(1:2)); + for ii=1:n + p = pos(indices{ii},1); + switch type(ii) + case "circle" + v = circ(p,r(ii)); + case "square" + v = sqr(p,r(ii)); + end + if ii==3 + color = [0.8500 0.3250 0.0980]; + else + color = [0 0.4470 0.7410]; + end + discs{ii} = patch(v(:,1),v(:,2), color); + end + t = pos(end,1); + v_C = circ([sin(t);cos(t)],1); + C = patch(v_C(:,1),v_C(:,2), [1 0 0], 'FaceAlpha', 0.0, 'LineStyle', '--', 'EdgeColor' , [1 0 0]); + if exist('vidname') + mkdir([vidname '_frames']) + writer = VideoWriter([vidname '.avi']); + open(writer); + frame = getframe(gca); + if export_frames + exportgraphics(gca, [vidname '_frames/' num2str(1) '.pdf']) + end + writeVideo(writer,frame) + end + pause(0.1); + for jj=2:(size(pos,2)-1) + for ii=1:n + p = pos(indices{ii},jj+1); + switch type(ii) + case "circle" + v = circ(p,r(ii)); + case "square" + v = sqr(p,r(ii)); + end + discs{ii}.Vertices = v; + end + t = pos(end,jj+1); + v_C = circ([sin(t);cos(t)],1); + C.Vertices = v_C; + drawnow limitrate; + %pause(h(jj)); + pause(0.1) + if exist('vidname') + frame = getframe(gca); + ff=jj+1; + %exportgraphics(gca, [vidname '_frames/' num2str(jj) '.pdf']) + writeVideo(writer,frame); + end + end + if exist('vidname') + close(writer); + end +end diff --git a/examples/sweeping_set/sweeping_set.m b/examples/sweeping_set/sweeping_set.m new file mode 100644 index 00000000..ffdbe3c7 --- /dev/null +++ b/examples/sweeping_set/sweeping_set.m @@ -0,0 +1,44 @@ +clear all +clc +close all +import casadi.* +%% discretization parameters +N_sim = 100; +T_sim = 10; + +%% NOSNOC settings +problem_options = nosnoc.Options(); +solver_options = nosnoc.solver.Options(); +problem_options.n_s = 2; +problem_options.rk_scheme = RKSchemes.RADAU_IIA; +problem_options.rk_representation= 'differential_lift_x'; +% problem_options.rk_representation= 'integral'; +problem_options.N_sim = N_sim; +problem_options.N_finite_elements = 2; +problem_options.T_sim = T_sim; + +%solver_options.homotopy_steering_strategy = 'ELL_INF'; +solver_options.print_level = 3; + +model = nosnoc.model.Pds(); + +model.x0 = [0;pi-2;0]; +x = SX.sym('x',2); +t = SX.sym('t'); +model.x = [x;t]; +model.c = [-norm(x - [sin(t);cos(t)])^2+(1-0.5)^2]; +model.f_x_unconstrained = [0; 0; 1]; +model.E = diag([1,1,0]); + +integrator = nosnoc.Integrator(model, problem_options, solver_options); +[t_grid, x_res] = integrator.simulate('natural_residual_ineq'); +% +figure +plot(x_res(1,:), x_res(2,:)) +grid on +xlabel('$x_1$','Interpreter','latex') +ylabel('$x_2$','Interpreter','latex') +grid on + +fig = figure('Position', [10 10 1600 800]); +plot_moving_set([],x_res,[0.5], ["circle"], fig, 'moving_set'); diff --git a/external/vdx b/external/vdx index bdacbdbd..693f8150 160000 --- a/external/vdx +++ b/external/vdx @@ -1 +1 @@ -Subproject commit bdacbdbd208866a1731c424ef8b9fc5f78236daf +Subproject commit 693f8150890771ba7d5963c0ab02f7eb7a5cd3b6