From 07585dcbefe06f2e077e005f101e5b7d4ad65b03 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Fri, 17 May 2024 11:57:13 +0200 Subject: [PATCH 01/43] initial work --- +nosnoc/+dcs/Gcs.m | 55 ++ +nosnoc/+discrete_time_problem/Gcs.m | 783 +++++++++++++++++++++++++++ +nosnoc/+model/Pds.m | 43 ++ 3 files changed, 881 insertions(+) create mode 100644 +nosnoc/+dcs/Gcs.m create mode 100644 +nosnoc/+discrete_time_problem/Gcs.m create mode 100644 +nosnoc/+model/Pds.m diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m new file mode 100644 index 00000000..acdf556c --- /dev/null +++ b/+nosnoc/+dcs/Gcs.m @@ -0,0 +1,55 @@ +classdef Gcs < nosnoc.dcs.Base + properties + model + + lambda + + f_x + + dims + + nabla_c_fun + c_fun + 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; + obj.lambda = define_casadi_symbolic(opts.casadi_symbolic_mode,'lambda',obj.dims.n_lambda); + + obj.dims = dims; + end + + function generate_equations(obj, opts) + import casadi.* + model = obj.model; + dims = obj.dims; + + nabla_c = model.c.jacobian(model.x)'; + + obj.f_x = model.f_x + obj.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, model.f_q}); + 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, model.z, model.v_global, model.p}, {model.c}) + 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_comp_path_fun = Function('g_comp_path', {model.x, model.z, model.u, model.v_global, model.p}, {model.g_comp_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 +end diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m new file mode 100644 index 00000000..26c99a20 --- /dev/null +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -0,0 +1,783 @@ +classdef Gcs < vdx.problems.Mpcc + properties + model + dcs + opts + + populated + 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.mlcp + obj.w.B_max(ii,2:opts.N_finite_elements(ii)) = {{'B_max', dims.n_lambda},-inf,inf}; + obj.w.pi_theta(ii,2:opts.N_finite_elements(ii)) = {{'pi_theta', dims.n_theta},-inf,inf}; + obj.w.pi_lambda(ii,2:opts.N_finite_elements(ii)) = {{'pi_lambda', dims.n_lambda},-inf,inf}; + obj.w.lambda_theta(ii,2:opts.N_finite_elements(ii)) = {{'lambda_theta', dims.n_theta},0,inf}; + obj.w.lambda_lambda(ii,2:opts.N_finite_elements(ii)) = {{'lambda_lambda', dims.n_lambda},0,inf}; + obj.w.eta(ii,2:opts.N_finite_elements(ii)) = {{'eta', dims.n_lambda},0,inf}; + 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}; + obj.w.z(0,0,opts.n_s) = {{'z', dims.n_z}, model.lbz, model.ubz, model.z0}; + obj.w.lambda(0,0,opts.n_s) = {{['lambda'], dims.n_lambda},0,inf}; + for ii=1:opts.N_stages + if (opts.rk_representation == RKRepresentation.integral ||... + opts.rk_representation == RKRepresentation.differential_lift_x) + 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}; + else + obj.w.x(ii,1:opts.N_finite_elements(ii),opts.n_s) = {{'x', dims.n_x}, model.lbx, model.ubx, model.x0}; + end + if (opts.rk_representation == RKRepresentation.differential ||... + 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}}; + end + + obj.w.z(ii,1:opts.N_finite_elements(ii),(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}; + + % Handle x_box settings + if ~opts.x_box_at_stg && opts.rk_representation ~= RKRepresentation.differential + 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(); + + x_0 = obj.w.x(0,0,opts.n_s); + z_0 = obj.w.z(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, z_0, v_global, [p_global;obj.p.p_time_var(1)]), 0, inf}; + + x_prev = obj.w.x(0,0,opts.n_s); + for ii=1:opts.N_stages + if obj.opts.use_fesd + t_stage = obj.p.T()/opts.N_stages; + h0 = obj.p.T().val/(opts.N_stages*opts.N_finite_elements(ii)); + else + h0 = obj.p.T().val/(opts.N_stages*opts.N_finite_elements(ii)); + end + + ui = 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 + + 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 = 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); + + fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); + qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, ui, 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.c_lb(ii,jj,kk) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; + + 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}; + end + if size(model.g_comp_path, 1) > 0 + g_comp_path = dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p); + obj.G.path(ii,jj,kk) = {g_comp_path(:,1)}; + obj.H.path(ii,jj,kk) = {g_comp_path(:,2)}; + end + if opts.cost_integration + % also integrate the objective + obj.f = obj.f + opts.B_rk(kk+1)*h*qj; + end + end + if ~opts.right_boundary_point_explicit + x_ijk = obj.w.x(ii,jj,opts.n_s+1); + z_ijk = obj.w.z(ii,jj,opts.n_s+1); + lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); + + obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; + obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, 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}; + end + case RKRepresentation.differential + % In differential representation stage variables are the state derivatives. + X_ijk = {}; + for kk = 1:opts.n_s + 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 + X_ijk = [X_ijk {x_temp}]; + end + X_ijk = [X_ijk, {obj.w.x(ii,jj,opts.n_s)}]; + x_ij_end = x_prev; + for kk=1:opts.n_s + x_ijk = X_ijk{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); + + fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); + qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, ui, 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.c_lb(ii,jj,kk) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; + + 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}; + end + if size(model.g_comp_path, 1) > 0 + g_comp_path = dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p); + obj.G.path(ii,jj,kk) = {g_comp_path(:,1)}; + obj.H.path(ii,jj,kk) = {g_comp_path(:,2)}; + end + if opts.cost_integration + % also integrate the objective + obj.f = obj.f + opts.b_rk(kk)*h*qj; + end + end + if ~opts.right_boundary_point_explicit + x_ijk = obj.w.x(ii,jj,opts.n_s+1); + z_ijk = obj.w.z(ii,jj,opts.n_s+1); + lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); + + obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; + obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, 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}; + end + 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); + + fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); + qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, ui, 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.c_lb(ii,jj,kk) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; + + 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}; + end + if size(model.g_comp_path, 1) > 0 + g_comp_path = dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p); + obj.G.path(ii,jj,kk) = {g_comp_path(:,1)}; + obj.H.path(ii,jj,kk) = {g_comp_path(:,2)}; + end + if opts.cost_integration + % also integrate the objective + obj.f = obj.f + opts.b_rk(kk)*h*qj; + end + end + if ~opts.right_boundary_point_explicit + x_ijk = obj.w.x(ii,jj,opts.n_s+1); + z_ijk = obj.w.z(ii,jj,opts.n_s+1); + lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); + mu_ijk = obj.w.mu(ii,jj,opts.n_s+1); + + obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; + obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; + obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, 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}; + 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, ui, v_global, p), model.lbg_path, model.ubg_path}; + end + + % Least Squares Costs + % TODO we should convert the refs to params + 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); + obj.f = obj.f + t_stage*dcs.f_lsq_u_fun(obj.w.u(ii),... + model.u_ref_val(:,ii),... + p); + if ~opts.cost_integration + obj.f = obj.f + dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); + end + + % Clock =sigmaB, B>=sigmaF + kkt_max = [1-lambda_theta-lambda_lambda; + B_max-pi_lambda; + B_max-pi_theta]; + 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_theta)]}; + obj.H.step_eq_kkt_max(ii,jj) = {[lambda_lambda;lambda_theta]}; + + % eta calculation + eta_const = [eta-pi_theta;eta-pi_lambda;eta-pi_theta-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=obj.p.T()/opts.N_stages; + 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) + if ~obj.populated + obj.populate_problem(); + end + + if ~exist('plugin') + plugin = 'scholtes_ineq'; + end + + % Sort by indices to recover almost block-band structure. + obj.w.sort_by_index(); + obj.g.sort_by_index(); + + solver_options.assume_lower_bounds = true; + + obj.solver = nosnoc.solver.mpccsol('Mpcc solver', plugin, obj.to_casadi_struct(), solver_options); + end + + function stats = solve(obj) + opts = obj.opts; + T_val = obj.p.T().val; + + 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 + + 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; + results.mu = obj.discrete_time_problem.w.mu(:,:,obj.opts.n_s).res; + results.theta = obj.discrete_time_problem.w.theta(:,:,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]; + results.mu = [obj.discrete_time_problem.w.mu(0,0,obj.opts.n_s).res,... + obj.discrete_time_problem.w.mu(1:opts.N_stages,:,obj.opts.n_s+1).res]; + results.theta = obj.discrete_time_problem.w.theta(:,:,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/+model/Pds.m b/+nosnoc/+model/Pds.m new file mode 100644 index 00000000..4ba926c3 --- /dev/null +++ b/+nosnoc/+model/Pds.m @@ -0,0 +1,43 @@ +classdef Pds < nosnoc.model.Base + properties + f_x + c + E + end + + methods + function obj = Pds() + obj = obj@nosnoc.model.Base(); + end + + function verify_and_backfill(obj, opts) + arguments + obj + opts nosnoc.Options + end + import casadi.* + verify_and_backfill@nosnoc.model.Base(obj,opts); + + dims = obj.dims; + + if size(obj.f_x,1) ~= dims.n_x + error("nosnoc: f_x has incorrect dimension. It must have the same dimension as x.") + end + + dims.n_c = size(obj.c,1); + + if ~isempty(E) + if size(E, 1) ~= size(E,2) + error("nosnoc: Projection matrix E must be square."); + end + if size(E, 1) ~= dims.n_c + error("nosnoc: Projection matrix E must be an n_c by n_c matrix where n_c is the number of functions defining the set C.") + end + else + E = eye(dims.n_c); + end + + obj.dims = dims; + end + end +end From c6704ac8a5007cb16f435fd05ca52f1eed903942 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Fri, 17 May 2024 12:20:22 +0200 Subject: [PATCH 02/43] initial cross comp work --- +nosnoc/+discrete_time_problem/Gcs.m | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 26c99a20..ba11b03f 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -363,12 +363,12 @@ function generate_direct_transcription_constraints(obj) 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); + obj.f = obj.f + dcs.f_q_T_fun(x_end, z_end, v_global, p_global); % Terminal_lsq_cost obj.f = obj.f + h0*opts.N_finite_elements(ii)*dcs.f_lsq_T_fun(x_end,... model.x_ref_end_val,... - p); + p_global); % Terminal constraint if opts.relax_terminal_constraint_homotopy @@ -410,12 +410,19 @@ function generate_complementarity_constraints(obj) % 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); + if opts.use_fesd switch opts.cross_comp_mode case CrossCompMode.STAGE_STAGE - lambda_prev = obj.w.lambda(0,0,opts.n_s); for ii=1:opts.N_stages + p_stage = obj.p.p_time_var(ii); for rr=1:opts.n_s theta_ijr = obj.w.theta(ii,jj,rr); @@ -440,7 +447,6 @@ function generate_complementarity_constraints(obj) end end case CrossCompMode.FE_STAGE - lambda_prev = obj.w.lambda(0,0,opts.n_s); for ii=1:opts.N_stages for jj=1:opts.N_finite_elements(ii); sum_lambda = lambda_prev + sum2(obj.w.lambda(ii,jj,:)); @@ -458,7 +464,6 @@ function generate_complementarity_constraints(obj) end end case CrossCompMode.STAGE_FE - lambda_prev = obj.w.lambda(0,0,opts.n_s); for ii=1:opts.N_stages for jj=1:opts.N_finite_elements(ii); sum_theta = sum2(obj.w.theta(ii,jj,:)); @@ -476,7 +481,6 @@ function generate_complementarity_constraints(obj) end end case CrossCompMode.FE_FE - lambda_prev = obj.w.lambda(0,0,opts.n_s); for ii=1:opts.N_stages for jj=1:opts.N_finite_elements(ii); sum_lambda = lambda_prev + sum2(obj.w.lambda(ii,jj,:)); From a6c7a68d0a6b2c0c67b63b494ce7e3daeef201b5 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Fri, 17 May 2024 12:56:59 +0200 Subject: [PATCH 03/43] initial work on cross complementatrity is done --- +nosnoc/+discrete_time_problem/Gcs.m | 93 +++++++++++++++++++++------- 1 file changed, 72 insertions(+), 21 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index ba11b03f..a30fba0a 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -208,6 +208,7 @@ function generate_direct_transcription_constraints(obj) obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; end case RKRepresentation.differential + error("Differential representation is currently unsupported") % In differential representation stage variables are the state derivatives. X_ijk = {}; for kk = 1:opts.n_s @@ -408,6 +409,7 @@ function generate_complementarity_constraints(obj) dcs = obj.dcs; model = obj.model; % Do Cross-Complementarity + % TODO(@anton) correctly handle differential rk representation. rbp = ~opts.right_boundary_point_explicit; @@ -417,91 +419,140 @@ function generate_complementarity_constraints(obj) 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_prev = dcs.c_fun(x_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); - for rr=1:opts.n_s - theta_ijr = obj.w.theta(ii,jj,rr); - - Gij = vertcat(Gij, lambda_prev); - Hij = vertcat(Hij, theta_ijr); - end + p = [p_global;p_stage]; for jj=1:opts.N_finite_elements(ii); Gij = {}; Hij = {}; + for rr=1:opts.n_s + x_ijr = obj.w.x(ii,jj,rr); + z_ijr = obj.w.z(ii,jj,rr); + c_ijr = dcs.c_fun(x_ijr,z_ijr, v_global, p); + + Gij = vertcat(Gij, {lambda_prev}); + Hij = vertcat(Hij, {c_ijr}); + end + for rr=1:opts.n_s + lambda_ijr = obj.w.lambda(ii,jj,rr); + + Gij = vertcat(Gij, {lambda_prev}); + 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 - theta_ijr = obj.w.theta(ii,jj,rr); - - Gij = vertcat(Gij, lambda_ijk); - Hij = vertcat(Hij, theta_ijr); + x_ijr = obj.w.x(ii,jj,rr); + z_ijr = obj.w.z(ii,jj,rr); + c_ijr = dcs.c_fun(x_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_prev = dcs.c_fun(x_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 = {}; - Hij = {}; + Gij = {sum_lambda}; + Hij = {c_prev}; for kk=1:opts.n_s - theta_ijk = obj.w.theta(ii,jj,kk); + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk,z_ijk, v_global, p); Gij = vertcat(Gij, {sum_lambda}); - Hij = vertcat(Hij, {theta_ijk}); + 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_prev = dcs.c_fun(x_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_theta = sum2(obj.w.theta(ii,jj,:)); + sum_c = c_fun(x_prev); + for kk=1:opts.n_s + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk,z_ijk,v_global,p); + sum_c = sum_c + c_ijk; + end Gij = {lambda_prev}; - Hij = {sum_theta}; + 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_theta}); + 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_prev = dcs.c_fun(x_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_theta = sum2(obj.w.theta(ii,jj,:)); + sum_c = c_fun(x_prev); + for kk=1:opts.n_s + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_ijk = dcs.c_fun(x_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_theta}; + 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_prev = dcs.c_fun(x_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 for jj=1:opts.N_finite_elements(ii); Gij = {}; Hij = {}; for kk=1:opts.n_s lambda_ijk = obj.w.lambda(ii,jj,kk); - theta_ijk = obj.w.theta(ii,jj,kk); + x_ijk = obj.w.x(ii,jj,kk); + z_ijk = obj.w.z(ii,jj,kk); + c_ijk = dcs.c_fun(x_ijk,z_ijk,v_global,p); obj.G.standard_comp(ii,jj, kk) = {lambda_ijk}; - obj.H.standard_comp(ii,jj, kk) = {theta_ijk}; + obj.H.standard_comp(ii,jj, kk) = {c_ijk}; end end end From e8c9c73024f039effc33b46cd9f9fe6f41828f08 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Fri, 17 May 2024 13:41:45 +0200 Subject: [PATCH 04/43] step-eq work --- +nosnoc/+discrete_time_problem/Gcs.m | 175 ++++++++++++++++++--------- 1 file changed, 117 insertions(+), 58 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index a30fba0a..c804fcbc 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -66,9 +66,9 @@ function create_variables(obj) end if obj.opts.step_equilibration == StepEquilibrationMode.mlcp obj.w.B_max(ii,2:opts.N_finite_elements(ii)) = {{'B_max', dims.n_lambda},-inf,inf}; - obj.w.pi_theta(ii,2:opts.N_finite_elements(ii)) = {{'pi_theta', dims.n_theta},-inf,inf}; + obj.w.pi_c(ii,2:opts.N_finite_elements(ii)) = {{'pi_c', dims.n_c},-inf,inf}; obj.w.pi_lambda(ii,2:opts.N_finite_elements(ii)) = {{'pi_lambda', dims.n_lambda},-inf,inf}; - obj.w.lambda_theta(ii,2:opts.N_finite_elements(ii)) = {{'lambda_theta', dims.n_theta},0,inf}; + obj.w.lambda_c(ii,2:opts.N_finite_elements(ii)) = {{'lambda_c', dims.n_c},0,inf}; obj.w.lambda_lambda(ii,2:opts.N_finite_elements(ii)) = {{'lambda_lambda', dims.n_lambda},0,inf}; obj.w.eta(ii,2:opts.N_finite_elements(ii)) = {{'eta', dims.n_lambda},0,inf}; obj.w.nu(ii,2:opts.N_finite_elements(ii)) = {{'nu', 1},0,inf}; @@ -430,7 +430,7 @@ function generate_complementarity_constraints(obj) for jj=1:opts.N_finite_elements(ii); Gij = {}; Hij = {}; - for rr=1:opts.n_s + for rr=1:(opts.n_s + rbp) x_ijr = obj.w.x(ii,jj,rr); z_ijr = obj.w.z(ii,jj,rr); c_ijr = dcs.c_fun(x_ijr,z_ijr, v_global, p); @@ -438,7 +438,7 @@ function generate_complementarity_constraints(obj) Gij = vertcat(Gij, {lambda_prev}); Hij = vertcat(Hij, {c_ijr}); end - for rr=1:opts.n_s + for rr=1:(opts.n_s + rbp) lambda_ijr = obj.w.lambda(ii,jj,rr); Gij = vertcat(Gij, {lambda_prev}); @@ -446,7 +446,7 @@ function generate_complementarity_constraints(obj) end for kk=1:(opts.n_s + rbp) lambda_ijk = obj.w.lambda(ii,jj,kk); - for rr=1:opts.n_s + for rr=1:(opts.n_s + rbp) x_ijr = obj.w.x(ii,jj,rr); z_ijr = obj.w.z(ii,jj,rr); c_ijr = dcs.c_fun(x_ijr,z_ijr, v_global, p); @@ -471,7 +471,7 @@ function generate_complementarity_constraints(obj) sum_lambda = lambda_prev + sum2(obj.w.lambda(ii,jj,:)); Gij = {sum_lambda}; Hij = {c_prev}; - for kk=1:opts.n_s + for kk=1:(opts.n_s + rbp) x_ijk = obj.w.x(ii,jj,kk); z_ijk = obj.w.z(ii,jj,kk); c_ijk = dcs.c_fun(x_ijk,z_ijk, v_global, p); @@ -493,7 +493,7 @@ function generate_complementarity_constraints(obj) p = [p_global;p_stage]; for jj=1:opts.N_finite_elements(ii); sum_c = c_fun(x_prev); - for kk=1:opts.n_s + for kk=1:(opts.n_s + rbp) x_ijk = obj.w.x(ii,jj,kk); z_ijk = obj.w.z(ii,jj,kk); c_ijk = dcs.c_fun(x_ijk,z_ijk,v_global,p); @@ -522,7 +522,7 @@ function generate_complementarity_constraints(obj) for jj=1:opts.N_finite_elements(ii); sum_lambda = lambda_prev + sum2(obj.w.lambda(ii,jj,:)); sum_c = c_fun(x_prev); - for kk=1:opts.n_s + for kk=1:(opts.n_s + rbp) x_ijk = obj.w.x(ii,jj,kk); z_ijk = obj.w.z(ii,jj,kk); c_ijk = dcs.c_fun(x_ijk,z_ijk,v_global,p); @@ -538,14 +538,13 @@ function generate_complementarity_constraints(obj) 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 for jj=1:opts.N_finite_elements(ii); Gij = {}; Hij = {}; - for kk=1:opts.n_s + 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); @@ -590,16 +589,28 @@ function generate_step_equilibration_constraints(obj) 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_lambda_B = sum2(obj.w.lambda(ii,jj-1,:)); - sigma_theta_B = sum2(obj.w.theta(ii,jj-1,:)); - - sigma_lambda_F = obj.w.lambda(ii,jj-1,opts.n_s + rbp) + sum2(obj.w.lambda(ii,jj,:)); - sigma_theta_F = sum2(obj.w.theta(ii,jj,:)); - - pi_lambda = sigma_lambda_B .* sigma_lambda_F; - pi_theta = sigma_theta_B .* sigma_theta_F; - nu = pi_lambda + pi_theta; + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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); + + sigma_c_F = sigma_c_F + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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); @@ -608,20 +619,32 @@ function generate_step_equilibration_constraints(obj) 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 + 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_lambda_B = sum2(obj.w.lambda(ii,jj-1,:)); - sigma_theta_B = sum2(obj.w.theta(ii,jj-1,:)); - - sigma_lambda_F = obj.w.lambda(ii,jj-1,opts.n_s + rbp) +sum2(obj.w.lambda(ii,jj,:)); - sigma_theta_F = sum2(obj.w.theta(ii,jj,:)); + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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); - pi_lambda = sigma_lambda_B .* sigma_lambda_F; - pi_theta = sigma_theta_B .* sigma_theta_F; - nu = pi_lambda + pi_theta; + sigma_c_F = sigma_c_F + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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); @@ -634,16 +657,29 @@ function generate_step_equilibration_constraints(obj) 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_lambda_B = sum2(obj.w.lambda(ii,jj-1,:)); - sigma_theta_B = sum2(obj.w.theta(ii,jj-1,:)); - - sigma_lambda_F = obj.w.lambda(ii,jj-1,opts.n_s + rbp) + sum2(obj.w.lambda(ii,jj,:)); - sigma_theta_F = sum2(obj.w.theta(ii,jj,:)); + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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); + + sigma_c_F = sigma_c_F + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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; - pi_lambda = sigma_lambda_B .* sigma_lambda_F; - pi_theta = sigma_theta_B .* sigma_theta_F; - nu = pi_lambda + pi_theta; eta = 1; for jjj=1:length(nu) eta = eta*nu(jjj); @@ -658,16 +694,28 @@ function generate_step_equilibration_constraints(obj) 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_lambda_B = sum2(obj.w.lambda(ii,jj-1,:)); - sigma_theta_B = sum2(obj.w.theta(ii,jj-1,:)); - - sigma_lambda_F = obj.w.lambda(ii,jj-1,opts.n_s + rbp) + sum2(obj.w.lambda(ii,jj,:)); - sigma_theta_F = sum2(obj.w.theta(ii,jj,:)); + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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); + + sigma_c_F = sigma_c_F + dcs.c_fun(x_ijk,z_ijk, v_global, p); + sigma_lambda_F = sigma_lambda_F + obj.w.lambda(ii,jj,kk); + end - pi_lambda = sigma_lambda_B .* sigma_lambda_F; - pi_theta = sigma_theta_B .* sigma_theta_F; - nu = pi_lambda + pi_theta; + 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); @@ -682,38 +730,49 @@ function generate_step_equilibration_constraints(obj) %obj.eta_fun = Function('eta_fun', {obj.w.sym}, {eta_vec}); case StepEquilibrationMode.mlcp 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) - h0 = obj.p.T()/(opts.N_stages*opts.N_finite_elements(ii)); - sigma_lambda_B = sum2(obj.w.lambda(ii,jj-1,:)); - sigma_theta_B = sum2(obj.w.theta(ii,jj-1,:)); - - sigma_lambda_F = obj.w.lambda(ii,jj-1,opts.n_s + rbp) + sum2(obj.w.lambda(ii,jj,:)); - sigma_theta_F = sum2(obj.w.theta(ii,jj,:)); + sigma_c_B = 0; + sigma_lambda_B = 0; + for kk=1:(opts.n_s + rbp) + sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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); + + sigma_c_F = sigma_c_F + dcs.c_fun(x_ijk,z_ijk, v_global, p); + sigma_lambda_F = sigma_lambda_F + obj.w.lambda(ii,jj,kk); + end lambda_lambda = obj.w.lambda_lambda(ii,jj); - lambda_theta = obj.w.lambda_theta(ii,jj); + lambda_c = obj.w.lambda_c(ii,jj); B_max = obj.w.B_max(ii,jj); pi_lambda = obj.w.pi_lambda(ii,jj); - pi_theta = obj.w.pi_theta(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_theta_or(ii,jj) = {[pi_theta-sigma_theta_F;pi_theta-sigma_theta_B;sigma_theta_F+sigma_theta_B-pi_theta],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-lambda_theta-lambda_lambda; + kkt_max = [1-lambda_c-lambda_lambda; B_max-pi_lambda; - B_max-pi_theta]; + 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_theta)]}; - obj.H.step_eq_kkt_max(ii,jj) = {[lambda_lambda;lambda_theta]}; + 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_lambda;lambda_c]}; % eta calculation - eta_const = [eta-pi_theta;eta-pi_lambda;eta-pi_theta-pi_lambda+B_max]; + 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)]}; From 307d4f140651da2b61cbcbe9b4806d23447e892a Mon Sep 17 00:00:00 2001 From: Anton Pozharskiy Date: Fri, 17 May 2024 18:27:48 +0200 Subject: [PATCH 05/43] working pds, though differential_lift_x is _quite_ bad --- +nosnoc/+dcs/Gcs.m | 8 +- +nosnoc/+discrete_time_problem/Gcs.m | 1 - +nosnoc/+model/Pds.m | 12 +-- +nosnoc/+ocp/Solver.m | 6 +- +nosnoc/Integrator.m | 14 ++- .../new_design_moving_set.m | 47 ++++++++ examples/new_design_examples/new_design_pds.m | 44 ++++++++ .../new_design_examples/plot_moving_set.m | 101 ++++++++++++++++++ 8 files changed, 216 insertions(+), 17 deletions(-) create mode 100644 examples/new_design_examples/new_design_moving_set.m create mode 100644 examples/new_design_examples/new_design_pds.m create mode 100644 examples/new_design_examples/plot_moving_set.m diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index acdf556c..e3ef0871 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -23,7 +23,7 @@ function generate_variables(obj, opts) dims = obj.dims; % dimensions dims.n_lambda = dims.n_c; - obj.lambda = define_casadi_symbolic(opts.casadi_symbolic_mode,'lambda',obj.dims.n_lambda); + obj.lambda = define_casadi_symbolic(opts.casadi_symbolic_mode,'lambda',dims.n_lambda); obj.dims = dims; end @@ -35,14 +35,14 @@ function generate_equations(obj, opts) nabla_c = model.c.jacobian(model.x)'; - obj.f_x = model.f_x + obj.E*nabla_c*obj.lambda; + obj.f_x = model.f_x + 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, model.f_q}); 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, model.z, model.v_global, model.p}, {model.c}) - obj.nabla_c_fun = Function('c_fun', {model.x, model.z, model.v_global, model.p}, {nabla_c}) + obj.c_fun = Function('c_fun', {model.x, model.z, model.v_global, model.p}, {model.c}); + 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_comp_path_fun = Function('g_comp_path', {model.x, model.z, model.u, model.v_global, model.p}, {model.g_comp_path}); obj.g_terminal_fun = Function('g_terminal', {model.x, model.z, model.v_global, model.p_global}, {model.g_terminal}); diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index c804fcbc..c95f984a 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -304,7 +304,6 @@ function generate_direct_transcription_constraints(obj) x_ijk = obj.w.x(ii,jj,opts.n_s+1); z_ijk = obj.w.z(ii,jj,opts.n_s+1); lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); - mu_ijk = obj.w.mu(ii,jj,opts.n_s+1); obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; diff --git a/+nosnoc/+model/Pds.m b/+nosnoc/+model/Pds.m index 4ba926c3..6ed120fd 100644 --- a/+nosnoc/+model/Pds.m +++ b/+nosnoc/+model/Pds.m @@ -26,15 +26,15 @@ function verify_and_backfill(obj, opts) dims.n_c = size(obj.c,1); - if ~isempty(E) - if size(E, 1) ~= size(E,2) - error("nosnoc: Projection matrix E must be square."); + if ~isempty(obj.E) + if size(obj.E, 1) ~= dims.n_x + error("nosnoc: Projection matrix E must be an n_x by n_x matrix where n_x is the number of functions defining the set C.") end - if size(E, 1) ~= dims.n_c - error("nosnoc: Projection matrix E must be an n_c by n_c matrix where n_c is the number of functions defining the set C.") + if size(obj.E, 2) ~= dims.n_x + error("nosnoc: Projection matrix E must be an n_x by n_x matrix where n_x is the number of functions defining the set C.") end else - E = eye(dims.n_c); + obj.E = eye(dims.n_c); end obj.dims = dims; diff --git a/+nosnoc/+ocp/Solver.m b/+nosnoc/+ocp/Solver.m index 4e90ecdc..3955d2c8 100644 --- a/+nosnoc/+ocp/Solver.m +++ b/+nosnoc/+ocp/Solver.m @@ -46,7 +46,11 @@ case "nosnoc.model.Cls" error("not implemented") case "nosnoc.model.Pds" - error("not implemented") + obj.dcs = nosnoc.dcs.Gcs(model); + obj.dcs.generate_variables(opts); + obj.dcs.generate_equations(opts); + obj.discrete_time_problem = nosnoc.discrete_time_problem.Gcs(obj.dcs, opts); + obj.discrete_time_problem.populate_problem(); otherwise error("Unknown model type") end diff --git a/+nosnoc/Integrator.m b/+nosnoc/Integrator.m index 45ff5ebc..4cf6eb0c 100644 --- a/+nosnoc/Integrator.m +++ b/+nosnoc/Integrator.m @@ -39,7 +39,7 @@ obj.dcs.generate_equations(opts); obj.discrete_time_problem = nosnoc.discrete_time_problem.Stewart(obj.dcs, opts); obj.discrete_time_problem.populate_problem(); - elseif opts.dcs_mode == DcsMode.Heaviside % TODO: RENAME + elseif opts.dcs_mode == DcsMode.Heaviside obj.dcs = nosnoc.dcs.Heaviside(model); obj.dcs.generate_variables(opts); obj.dcs.generate_equations(opts); @@ -48,16 +48,20 @@ else error("PSS models can only be reformulated using the Stewart or Heaviside Step reformulations.") end - case "nosnoc.model.heaviside" + case "nosnoc.model.Heaviside" obj.dcs = nosnoc.dcs.Heaviside(model); obj.dcs.generate_variables(opts); obj.dcs.generate_equations(opts); obj.discrete_time_problem = nosnoc.discrete_time_problem.Heaviside(obj.dcs, opts); obj.discrete_time_problem.populate_problem(); - case "nosnoc.model.cls" - error("not implemented") - case "nosnoc.model.pds" + case "nosnoc.model.Cls" error("not implemented") + case "nosnoc.model.Pds" + obj.dcs = nosnoc.dcs.Gcs(model); + obj.dcs.generate_variables(opts); + obj.dcs.generate_equations(opts); + obj.discrete_time_problem = nosnoc.discrete_time_problem.Gcs(obj.dcs, opts); + obj.discrete_time_problem.populate_problem(); end end diff --git a/examples/new_design_examples/new_design_moving_set.m b/examples/new_design_examples/new_design_moving_set.m new file mode 100644 index 00000000..27e4366c --- /dev/null +++ b/examples/new_design_examples/new_design_moving_set.m @@ -0,0 +1,47 @@ + +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; +settings.homotopy_update_slope = 0.1; +%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; +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'; + + +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 = [0; 0; 1]; +model.E = diag([1,1,0]); + +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('$t$','Interpreter','latex') +ylabel('$x(t)$','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/examples/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m new file mode 100644 index 00000000..f1d4163d --- /dev/null +++ b/examples/new_design_examples/new_design_pds.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; +settings.homotopy_update_slope = 0.1; +%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; +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'; + + +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 = [x(2);-x(1)]; + +model.x0 = [0;pi-2;0]; + +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('$t$','Interpreter','latex') +ylabel('$x(t)$','Interpreter','latex') +grid on + diff --git a/examples/new_design_examples/plot_moving_set.m b/examples/new_design_examples/plot_moving_set.m new file mode 100644 index 00000000..37e6a668 --- /dev/null +++ b/examples/new_design_examples/plot_moving_set.m @@ -0,0 +1,101 @@ +function plot_moving_set(h,pos,r,type,fig,vidname) + n = length(r); + if ~exist('type') + type = []; + for ii=1:n + type = [type, "circle"]; + end + 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) + ylabel("$c_y$", "fontsize", 32) + + %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); + exportgraphics(gca, [vidname '_frames/' num2str(1) '.pdf']) + 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); + 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 From 0cddae476572ff89128637a5e4f824f230e50d31 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Tue, 21 May 2024 13:17:13 +0200 Subject: [PATCH 06/43] fix typo in user z --- +nosnoc/+discrete_time_problem/Gcs.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index c95f984a..5cf33da2 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -95,7 +95,7 @@ function create_variables(obj) obj.w.v(ii,1:opts.N_finite_elements(ii),1:opts.n_s) = {{'v', dims.n_x}}; end - obj.w.z(ii,1:opts.N_finite_elements(ii),(opts.n_s+rbp)) = {{'z', dims.n_z}, model.lbz, model.ubz, model.z0}; + 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}; % Handle x_box settings From ecee02ce4aa9f1ed75ed017a88f21c0d9e8f2d64 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Fri, 24 May 2024 13:30:31 +0200 Subject: [PATCH 07/43] fix to new path complementarity handling --- +nosnoc/+discrete_time_problem/Gcs.m | 27 +++++++++++++++------------ 1 file changed, 15 insertions(+), 12 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 5cf33da2..b37a54ab 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -185,10 +185,11 @@ function generate_direct_transcription_constraints(obj) 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}; end - if size(model.g_comp_path, 1) > 0 - g_comp_path = dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p); - obj.G.path(ii,jj,kk) = {g_comp_path(:,1)}; - obj.H.path(ii,jj,kk) = {g_comp_path(:,2)}; + 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); + 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 @@ -237,10 +238,11 @@ function generate_direct_transcription_constraints(obj) 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}; end - if size(model.g_comp_path, 1) > 0 - g_comp_path = dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p); - obj.G.path(ii,jj,kk) = {g_comp_path(:,1)}; - obj.H.path(ii,jj,kk) = {g_comp_path(:,2)}; + 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); + 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 @@ -290,10 +292,11 @@ function generate_direct_transcription_constraints(obj) 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}; end - if size(model.g_comp_path, 1) > 0 - g_comp_path = dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p); - obj.G.path(ii,jj,kk) = {g_comp_path(:,1)}; - obj.H.path(ii,jj,kk) = {g_comp_path(:,2)}; + 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); + 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 From 2c0533f42fc108e3d38fec9aa5274e11f61041aa Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Fri, 24 May 2024 13:31:45 +0200 Subject: [PATCH 08/43] custom display --- +nosnoc/+dcs/Gcs.m | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index e3ef0871..63ea01aa 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -52,4 +52,14 @@ function generate_equations(obj, opts) 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.lambda; + propgrp(2) = matlab.mixin.util.PropertyGroup(var_list, group_title); + end + end end From f620977fc899353a21a77a7ce627e28e010c3b0c Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 29 May 2024 09:55:03 +0200 Subject: [PATCH 09/43] some experimentation --- +nosnoc/+dcs/Gcs.m | 9 +++--- +nosnoc/+discrete_time_problem/Gcs.m | 28 +++++++++++-------- examples/new_design_examples/new_design_car.m | 4 +-- examples/new_design_examples/new_design_pds.m | 21 ++++++++++---- 4 files changed, 39 insertions(+), 23 deletions(-) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index 63ea01aa..b0cef75f 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -1,7 +1,5 @@ classdef Gcs < nosnoc.dcs.Base properties - model - lambda f_x @@ -10,6 +8,7 @@ nabla_c_fun c_fun + g_comp_path_fun end methods @@ -35,7 +34,9 @@ function generate_equations(obj, opts) nabla_c = model.c.jacobian(model.x)'; - obj.f_x = model.f_x + model.E*nabla_c*obj.lambda; + obj.f_x = model.f_x + model.E*nabla_c*obj.lambda; + + obj.c_dot = nabla_c*model.f_x; obj.f_x_fun = Function('f_x', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {obj.f_x, model.f_q}); obj.f_q_fun = Function('f_q', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {model.f_q}); @@ -44,7 +45,7 @@ function generate_equations(obj, opts) obj.c_fun = Function('c_fun', {model.x, model.z, model.v_global, model.p}, {model.c}); 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_comp_path_fun = Function('g_comp_path', {model.x, model.z, model.u, model.v_global, model.p}, {model.g_comp_path}); + obj.g_comp_path_fun = Function('g_comp_path', {model.x, model.z, model.u, model.v_global, model.p}, {[model.G_path,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}); diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index b37a54ab..3d73ecb1 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -328,12 +328,16 @@ function generate_direct_transcription_constraints(obj) % Least Squares Costs % TODO we should convert the refs to params - 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); - obj.f = obj.f + t_stage*dcs.f_lsq_u_fun(obj.w.u(ii),... - model.u_ref_val(:,ii),... - p); + 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, ui, v_global, p); end @@ -369,9 +373,11 @@ function generate_direct_transcription_constraints(obj) obj.f = obj.f + dcs.f_q_T_fun(x_end, z_end, v_global, p_global); % Terminal_lsq_cost - obj.f = obj.f + h0*opts.N_finite_elements(ii)*dcs.f_lsq_T_fun(x_end,... - model.x_ref_end_val,... - p_global); + 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 @@ -494,7 +500,7 @@ function generate_complementarity_constraints(obj) p_stage = obj.p.p_time_var(ii); p = [p_global;p_stage]; for jj=1:opts.N_finite_elements(ii); - sum_c = c_fun(x_prev); + 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); @@ -523,7 +529,7 @@ function generate_complementarity_constraints(obj) 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_fun(x_prev); + 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); 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/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m index f1d4163d..94534564 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/new_design_examples/new_design_pds.m @@ -4,7 +4,7 @@ close all import casadi.* %% discretization parameters -N_sim = 100; +N_sim = 31; T_sim = 10; %% NOSNOC settings @@ -13,14 +13,18 @@ problem_options.n_s = 2; settings.homotopy_update_slope = 0.1; %problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; +%problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; problem_options.rk_scheme = RKSchemes.RADAU_IIA; -problem_options.rk_representation= 'differential_lift_x'; -%problem_options.rk_representation= 'integral'; +%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; -solver_options.homotopy_steering_strategy = 'ELL_INF'; - +problem_options.rho_h = 1e-4; +%solver_options.homotopy_steering_strategy = 'ELL_INF'; +solver_options.complementarity_tol = 1e-12; model = nosnoc.model.Pds(); @@ -30,7 +34,7 @@ model.c = x(2)+0.2; model.f_x = [x(2);-x(1)]; -model.x0 = [0;pi-2;0]; +model.x0 = [0;pi-2]; integrator = nosnoc.Integrator(model, problem_options, solver_options); [t_grid, x_res] = integrator.simulate(); @@ -42,3 +46,8 @@ ylabel('$x(t)$','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'); +lam_full = integrator.get_full('lambda'); +[x_full',c,lam_full'] From 7ccc10d6d4e5e767e0aef18264ca4d5aa5b2cdc1 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 17 Jun 2024 13:22:49 +0200 Subject: [PATCH 10/43] minor changes and a few fixes --- +nosnoc/+dcs/Gcs.m | 2 -- +nosnoc/+discrete_time_problem/Gcs.m | 2 +- examples/new_design_examples/new_design_moving_set.m | 9 ++++----- examples/new_design_examples/new_design_pds.m | 5 ++--- examples/new_design_examples/plot_moving_set.m | 9 +++++++-- 5 files changed, 14 insertions(+), 13 deletions(-) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index b0cef75f..b527094f 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -36,8 +36,6 @@ function generate_equations(obj, opts) obj.f_x = model.f_x + model.E*nabla_c*obj.lambda; - obj.c_dot = nabla_c*model.f_x; - obj.f_x_fun = Function('f_x', {model.x, model.z, obj.lambda, model.u, model.v_global, model.p}, {obj.f_x, model.f_q}); 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}); diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 3d73ecb1..d8fe7545 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -368,7 +368,7 @@ function generate_direct_transcription_constraints(obj) 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) + 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); diff --git a/examples/new_design_examples/new_design_moving_set.m b/examples/new_design_examples/new_design_moving_set.m index 27e4366c..5a5287ba 100644 --- a/examples/new_design_examples/new_design_moving_set.m +++ b/examples/new_design_examples/new_design_moving_set.m @@ -10,18 +10,17 @@ %% NOSNOC settings problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); -problem_options.n_s = 2; -settings.homotopy_update_slope = 0.1; +problem_options.n_s = 3; %problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; problem_options.rk_scheme = RKSchemes.RADAU_IIA; -problem_options.rk_representation= 'differential_lift_x'; -%problem_options.rk_representation= 'integral'; +%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 = 2; model = nosnoc.model.Pds(); diff --git a/examples/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m index 94534564..61b71026 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/new_design_examples/new_design_pds.m @@ -10,8 +10,7 @@ %% NOSNOC settings problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); -problem_options.n_s = 2; -settings.homotopy_update_slope = 0.1; +problem_options.n_s = 4; %problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; %problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; problem_options.rk_scheme = RKSchemes.RADAU_IIA; @@ -25,6 +24,7 @@ problem_options.rho_h = 1e-4; %solver_options.homotopy_steering_strategy = 'ELL_INF'; solver_options.complementarity_tol = 1e-12; +solver_options.print_level = 2; model = nosnoc.model.Pds(); @@ -50,4 +50,3 @@ c = full(c_fun(integrator.get_full('x')))'; x_full = integrator.get_full('x'); lam_full = integrator.get_full('lambda'); -[x_full',c,lam_full'] diff --git a/examples/new_design_examples/plot_moving_set.m b/examples/new_design_examples/plot_moving_set.m index 37e6a668..1b837866 100644 --- a/examples/new_design_examples/plot_moving_set.m +++ b/examples/new_design_examples/plot_moving_set.m @@ -1,4 +1,4 @@ -function plot_moving_set(h,pos,r,type,fig,vidname) +function plot_moving_set(h,pos,r,type,fig,vidname,export_frames) n = length(r); if ~exist('type') type = []; @@ -6,6 +6,9 @@ function plot_moving_set(h,pos,r,type,fig,vidname) type = [type, "circle"]; end end + if ~exist('export_frames') + export_frames = false; + end if length(type) ~= n error('not all types specified') end @@ -67,7 +70,9 @@ function plot_moving_set(h,pos,r,type,fig,vidname) writer = VideoWriter([vidname '.avi']); open(writer); frame = getframe(gca); - exportgraphics(gca, [vidname '_frames/' num2str(1) '.pdf']) + if export_frames + exportgraphics(gca, [vidname '_frames/' num2str(1) '.pdf']) + end writeVideo(writer,frame) end pause(0.1); From f02892fae0fb864a79fbc38d9afb594d710f9338 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 17 Jun 2024 13:27:05 +0200 Subject: [PATCH 11/43] fix typo --- +nosnoc/+discrete_time_problem/Gcs.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index d8fe7545..b2d70d14 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -342,7 +342,7 @@ function generate_direct_transcription_constraints(obj) obj.f = obj.f + dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); end - % Clock Date: Wed, 19 Jun 2024 11:29:50 +0200 Subject: [PATCH 12/43] mlcp rename --- +nosnoc/+discrete_time_problem/Gcs.m | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index b2d70d14..9d8dac4c 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -64,7 +64,7 @@ function create_variables(obj) end obj.w.h(ii,1:opts.N_finite_elements(ii)) = {{'h', 1}, lbh, ubh, h0}; end - if obj.opts.step_equilibration == StepEquilibrationMode.mlcp + 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}; obj.w.pi_c(ii,2:opts.N_finite_elements(ii)) = {{'pi_c', dims.n_c},-inf,inf}; obj.w.pi_lambda(ii,2:opts.N_finite_elements(ii)) = {{'pi_lambda', dims.n_lambda},-inf,inf}; @@ -736,7 +736,7 @@ function generate_step_equilibration_constraints(obj) end end %obj.eta_fun = Function('eta_fun', {obj.w.sym}, {eta_vec}); - case StepEquilibrationMode.mlcp + case StepEquilibrationMode.linear_complementarity for ii=1:opts.N_stages p_stage = obj.p.p_time_var(ii); p =[p_global;p_stage]; From 52243eec19549c2ca11142aff7d529c94e0e2a6a Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 19 Jun 2024 14:37:28 +0200 Subject: [PATCH 13/43] bring in line with stewart re t_stage --- +nosnoc/+discrete_time_problem/Gcs.m | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 9d8dac4c..0cae58d0 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -132,12 +132,7 @@ function generate_direct_transcription_constraints(obj) x_prev = obj.w.x(0,0,opts.n_s); for ii=1:opts.N_stages - if obj.opts.use_fesd - t_stage = obj.p.T()/opts.N_stages; - h0 = obj.p.T().val/(opts.N_stages*opts.N_finite_elements(ii)); - else - h0 = obj.p.T().val/(opts.N_stages*opts.N_finite_elements(ii)); - end + h0 = obj.p.T().val/(opts.N_stages*opts.N_finite_elements(ii)); ui = obj.w.u(ii); p_stage = obj.p.p_time_var(ii); @@ -149,6 +144,13 @@ function generate_direct_transcription_constraints(obj) 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) From 0e89e24e47fbe9ae8619ba513a24e2aed85f88ce Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Thu, 20 Jun 2024 15:06:34 +0200 Subject: [PATCH 14/43] add Cybership OCP example --- examples/new_design_examples/new_design_msv.m | 98 +++++++++++++++++++ 1 file changed, 98 insertions(+) create mode 100644 examples/new_design_examples/new_design_msv.m diff --git a/examples/new_design_examples/new_design_msv.m b/examples/new_design_examples/new_design_msv.m new file mode 100644 index 00000000..cc8ebf93 --- /dev/null +++ b/examples/new_design_examples/new_design_msv.m @@ -0,0 +1,98 @@ +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.GAUSS_LEGENDRE; +%problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; +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; +%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.lbu = [-100/sqrt(2);-100/sqrt(2);0;0]; +%model.ubu = [100/sqrt(2);100/sqrt(2);0;0]; +model.u0 = model.ubu; +model.c = [norm_2(x2-x1)-2*R]; +model.f_x = [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'); +h_res = ocp_solver.get('h'); +t_res = [0,cumsum(h_res)]; +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,:))); +%plot_discs(h_res,x_res,[3.5,3.5], ["circle", "circle"]) +v_res = u_rep + repmat(lambda_res(2:end),4,1).*nabla_c_res(:, 2:end); +figure('Position', [0,0, 400. 400]) +ax = subplot(3,1,1); +plot(t_res(1:end-1), v_res([1,3],:), "LineWidth", 2) +xlabel("$t$") +ylabel("$v_x$") +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$") +ylabel("$v_y$") +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$") +ylabel("$c(x)$") +ax.FontSize = fontsize; +ax.FontSize = fontsize; +ylim([-0.1, 10]) +xlim([0, T_final]) +grid on From 45ca56452b480a57b437c57edaf52471a5cdd668 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Thu, 27 Jun 2024 10:29:23 +0200 Subject: [PATCH 15/43] bump vdx --- external/vdx | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From a49b687a67c057e5e77c42c2c88dfb1faf8bf9a6 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Thu, 27 Jun 2024 10:58:16 +0200 Subject: [PATCH 16/43] some testing of c_n != 1 and formally erroring on differential rk representation --- +nosnoc/+discrete_time_problem/Gcs.m | 56 +------------------ examples/new_design_examples/new_design_pds.m | 14 ++--- 2 files changed, 9 insertions(+), 61 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 0cae58d0..e143c295 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -211,60 +211,8 @@ function generate_direct_transcription_constraints(obj) obj.g.path(ii,jj) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p), model.lbg_path, model.ubg_path}; end case RKRepresentation.differential - error("Differential representation is currently unsupported") - % In differential representation stage variables are the state derivatives. - X_ijk = {}; - for kk = 1:opts.n_s - 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 - X_ijk = [X_ijk {x_temp}]; - end - X_ijk = [X_ijk, {obj.w.x(ii,jj,opts.n_s)}]; - x_ij_end = x_prev; - for kk=1:opts.n_s - x_ijk = X_ijk{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); - - fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); - qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, ui, 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.c_lb(ii,jj,kk) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; - - 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}; - 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); - 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; - end - end - if ~opts.right_boundary_point_explicit - x_ijk = obj.w.x(ii,jj,opts.n_s+1); - z_ijk = obj.w.z(ii,jj,opts.n_s+1); - lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); - - obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; - obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, 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}; - end + 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. diff --git a/examples/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m index 61b71026..563d3dbf 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/new_design_examples/new_design_pds.m @@ -11,20 +11,20 @@ problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); problem_options.n_s = 4; -%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; +problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; %problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; -problem_options.rk_scheme = RKSchemes.RADAU_IIA; -%problem_options.rk_representation= RKRepresentation.differential_lift_x; -problem_options.rk_representation = RKRepresentation.integral; +%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.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; %solver_options.homotopy_steering_strategy = 'ELL_INF'; -solver_options.complementarity_tol = 1e-12; -solver_options.print_level = 2; +solver_options.complementarity_tol = 1e-10; +solver_options.print_level = 3; model = nosnoc.model.Pds(); From 32ce3ef0e86d3894cde83547f25a4210e048a74c Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Fri, 28 Jun 2024 16:18:37 +0200 Subject: [PATCH 17/43] some options for handling PDS with c_n != 0 --- +nosnoc/+dcs/Gcs.m | 5 ++++- +nosnoc/+discrete_time_problem/Gcs.m | 19 +++++++++++++++++-- examples/new_design_examples/new_design_pds.m | 6 +++--- 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index b527094f..29dc27be 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -6,6 +6,7 @@ dims + f_fun nabla_c_fun c_fun g_comp_path_fun @@ -37,13 +38,15 @@ function generate_equations(obj, opts) obj.f_x = model.f_x + 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, model.f_q}); + obj.f_fun = Function('f', {model.x, model.z, model.u, model.v_global, model.p}, {model.f_x}); 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, model.z, model.v_global, model.p}, {model.c}); 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_comp_path_fun = Function('g_comp_path', {model.x, model.z, model.u, model.v_global, model.p}, {[model.G_path,model.H_path]}); + 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}); diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index e143c295..dc5d947b 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -73,6 +73,10 @@ function create_variables(obj) obj.w.eta(ii,2:opts.N_finite_elements(ii)) = {{'eta', dims.n_lambda},0,inf}; obj.w.nu(ii,2:opts.N_finite_elements(ii)) = {{'nu', 1},0,inf}; end + if ~opts.right_boundary_point_explicit + obj.w.not_c_primals(ii,1:opts.N_finite_elements(ii)) = {{'not_c_primal', 2}, 0 inf, 0.5}; + obj.w.mu_not_c(ii,1:opts.N_finite_elements(ii)) = {{'mu_lambda', 3}, 0 inf, 0.5}; + end end % For c_n ~= 1 case @@ -165,7 +169,7 @@ function generate_direct_transcription_constraints(obj) switch opts.rk_representation case RKRepresentation.integral % In integral representation stage variables are states. - x_ij_end = x_prev; + 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); @@ -202,7 +206,18 @@ function generate_direct_transcription_constraints(obj) x_ijk = obj.w.x(ii,jj,opts.n_s+1); z_ijk = obj.w.z(ii,jj,opts.n_s+1); lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); - + mu_not_c_ij = obj.w.mu_not_c(ii,jj); + not_c_primals_ij = obj.w.not_c_primals(ii,jj); + f_ijk = dcs.f_fun(x_ijk, z_ijk, ui, v_global, p); + nabla_c_ijk = dcs.nabla_c_fun(x_ijk, z_ijk, v_global, p); + lambda_cf_ijk = inv(nabla_c_ijk'*nabla_c_ijk)*nabla_c_ijk'*f_ijk; + + c = not_c_primals_ij(1); d = not_c_primals_ij(2); + c_ijk = dcs.c_fun(x_ijk, z_ijk, v_global, p); + obj.g.not_c(ii,jj) = {[c + d*c_ijk - 1;1-mu_not_c_ij(1) - mu_not_c_ij(2);mu_not_c_ij(1)*c_ijk + mu_not_c_ij(3)], [0;0;0], [inf;0;0]}; + obj.G.not_c(ii,jj) = {[c + d*c_ijk - 1;c;d]}; + obj.H.not_c(ii,jj) = {[mu_not_c_ij]}; + obj.g.lambda_explicit(ii,jj) = {[lambda_ijk-c*lambda_cf_ijk]}; obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, ui, v_global, p)}; diff --git a/examples/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m index 563d3dbf..aae8ddc1 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/new_design_examples/new_design_pds.m @@ -10,12 +10,12 @@ %% NOSNOC settings problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); -problem_options.n_s = 4; +problem_options.n_s = 3; problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; %problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; %problem_options.rk_scheme = RKSchemes.RADAU_IIA; -problem_options.rk_representation= RKRepresentation.differential_lift_x; -%problem_options.rk_representation = RKRepresentation.integral; +%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; From 813331f7c1c7d6e17b8f975b71e122f1207697b7 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 10:26:58 +0200 Subject: [PATCH 18/43] updating and addressing some of armins comments --- +nosnoc/+discrete_time_problem/Gcs.m | 93 ++++++++++--------- +nosnoc/Integrator.m | 2 +- .../new_design_moving_set.m | 6 +- examples/new_design_examples/new_design_pds.m | 4 +- .../new_design_examples/plot_moving_set.m | 2 +- 5 files changed, 58 insertions(+), 49 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index dc5d947b..f7ad57a1 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -5,6 +5,7 @@ opts populated + sorted end methods @@ -66,11 +67,17 @@ function create_variables(obj) 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}; - obj.w.lambda_c(ii,2:opts.N_finite_elements(ii)) = {{'lambda_c', dims.n_c},0,inf}; - obj.w.lambda_lambda(ii,2:opts.N_finite_elements(ii)) = {{'lambda_lambda', dims.n_lambda},0,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 if ~opts.right_boundary_point_explicit @@ -138,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 @@ -175,31 +182,31 @@ function generate_direct_transcription_constraints(obj) z_ijk = obj.w.z(ii,jj,kk); lambda_ijk = obj.w.lambda(ii,jj,kk); - fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); - qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); + 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 * fj - xk}; - obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_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.c_lb(ii,jj,kk) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; 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 @@ -208,7 +215,7 @@ function generate_direct_transcription_constraints(obj) lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); mu_not_c_ij = obj.w.mu_not_c(ii,jj); not_c_primals_ij = obj.w.not_c_primals(ii,jj); - f_ijk = dcs.f_fun(x_ijk, z_ijk, ui, v_global, p); + f_ijk = dcs.f_fun(x_ijk, z_ijk, u_i, v_global, p); nabla_c_ijk = dcs.nabla_c_fun(x_ijk, z_ijk, v_global, p); lambda_cf_ijk = inv(nabla_c_ijk'*nabla_c_ijk)*nabla_c_ijk'*f_ijk; @@ -220,10 +227,10 @@ function generate_direct_transcription_constraints(obj) obj.g.lambda_explicit(ii,jj) = {[lambda_ijk-c*lambda_cf_ijk]}; obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; - 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 error("Differential representation without lifting is unsupported for gradient complementarity systems") @@ -246,26 +253,26 @@ function generate_direct_transcription_constraints(obj) z_ijk = obj.w.z(ii,jj,kk); lambda_ijk = obj.w.lambda(ii,jj,kk); - fj = s_sot*dcs.f_x_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); - qj = s_sot*dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, ui, v_global, p); + 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) = {fj - v_ijk}; - obj.g.z(ii,jj,kk) = {dcs.g_z_fun(x_ijk, z_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.c_lb(ii,jj,kk) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; 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 +282,12 @@ function generate_direct_transcription_constraints(obj) obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; - 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); @@ -288,7 +295,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 @@ -304,12 +311,10 @@ 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, ui, v_global, p); + obj.f = obj.f + dcs.f_q_fun(x_ijk, z_ijk, lambda_ijk, u_i, v_global, p); end % Clock Constraints - % TODO(@anton) HERE BE DRAGONS. This is by far the worst part of current nosnoc as it requires the discrete problem - % to understand something about the time-freezing reformulation which is ugly. if obj.opts.use_fesd && opts.equidistant_control_grid if opts.time_optimal_problem if opts.use_speed_of_time_variables @@ -722,8 +727,8 @@ function generate_step_equilibration_constraints(obj) sigma_lambda_F = sigma_lambda_F + obj.w.lambda(ii,jj,kk); end - lambda_lambda = obj.w.lambda_lambda(ii,jj); - lambda_c = obj.w.lambda_c(ii,jj); + 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); @@ -734,7 +739,7 @@ function generate_step_equilibration_constraints(obj) 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-lambda_c-lambda_lambda; + kkt_max = [1-c_mult-lambda_mult; B_max-pi_lambda; B_max-pi_c]; obj.g.kkt_max(ii,jj) = {kkt_max, @@ -742,7 +747,7 @@ function generate_step_equilibration_constraints(obj) [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_lambda;lambda_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]; @@ -772,7 +777,7 @@ function populate_problem(obj) obj.populated = true; end - function create_solver(obj, solver_options, plugin) + function create_solver(obj, solver_options, plugin, regenerate) if ~obj.populated obj.populate_problem(); end @@ -781,13 +786,22 @@ function create_solver(obj, solver_options, plugin) plugin = 'scholtes_ineq'; end - % Sort by indices to recover almost block-band structure. - obj.w.sort_by_index(); - obj.g.sort_by_index(); + 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; - obj.solver = nosnoc.solver.mpccsol('Mpcc solver', plugin, obj.to_casadi_struct(), solver_options); + 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) @@ -823,8 +837,6 @@ function create_solver(obj, solver_options, plugin) 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; - results.mu = obj.discrete_time_problem.w.mu(:,:,obj.opts.n_s).res; - results.theta = obj.discrete_time_problem.w.theta(:,:,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]; @@ -832,9 +844,6 @@ function create_solver(obj, solver_options, plugin) 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]; - results.mu = [obj.discrete_time_problem.w.mu(0,0,obj.opts.n_s).res,... - obj.discrete_time_problem.w.mu(1:opts.N_stages,:,obj.opts.n_s+1).res]; - results.theta = obj.discrete_time_problem.w.theta(:,:,obj.opts.n_s+1).res; end results.u = obj.w.u.res; diff --git a/+nosnoc/Integrator.m b/+nosnoc/Integrator.m index 4cf6eb0c..b0670ba1 100644 --- a/+nosnoc/Integrator.m +++ b/+nosnoc/Integrator.m @@ -75,7 +75,7 @@ obj.stats = [obj.stats,stats]; end - function [t_grid,x_res,t_grid_full,x_res_full] = simulate(obj) + function [t_grid,x_res,t_grid_full,x_res_full] = simulate(obj, plugin) if ~exist('plugin', 'var') plugin = 'scholtes_ineq'; end diff --git a/examples/new_design_examples/new_design_moving_set.m b/examples/new_design_examples/new_design_moving_set.m index 5a5287ba..f4e84943 100644 --- a/examples/new_design_examples/new_design_moving_set.m +++ b/examples/new_design_examples/new_design_moving_set.m @@ -33,13 +33,13 @@ model.E = diag([1,1,0]); integrator = nosnoc.Integrator(model, problem_options, solver_options); -[t_grid, x_res] = integrator.simulate(); +[t_grid, x_res] = integrator.simulate('natural_residual_ineq'); % figure plot(x_res(1,:), x_res(2,:)) grid on -xlabel('$t$','Interpreter','latex') -ylabel('$x(t)$','Interpreter','latex') +xlabel('$x_1(t)$','Interpreter','latex') +ylabel('$x_2(t)$','Interpreter','latex') grid on fig = figure('Position', [10 10 1600 800]); diff --git a/examples/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m index aae8ddc1..269527dc 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/new_design_examples/new_design_pds.m @@ -42,8 +42,8 @@ figure plot(x_res(1,:), x_res(2,:)) grid on -xlabel('$t$','Interpreter','latex') -ylabel('$x(t)$','Interpreter','latex') +xlabel('$x_1(t)$','Interpreter','latex') +ylabel('$x_2(t)$','Interpreter','latex') grid on c_fun = casadi.Function('c', {model.x}, {model.c}); diff --git a/examples/new_design_examples/plot_moving_set.m b/examples/new_design_examples/plot_moving_set.m index 1b837866..3dfc37cd 100644 --- a/examples/new_design_examples/plot_moving_set.m +++ b/examples/new_design_examples/plot_moving_set.m @@ -87,7 +87,7 @@ function plot_moving_set(h,pos,r,type,fig,vidname,export_frames) end discs{ii}.Vertices = v; end - t = pos(end,jj); + t = pos(end,jj+1); v_C = circ([sin(t);cos(t)],1); C.Vertices = v_C; drawnow limitrate; From b8077709b72336ee364c23978b657ac2e4bacde9 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 10:50:38 +0200 Subject: [PATCH 19/43] fix use_fesd=false issues --- +nosnoc/+discrete_time_problem/Gcs.m | 32 +++++++++++-------- examples/new_design_examples/new_design_msv.m | 7 ++-- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index f7ad57a1..f7354ea1 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -4,8 +4,8 @@ dcs opts - populated - sorted + populated = false + sorted = false end methods @@ -519,6 +519,8 @@ function generate_complementarity_constraints(obj) 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 = {}; @@ -808,19 +810,21 @@ function create_solver(obj, solver_options, plugin, regenerate) opts = obj.opts; T_val = obj.p.T().val; - 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; + 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 - obj.w.h(ii,jj).lb = lbh; - obj.w.h(ii,jj).ub = ubh; end end diff --git a/examples/new_design_examples/new_design_msv.m b/examples/new_design_examples/new_design_msv.m index cc8ebf93..ba75082c 100644 --- a/examples/new_design_examples/new_design_msv.m +++ b/examples/new_design_examples/new_design_msv.m @@ -20,6 +20,7 @@ problem_options.T = 1; problem_options.rho_h = 1e-4; problem_options.time_optimal_problem = true; +problem_options.use_fesd = true; %solver_options.homotopy_steering_strategy = 'ELL_INF'; solver_options.complementarity_tol = 1e-10; solver_options.print_level = 3; @@ -60,8 +61,10 @@ 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'); -h_res = ocp_solver.get('h'); -t_res = [0,cumsum(h_res)]; +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,:))); From 6dbc31a51fa01aeff45a9f9dd7957b1d28c4842c Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 11:16:00 +0200 Subject: [PATCH 20/43] fixing stage_stage bug and some other minor issues --- +nosnoc/+discrete_time_problem/Gcs.m | 2 +- +nosnoc/+ocp/Solver.m | 8 +++++++- +nosnoc/+solver/MpccSolver.m | 2 +- examples/new_design_examples/new_design_msv.m | 6 +++--- examples/new_design_examples/new_design_pds.m | 4 ++-- 5 files changed, 14 insertions(+), 8 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index f7354ea1..7d1a7b16 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -419,7 +419,7 @@ function generate_complementarity_constraints(obj) for rr=1:(opts.n_s + rbp) lambda_ijr = obj.w.lambda(ii,jj,rr); - Gij = vertcat(Gij, {lambda_prev}); + Gij = vertcat(Gij, {lambda_ijr}); Hij = vertcat(Hij, {c_prev}); end for kk=1:(opts.n_s + rbp) diff --git a/+nosnoc/+ocp/Solver.m b/+nosnoc/+ocp/Solver.m index 3955d2c8..c4d3ba95 100644 --- a/+nosnoc/+ocp/Solver.m +++ b/+nosnoc/+ocp/Solver.m @@ -70,7 +70,13 @@ function solve(obj, plugin) try var = obj.discrete_time_problem.w.(field); catch - error(['nosnoc:' char(field) ' is not a valid field for this OCP']); + if strcmp(field, 'T_final') + warning("You are trying to get the final time from a non-time-optimal problem. Instead returning the fixed time.") + ret = obj.discrete_time_problem.p.T.val; + return + else + error(['nosnoc:' char(field) ' is not a valid field for this OCP']); + end % TODO@anton print list of valid fields. end if var.depth == 3 diff --git a/+nosnoc/+solver/MpccSolver.m b/+nosnoc/+solver/MpccSolver.m index 08b8a22b..38294ddc 100644 --- a/+nosnoc/+solver/MpccSolver.m +++ b/+nosnoc/+solver/MpccSolver.m @@ -205,7 +205,7 @@ function varargout = size(obj,varargin) % This is a required overload for matlab.mixin.indexing.RedefinesParen. % In the case of a scalar like this class returning 1 or throwing an error is prefered. - varargout = 1; + varargout = {1}; end function ind = end(obj,k,n) diff --git a/examples/new_design_examples/new_design_msv.m b/examples/new_design_examples/new_design_msv.m index ba75082c..565e176c 100644 --- a/examples/new_design_examples/new_design_msv.m +++ b/examples/new_design_examples/new_design_msv.m @@ -10,10 +10,10 @@ %problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; %problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; problem_options.rk_scheme = RKSchemes.RADAU_IIA; -%problem_options.rk_representation= RKRepresentation.differential_lift_x; -problem_options.rk_representation = RKRepresentation.integral; +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.cross_comp_mode = CrossCompMode.FE_FE; problem_options.N_finite_elements = 2; problem_options.n_s = 2; problem_options.N_stages = 25; diff --git a/examples/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m index 269527dc..6570a42b 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/new_design_examples/new_design_pds.m @@ -11,9 +11,9 @@ problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); problem_options.n_s = 3; -problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; +%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; %problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; -%problem_options.rk_scheme = RKSchemes.RADAU_IIA; +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; From 83472e9a35f5c5f7be087b1a42a09155890aa0cf Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 11:28:50 +0200 Subject: [PATCH 21/43] Error on c_n != 0 --- +nosnoc/+discrete_time_problem/Gcs.m | 31 ++----------------- +nosnoc/+ocp/Solver.m | 3 ++ examples/new_design_examples/new_design_msv.m | 4 +-- 3 files changed, 7 insertions(+), 31 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 7d1a7b16..defbb913 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -80,10 +80,6 @@ function create_variables(obj) % an or of all the switch indicators. obj.w.nu(ii,2:opts.N_finite_elements(ii)) = {{'nu', 1},0,inf}; end - if ~opts.right_boundary_point_explicit - obj.w.not_c_primals(ii,1:opts.N_finite_elements(ii)) = {{'not_c_primal', 2}, 0 inf, 0.5}; - obj.w.mu_not_c(ii,1:opts.N_finite_elements(ii)) = {{'mu_lambda', 3}, 0 inf, 0.5}; - end end % For c_n ~= 1 case @@ -210,24 +206,7 @@ function generate_direct_transcription_constraints(obj) end end if ~opts.right_boundary_point_explicit - x_ijk = obj.w.x(ii,jj,opts.n_s+1); - z_ijk = obj.w.z(ii,jj,opts.n_s+1); - lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); - mu_not_c_ij = obj.w.mu_not_c(ii,jj); - not_c_primals_ij = obj.w.not_c_primals(ii,jj); - f_ijk = dcs.f_fun(x_ijk, z_ijk, u_i, v_global, p); - nabla_c_ijk = dcs.nabla_c_fun(x_ijk, z_ijk, v_global, p); - lambda_cf_ijk = inv(nabla_c_ijk'*nabla_c_ijk)*nabla_c_ijk'*f_ijk; - - c = not_c_primals_ij(1); d = not_c_primals_ij(2); - c_ijk = dcs.c_fun(x_ijk, z_ijk, v_global, p); - obj.g.not_c(ii,jj) = {[c + d*c_ijk - 1;1-mu_not_c_ij(1) - mu_not_c_ij(2);mu_not_c_ij(1)*c_ijk + mu_not_c_ij(3)], [0;0;0], [inf;0;0]}; - obj.G.not_c(ii,jj) = {[c + d*c_ijk - 1;c;d]}; - obj.H.not_c(ii,jj) = {[mu_not_c_ij]}; - obj.g.lambda_explicit(ii,jj) = {[lambda_ijk-c*lambda_cf_ijk]}; - obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; - obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + 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}; @@ -276,13 +255,7 @@ function generate_direct_transcription_constraints(obj) end end if ~opts.right_boundary_point_explicit - x_ijk = obj.w.x(ii,jj,opts.n_s+1); - z_ijk = obj.w.z(ii,jj,opts.n_s+1); - lambda_ijk = obj.w.lambda(ii,jj,opts.n_s+1); - - obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ijk - x_ij_end}; - obj.g.c_lb(ii,jj,opts.n_s+1) = {dcs.c_fun(x_ijk, z_ijk, v_global, p), 0, inf}; - obj.g.z(ii,jj,opts.n_s+1) = {dcs.g_z_fun(x_ijk, z_ijk, u_i, v_global, p)}; + error("not implemented"); else obj.g.dynamics(ii,jj,opts.n_s+1) = {x_ij_end - obj.w.x(ii,jj,opts.n_s)}; end diff --git a/+nosnoc/+ocp/Solver.m b/+nosnoc/+ocp/Solver.m index c4d3ba95..d3a6585e 100644 --- a/+nosnoc/+ocp/Solver.m +++ b/+nosnoc/+ocp/Solver.m @@ -46,6 +46,9 @@ case "nosnoc.model.Cls" error("not implemented") case "nosnoc.model.Pds" + if ~opts.right_boundary_point_explicit + error("You are using an rk scheme with its right boundary point (c_n) not equal to one. Please choose another scheme e.g. RADAU_IIA") + end obj.dcs = nosnoc.dcs.Gcs(model); obj.dcs.generate_variables(opts); obj.dcs.generate_equations(opts); diff --git a/examples/new_design_examples/new_design_msv.m b/examples/new_design_examples/new_design_msv.m index 565e176c..644bae60 100644 --- a/examples/new_design_examples/new_design_msv.m +++ b/examples/new_design_examples/new_design_msv.m @@ -10,8 +10,8 @@ %problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; %problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; problem_options.rk_scheme = RKSchemes.RADAU_IIA; -problem_options.rk_representation= RKRepresentation.differential_lift_x; -%problem_options.rk_representation = RKRepresentation.integral; +%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; From 4f8f672630d1050a89f87dd568a4948f30920e39 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 11:33:00 +0200 Subject: [PATCH 22/43] comment on (0,0,n_s) --- +nosnoc/+discrete_time_problem/Gcs.m | 1 + 1 file changed, 1 insertion(+) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index defbb913..068af4d7 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -130,6 +130,7 @@ function generate_direct_transcription_constraints(obj) 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); lambda_0 = obj.w.lambda(0,0,opts.n_s); From b163b8129c4fecc1daa0c0736cb38f9b8517078f Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 11:36:09 +0200 Subject: [PATCH 23/43] add error for differential mode and errors also in integrator --- +nosnoc/+discrete_time_problem/Gcs.m | 12 +++--------- +nosnoc/+ocp/Solver.m | 3 +++ +nosnoc/Integrator.m | 6 ++++++ 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 068af4d7..4df0d864 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -91,14 +91,8 @@ function create_variables(obj) obj.w.z(0,0,opts.n_s) = {{'z', dims.n_z}, model.lbz, model.ubz, model.z0}; obj.w.lambda(0,0,opts.n_s) = {{['lambda'], dims.n_lambda},0,inf}; for ii=1:opts.N_stages - if (opts.rk_representation == RKRepresentation.integral ||... - opts.rk_representation == RKRepresentation.differential_lift_x) - 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}; - else - obj.w.x(ii,1:opts.N_finite_elements(ii),opts.n_s) = {{'x', dims.n_x}, model.lbx, model.ubx, model.x0}; - end - if (opts.rk_representation == RKRepresentation.differential ||... - opts.rk_representation == RKRepresentation.differential_lift_x) + 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}}; end @@ -106,7 +100,7 @@ function create_variables(obj) obj.w.lambda(ii,1:opts.N_finite_elements(ii),1:(opts.n_s+rbp)) = {{'lambda', dims.n_lambda},0, inf}; % Handle x_box settings - if ~opts.x_box_at_stg && opts.rk_representation ~= RKRepresentation.differential + 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 diff --git a/+nosnoc/+ocp/Solver.m b/+nosnoc/+ocp/Solver.m index d3a6585e..fc00cc0f 100644 --- a/+nosnoc/+ocp/Solver.m +++ b/+nosnoc/+ocp/Solver.m @@ -49,6 +49,9 @@ if ~opts.right_boundary_point_explicit error("You are using an rk scheme with its right boundary point (c_n) not equal to one. Please choose another scheme e.g. RADAU_IIA") end + if opts.rk_representation == RKRepresentation.differential + error("Differential representation without lifting is unsupported for gradient complementarity systems. Use integral or lifted differential representation") + end obj.dcs = nosnoc.dcs.Gcs(model); obj.dcs.generate_variables(opts); obj.dcs.generate_equations(opts); diff --git a/+nosnoc/Integrator.m b/+nosnoc/Integrator.m index b0670ba1..0c2d9162 100644 --- a/+nosnoc/Integrator.m +++ b/+nosnoc/Integrator.m @@ -57,6 +57,12 @@ case "nosnoc.model.Cls" error("not implemented") case "nosnoc.model.Pds" + if ~opts.right_boundary_point_explicit + error("You are using an rk scheme with its right boundary point (c_n) not equal to one. Please choose another scheme e.g. RADAU_IIA") + end + if opts.rk_representation == RKRepresentation.differential + error("Differential representation without lifting is unsupported for gradient complementarity systems. Use integral or lifted differential representation") + end obj.dcs = nosnoc.dcs.Gcs(model); obj.dcs.generate_variables(opts); obj.dcs.generate_equations(opts); From bc00c1821367f4729427935bd845db20a87af19b Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 11:37:26 +0200 Subject: [PATCH 24/43] comment v=f_x --- +nosnoc/+discrete_time_problem/Gcs.m | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 4df0d864..06323b46 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -93,7 +93,7 @@ function create_variables(obj) 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}}; + 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}; @@ -355,7 +355,6 @@ function generate_complementarity_constraints(obj) dcs = obj.dcs; model = obj.model; % Do Cross-Complementarity - % TODO(@anton) correctly handle differential rk representation. rbp = ~opts.right_boundary_point_explicit; From b51b459068a271375108d4a364b51cc91ffb1f1f Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 14:12:14 +0200 Subject: [PATCH 25/43] fixed bugs in alternative step_eq, and added lifting for c(x) --- +nosnoc/+dcs/Gcs.m | 20 +++- +nosnoc/+discrete_time_problem/Gcs.m | 103 +++++++++++++----- +nosnoc/Options.m | 6 + examples/new_design_examples/new_design_msv.m | 1 + examples/new_design_examples/new_design_pds.m | 2 + 5 files changed, 103 insertions(+), 29 deletions(-) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index 29dc27be..a781ba87 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -1,6 +1,7 @@ classdef Gcs < nosnoc.dcs.Base properties lambda + c_lift f_x @@ -10,6 +11,7 @@ nabla_c_fun c_fun g_comp_path_fun + g_c_lift_fun end methods @@ -23,8 +25,14 @@ function generate_variables(obj, opts) 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 @@ -34,6 +42,13 @@ function generate_equations(obj, opts) 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 + model.E*nabla_c*obj.lambda; @@ -42,7 +57,8 @@ function generate_equations(obj, opts) 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, model.z, model.v_global, model.p}, {model.c}); + 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}); diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 06323b46..5d61070c 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -90,15 +90,16 @@ function create_variables(obj) obj.w.x(0,0,opts.n_s) = {{['x_0'], dims.n_x}, model.x0, model.x0, model.x0}; obj.w.z(0,0,opts.n_s) = {{'z', dims.n_z}, model.lbz, model.ubz, model.z0}; obj.w.lambda(0,0,opts.n_s) = {{['lambda'], dims.n_lambda},0,inf}; + 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) + 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}; - + 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); @@ -127,10 +128,12 @@ function generate_direct_transcription_constraints(obj) % 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, z_0, v_global, [p_global;obj.p.p_time_var(1)]), 0, inf}; + 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 @@ -172,6 +175,7 @@ function generate_direct_transcription_constraints(obj) 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); @@ -182,7 +186,8 @@ function generate_direct_transcription_constraints(obj) 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, z_ijk, v_global, p), 0, inf}; + 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; @@ -233,7 +238,8 @@ function generate_direct_transcription_constraints(obj) 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, z_ijk, v_global, p), 0, inf}; + 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}; @@ -364,7 +370,8 @@ function generate_complementarity_constraints(obj) 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_prev = dcs.c_fun(x_prev,z_prev, v_global, [p_global;obj.p.p_time_var(1)]); + 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 @@ -378,7 +385,8 @@ function generate_complementarity_constraints(obj) for rr=1:(opts.n_s + rbp) x_ijr = obj.w.x(ii,jj,rr); z_ijr = obj.w.z(ii,jj,rr); - c_ijr = dcs.c_fun(x_ijr,z_ijr, v_global, p); + 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}); @@ -394,7 +402,8 @@ function generate_complementarity_constraints(obj) for rr=1:(opts.n_s + rbp) x_ijr = obj.w.x(ii,jj,rr); z_ijr = obj.w.z(ii,jj,rr); - c_ijr = dcs.c_fun(x_ijr,z_ijr, v_global, p); + 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}); @@ -405,7 +414,8 @@ function generate_complementarity_constraints(obj) 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_prev = dcs.c_fun(x_prev,z_prev, v_global, p); + 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 @@ -419,7 +429,8 @@ function generate_complementarity_constraints(obj) for kk=1:(opts.n_s + rbp) x_ijk = obj.w.x(ii,jj,kk); z_ijk = obj.w.z(ii,jj,kk); - c_ijk = dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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}); @@ -429,7 +440,8 @@ function generate_complementarity_constraints(obj) 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_prev = dcs.c_fun(x_prev,z_prev, v_global, p); + 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 @@ -441,7 +453,8 @@ function generate_complementarity_constraints(obj) for kk=1:(opts.n_s + rbp) x_ijk = obj.w.x(ii,jj,kk); z_ijk = obj.w.z(ii,jj,kk); - c_ijk = dcs.c_fun(x_ijk,z_ijk,v_global,p); + 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}; @@ -457,7 +470,8 @@ function generate_complementarity_constraints(obj) 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_prev = dcs.c_fun(x_prev,z_prev, v_global, p); + 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 @@ -470,7 +484,8 @@ function generate_complementarity_constraints(obj) for kk=1:(opts.n_s + rbp) x_ijk = obj.w.x(ii,jj,kk); z_ijk = obj.w.z(ii,jj,kk); - c_ijk = dcs.c_fun(x_ijk,z_ijk,v_global,p); + 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}; @@ -478,7 +493,8 @@ function generate_complementarity_constraints(obj) 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_prev = dcs.c_fun(x_prev,z_prev, v_global, p); + 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 @@ -495,7 +511,8 @@ function generate_complementarity_constraints(obj) 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_ijk = dcs.c_fun(x_ijk,z_ijk,v_global,p); + 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}; @@ -508,6 +525,7 @@ function generate_complementarity_constraints(obj) 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(); @@ -542,7 +560,11 @@ function generate_step_equilibration_constraints(obj) sigma_c_B = 0; sigma_lambda_B = 0; for kk=1:(opts.n_s + rbp) - sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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; @@ -550,8 +572,10 @@ function generate_step_equilibration_constraints(obj) 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 + dcs.c_fun(x_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 @@ -576,7 +600,11 @@ function generate_step_equilibration_constraints(obj) sigma_c_B = 0; sigma_lambda_B = 0; for kk=1:(opts.n_s + rbp) - sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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; @@ -584,8 +612,10 @@ function generate_step_equilibration_constraints(obj) 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 + dcs.c_fun(x_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 @@ -610,7 +640,11 @@ function generate_step_equilibration_constraints(obj) sigma_c_B = 0; sigma_lambda_B = 0; for kk=1:(opts.n_s + rbp) - sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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; @@ -618,8 +652,10 @@ function generate_step_equilibration_constraints(obj) 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 + dcs.c_fun(x_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 @@ -647,7 +683,11 @@ function generate_step_equilibration_constraints(obj) sigma_c_B = 0; sigma_lambda_B = 0; for kk=1:(opts.n_s + rbp) - sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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; @@ -655,8 +695,10 @@ function generate_step_equilibration_constraints(obj) 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 + dcs.c_fun(x_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 @@ -679,11 +721,16 @@ function generate_step_equilibration_constraints(obj) 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) - sigma_c_B = sigma_c_B + dcs.c_fun(x_ijk,z_ijk, v_global, p); + 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; @@ -691,8 +738,10 @@ function generate_step_equilibration_constraints(obj) 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 + dcs.c_fun(x_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 @@ -727,7 +776,7 @@ function generate_step_equilibration_constraints(obj) obj.g.nu_or(ii,jj) = {[nu-eta;sum(eta)-nu],0,inf}; % the actual step eq conditions - M=obj.p.T()/opts.N_stages; + 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]; diff --git a/+nosnoc/Options.m b/+nosnoc/Options.m index 05e4c2fa..9c040c9c 100644 --- a/+nosnoc/Options.m +++ b/+nosnoc/Options.m @@ -65,6 +65,12 @@ pss_lift_step_functions(1,1) logical = 0 n_depth_step_lifting(1,1) {mustBeInteger, mustBeGreaterThanOrEqual(n_depth_step_lifting, 2)} = 2 + % lift c in gcs + gcs_lift_gap_functions(1,1) logical = 0 + + % linear_complementarity M multiplier. for smaller step sizes + linear_complemtarity_M(1,1) double {mustBeReal, mustBePositive} = 1000 + %General NLP/OCP Settings g_path_at_fe(1,1) logical = 0 % evaluate nonlinear path constraint at every finte element boundary g_path_at_stg(1,1) logical = 0 % evaluate nonlinear path constraint at every stage diff --git a/examples/new_design_examples/new_design_msv.m b/examples/new_design_examples/new_design_msv.m index 644bae60..540ebebd 100644 --- a/examples/new_design_examples/new_design_msv.m +++ b/examples/new_design_examples/new_design_msv.m @@ -21,6 +21,7 @@ problem_options.rho_h = 1e-4; problem_options.time_optimal_problem = true; problem_options.use_fesd = true; +problem_options.gcs_lift_gap_functions = false; %solver_options.homotopy_steering_strategy = 'ELL_INF'; solver_options.complementarity_tol = 1e-10; solver_options.print_level = 3; diff --git a/examples/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m index 6570a42b..3c6eea99 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/new_design_examples/new_design_pds.m @@ -22,6 +22,8 @@ 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; From a752f3b6340f2b83940fd3739d4c854097658474 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 14:54:37 +0200 Subject: [PATCH 26/43] rename f_x to f_x_unconstrained, minor changes to printing etc --- +nosnoc/+dcs/Gcs.m | 21 +++++++++++++------ +nosnoc/+model/Pds.m | 6 +++--- .../new_design_moving_set.m | 2 +- examples/new_design_examples/new_design_msv.m | 2 +- examples/new_design_examples/new_design_pds.m | 2 +- 5 files changed, 21 insertions(+), 12 deletions(-) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index a781ba87..b0fd3ece 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -7,10 +7,9 @@ dims - f_fun + f_unconstrained_fun nabla_c_fun c_fun - g_comp_path_fun g_c_lift_fun end @@ -50,10 +49,10 @@ function generate_equations(obj, opts) c = model.c; end - obj.f_x = model.f_x + model.E*nabla_c*obj.lambda; + 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, model.f_q}); - obj.f_fun = Function('f', {model.x, model.z, model.u, model.v_global, model.p}, {model.f_x}); + 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}, {[]}); @@ -76,7 +75,17 @@ function generate_equations(obj, opts) propgrp = getPropertyGroups@nosnoc.dcs.Base(obj); group_title = 'Variables'; var_list = struct; - var_list.lambda; + 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 diff --git a/+nosnoc/+model/Pds.m b/+nosnoc/+model/Pds.m index 6ed120fd..1d0c0c44 100644 --- a/+nosnoc/+model/Pds.m +++ b/+nosnoc/+model/Pds.m @@ -1,6 +1,6 @@ classdef Pds < nosnoc.model.Base properties - f_x + f_x_unconstrained c E end @@ -20,8 +20,8 @@ function verify_and_backfill(obj, opts) dims = obj.dims; - if size(obj.f_x,1) ~= dims.n_x - error("nosnoc: f_x has incorrect dimension. It must have the same dimension as x.") + if size(obj.f_x_unconstrained,1) ~= dims.n_x + error("nosnoc: f_x_unconstrained has incorrect dimension. It must have the same dimension as x.") end dims.n_c = size(obj.c,1); diff --git a/examples/new_design_examples/new_design_moving_set.m b/examples/new_design_examples/new_design_moving_set.m index f4e84943..74e50c3f 100644 --- a/examples/new_design_examples/new_design_moving_set.m +++ b/examples/new_design_examples/new_design_moving_set.m @@ -29,7 +29,7 @@ t = SX.sym('t'); model.x = [x;t]; model.c = [-norm(x - [sin(t);cos(t)])^2+(1-0.5)^2]; -model.f_x = [0; 0; 1]; +model.f_x_unconstrained = [0; 0; 1]; model.E = diag([1,1,0]); integrator = nosnoc.Integrator(model, problem_options, solver_options); diff --git a/examples/new_design_examples/new_design_msv.m b/examples/new_design_examples/new_design_msv.m index 540ebebd..e6eb1b7f 100644 --- a/examples/new_design_examples/new_design_msv.m +++ b/examples/new_design_examples/new_design_msv.m @@ -45,7 +45,7 @@ %model.ubu = [100/sqrt(2);100/sqrt(2);0;0]; model.u0 = model.ubu; model.c = [norm_2(x2-x1)-2*R]; -model.f_x = [u1;u2]; +model.f_x_unconstrained = [u1;u2]; % costs model.f_q = 0; diff --git a/examples/new_design_examples/new_design_pds.m b/examples/new_design_examples/new_design_pds.m index 3c6eea99..5c8df341 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/new_design_examples/new_design_pds.m @@ -34,7 +34,7 @@ x = SX.sym('x',2); model.x = x; model.c = x(2)+0.2; -model.f_x = [x(2);-x(1)]; +model.f_x_unconstrained = [x(2);-x(1)]; model.x0 = [0;pi-2]; From 69ff75ba207ba878fc7f0a20359cbda037671443 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 15:00:39 +0200 Subject: [PATCH 27/43] also do renames in stewart/heaviside --- +nosnoc/+discrete_time_problem/Heaviside.m | 72 +++++++++++----------- +nosnoc/+discrete_time_problem/Stewart.m | 72 +++++++++++----------- 2 files changed, 72 insertions(+), 72 deletions(-) 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 Date: Mon, 1 Jul 2024 15:22:40 +0200 Subject: [PATCH 28/43] some comments and update sphinx conf --- +nosnoc/+model/Pds.m | 8 ++++---- docs/source/conf.py | 3 ++- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/+nosnoc/+model/Pds.m b/+nosnoc/+model/Pds.m index 1d0c0c44..73414e79 100644 --- a/+nosnoc/+model/Pds.m +++ b/+nosnoc/+model/Pds.m @@ -1,8 +1,8 @@ classdef Pds < nosnoc.model.Base properties - f_x_unconstrained - c - E + f_x_unconstrained % casadi.SX or casadi.MX: Unconstrained state dynamics + c % casadi.SX or casadi.MX: Function used to define the feasible set $C = \{x | c(x) \ge 0\}$ + E % double: Projection matrix which can be used to end methods @@ -34,7 +34,7 @@ function verify_and_backfill(obj, opts) error("nosnoc: Projection matrix E must be an n_x by n_x matrix where n_x is the number of functions defining the set C.") end else - obj.E = eye(dims.n_c); + obj.E = eye(dims.n_x); end obj.dims = dims; diff --git a/docs/source/conf.py b/docs/source/conf.py index b0f9a0ca..8181335f 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -6,7 +6,7 @@ project = 'nosnoc' copyright = '2024, Armin Nurkanovic, Jonathan Frey, Anton Pozharskiy, Moritz Diehl' -author = 'Anton Pozharskiy' +author = 'Armin Nurkanovic' release = '0.5' version = '0.5.0' @@ -19,6 +19,7 @@ 'sphinx.ext.autodoc', 'sphinx.ext.autosummary', 'sphinx.ext.intersphinx', + 'sphinx.ext.napoleon', 'sphinxcontrib.matlab', ] From 75106b5e50d7449b4bf5b47624d10adb20432d82 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 15:33:28 +0200 Subject: [PATCH 29/43] some more docs --- +nosnoc/+dcs/Gcs.m | 16 ++++++++-------- +nosnoc/+model/Pds.m | 6 +++--- 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index b0fd3ece..8996637a 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -1,16 +1,16 @@ classdef Gcs < nosnoc.dcs.Base properties - lambda - c_lift + lambda %casadi.SX or casadi.MX: Lagrange multipliers for the gradient complementarity system. + c_lift %casadi.SX or casadi.MX: Lift variables for the gap functions in :class:`nosnoc.model.Pds`. - f_x + f_x % casadi.SX or casadi.MX: Right hand side of the gradient complementarity system - dims + dims % struct: dimensions struct. - f_unconstrained_fun - nabla_c_fun - c_fun - g_c_lift_fun + 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 diff --git a/+nosnoc/+model/Pds.m b/+nosnoc/+model/Pds.m index 73414e79..522419e7 100644 --- a/+nosnoc/+model/Pds.m +++ b/+nosnoc/+model/Pds.m @@ -1,8 +1,8 @@ classdef Pds < nosnoc.model.Base properties - f_x_unconstrained % casadi.SX or casadi.MX: Unconstrained state dynamics - c % casadi.SX or casadi.MX: Function used to define the feasible set $C = \{x | c(x) \ge 0\}$ - E % double: Projection matrix which can be used to + f_x_unconstrained % casadi.SX or casadi.MX: Unconstrained state dynamics. + c % casadi.SX or casadi.MX: Function used to define the feasible set $C = \{x | c(x) \ge 0\}$. + E % double: (EXPERIMENTAL) Projection matrix which can be used to adjust the projection used for the PDS. end methods From bd6b411754ced7a6346d4c1cd57923c5266ea92b Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 15:36:22 +0200 Subject: [PATCH 30/43] docs test --- docs/source/api.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/docs/source/api.rst b/docs/source/api.rst index 1e181fac..697c5b1c 100644 --- a/docs/source/api.rst +++ b/docs/source/api.rst @@ -1,3 +1,7 @@ API === +.. automodule:: +nosnoc/+model +.. autoclass:: Pds + :show-inheritance: + :members: From 91cfa780616b7c494d62602b5492450d2a7c1209 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Mon, 1 Jul 2024 17:49:26 +0200 Subject: [PATCH 31/43] three hours of fighting with sphinx --- docs/Makefile | 2 +- docs/requirements.txt | 3 ++- docs/source/api.rst | 4 +--- docs/source/conf.py | 28 +++++++++++++++++++++++----- 4 files changed, 27 insertions(+), 10 deletions(-) diff --git a/docs/Makefile b/docs/Makefile index d0c3cbf1..f72dce2c 100644 --- a/docs/Makefile +++ b/docs/Makefile @@ -3,7 +3,7 @@ # You can set these variables from the command line, and also # from the environment for the first two. -SPHINXOPTS ?= +SPHINXOPTS ?= -vvv -N SPHINXBUILD ?= sphinx-build SOURCEDIR = source BUILDDIR = build diff --git a/docs/requirements.txt b/docs/requirements.txt index 3d9bc03f..e7674a44 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,3 +1,4 @@ -sphinx==7.1.2 +sphinx==7.3.7 sphinx-rtd-theme==1.3.0rc1 sphinxcontrib-matlabdomain==0.21.5 +sphinx-math-dollar diff --git a/docs/source/api.rst b/docs/source/api.rst index 697c5b1c..ac439cdb 100644 --- a/docs/source/api.rst +++ b/docs/source/api.rst @@ -1,7 +1,5 @@ API === -.. automodule:: +nosnoc/+model +.. module:: +nosnoc.+model .. autoclass:: Pds - :show-inheritance: - :members: diff --git a/docs/source/conf.py b/docs/source/conf.py index 8181335f..fccc6103 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -13,14 +13,15 @@ # -- General configuration + extensions = [ - 'sphinx.ext.duration', - 'sphinx.ext.doctest', 'sphinx.ext.autodoc', 'sphinx.ext.autosummary', 'sphinx.ext.intersphinx', - 'sphinx.ext.napoleon', 'sphinxcontrib.matlab', + 'sphinx.ext.napoleon', + 'sphinx.ext.mathjax', + 'sphinx_math_dollar', ] intersphinx_mapping = { @@ -39,7 +40,24 @@ epub_show_urls = 'footnote' matlab_src_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -matlab_short_links = True -matlab_auto_link = "basic" +matlab_keep_package_prefix = False primary_domain = "mat" +default_role = "obj" autodoc_member_order = 'bysource' + +napoleon_numpy_docstring = False +napoleon_preprocess_types = True +napoleon_use_param = False +napoleon_attr_annotations = False + +autodoc_default_options = {'members': True, 'show-inheritance': True} +autosummary_generate = True + +# -- magic to kill mathjax + +mathjax3_config = { + "tex": { + "inlineMath": [['\\(', '\\)']], + "displayMath": [["\\[", "\\]"]], + } +} From 3cbc03b1da5d83b9bd2c621b779e23c9177bd403 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 10:28:52 +0200 Subject: [PATCH 32/43] A pretty decent initial pass at documentation --- +nosnoc/+model/Base.m | 121 +++++++++++++++++++---------------------- +nosnoc/+model/Pds.m | 12 +++- .gitignore | 3 +- docs/Makefile | 2 +- docs/requirements.txt | 2 +- docs/source/api.rst | 5 -- docs/source/conf.py | 9 +-- docs/source/index.rst | 2 +- docs/source/models.rst | 7 +++ 9 files changed, 80 insertions(+), 83 deletions(-) delete mode 100644 docs/source/api.rst create mode 100644 docs/source/models.rst diff --git a/+nosnoc/+model/Base.m b/+nosnoc/+model/Base.m index 0f135e1e..01712576 100644 --- a/+nosnoc/+model/Base.m +++ b/+nosnoc/+model/Base.m @@ -1,74 +1,67 @@ classdef Base < matlab.mixin.Scalar & handle & matlab.mixin.CustomDisplay +% Base class for all nosnoc models. It contains shared properties such as the state, user algebraics, controls etc. +% It also contains the fields used to populate an Optimal Control problem such as Lagrange and Mayer cost terms, +% an interface for least squares costs as well as non-box path and terminal constraints. properties - % Differential state - x % CasADi symbolic variable - lbx % Double vector - ubx % Double vector - x0 % Double vector - - % user algebraics - z % CasADi symbolic variable - z0 % Double vector - lbz % Double vector - ubz % Double vector - g_z % user algebraic constraints - CasADi symbolic expression - - % Controls - u % CasADi symbolic variable - lbu % Double vector - ubu % Double vector - u0 % Double vector - - % global variables (not time dependent) - v_global % CasADi symbolic variable - v0_global % Double vector - lbv_global % Double vector - ubv_global % Double vector + x % casadi.SX|casadi.MX: Differential state $x \in \mathbb{R}^{n_x}$ + lbx % double: $\underline{x} \in \mathbb{R}^{n_x}$, differential state lower bound. + ubx % double: $\bar{x} \in \mathbb{R}^{n_x}$, differential state upper bound. + x0 % double: $x_0 \in \mathbb{R}^{n_x}$, initial differential state, also used to initialize all differential state variables in the resulting MPCC. + + z % casadi.SX|casadi.MX: User algebraics $z \in \mathbb{R}^{n_z}$ + z0 % double: $z_0 \in \mathbb{R}^{n_z}$, used to initialize all user algebraic variables in the resulting MPCC. + lbz % double: $\underline{z} \in \mathbb{R}^{n_z}$, user algebraic lower bound. + ubz % double: $\bar{z} \in \mathbb{R}^{n_z}$, user algebraic upper bound. + g_z % casadi.SX|casadi.MX: Constraint expression used to define the behavior of user algebraics. + + u % casadi.SX|casadi.MX: Controls $u \in \mathbb{R}^{n_u}$. + lbu % double: $\underline{u} \in \mathbb{R}^{n_u}$, controls lower bound. + ubu % double: $\bar{u} \in \mathbb{R}^{n_u}$, controls upper bound. + u0 % double: $u_0 \in \mathbb{R}^{n_u}$, used to initialize all control variables in the resulting MPCC. + + v_global % casadi.SX|casadi.MX: $\nu \in \mathbb{R}^{n_{\nu}}$ global variables (not time dependent). + v0_global % double: $\nu_0 \in \mathbb{R}^{n_{\nu}}$, used to initialize all global variables in the resulting MPCC. + lbv_global % double: $\underline{\nu} \in \mathbb{R}^{n_{\nu}}$, global variables lower bound. + ubv_global % double: $\bar{\nu} \in \mathbb{R}^{n_{\nu}}$, global variables upper bound. - % global parameters (not time dependent) - p_global % CasADi symbolic variable - p_global_val + p_global % casadi.SX|casadi.MX: Global parameters. + p_global_val % double: Values for global parameters - % time varying parameters - p_time_var % CasADi symbolic variable - p_time_var_val - % all params - p % CasADi symbolic variable (TODO: Also algoritmic parameters??) + p_time_var % casadi.SX|casadi.MX: Time varying parameters which are considered to be constant over each control/integration interval. + p_time_var_val % double: Values for time varying parameters. + p % casadi.SX|casadi.MX: All model parameters - % Objective - f_q % CasADi symbolic expression - f_q_T % CasADi symbolic expression + f_q % casadi.SX|casadi.MX: Lagrange term cost. + f_q_T % casadi.SX|casadi.MX: Mayer term cost. % least squares (TODO @Anton: more precise description, e.g. where are the weight matrices) - lsq_x % TODO Described - x_ref % Subset of x? - f_lsq_x % CasADi symbolic expression - x_ref_val % Double vector - lsq_u % CasADi symbolic expression - u_ref % Subset of u? - f_lsq_u % CasADi symbolic expression - u_ref_val % Double vector - lsq_T % CasADi symbolic expression - x_ref_end % Subset of x? - f_lsq_T % CasADi symbolic expression - x_ref_end_val % Double vector - - % Path constraints - g_path % CasADi symbolic expression - lbg_path % Double vector - ubg_path % Double vector - - % Terminal constraints - g_terminal % CasADi symbolic expression - lbg_terminal % Double vector - ubg_terminal % Double vector - - % Path Complementarities - G_path % CasADi symbolic expression - H_path % CasADi symbolic expression - - % Dimensions - dims + % TODO(@anton) perhaps the calculated parts of the lsq interface should be read only + + lsq_x % cell: TODO describe + x_ref % casadi.SX|casadi.MX: + f_lsq_x % casadi.SX|casadi.MX: + x_ref_val % double: + lsq_u % casadi.SX|casadi.MX: + u_ref % casadi.SX|casadi.MX: + f_lsq_u % casadi.SX|casadi.MX: + u_ref_val % double: vector + lsq_T % casadi.SX|casadi.MX: + x_ref_end % casadi.SX|casadi.MX: + f_lsq_T % casadi.SX|casadi.MX: + x_ref_end_val % double: vector + + g_path % casadi.SX|casadi.MX: Path constraints. + lbg_path % double: Lower bound on path constraints. + ubg_path % double: Upper bound on path constraints. + + g_terminal % casadi.SX|casadi.MX: Terminal constraints. + lbg_terminal % double: Lower bound on path constraints. + ubg_terminal % double: Upper bound on path constraints. + + G_path % casadi.SX|casadi.MX: One half of path complementarities. + H_path % casadi.SX|casadi.MX: One half of path complementarities. + + dims % struct: Dimensions struct, the contents of which depends on the subclass. end methods diff --git a/+nosnoc/+model/Pds.m b/+nosnoc/+model/Pds.m index 522419e7..6e0058fe 100644 --- a/+nosnoc/+model/Pds.m +++ b/+nosnoc/+model/Pds.m @@ -1,8 +1,14 @@ classdef Pds < nosnoc.model.Base +% A model that represents a Projected dynamical system. Which has the dynamics: +% $$\dot{x} = \operatorname{P}_{\mathcal{T}_{\mathcal{C}(x)}}(f(x))$$ +% with $\operatorname{P}_K(v) = \operatorname{arg\, min}_{s\in K} \frac{1}{2}||s-v||^2_2$, +% and the feasible set set $\mathcal{C} = \{x | c(x) \ge 0\}$. properties - f_x_unconstrained % casadi.SX or casadi.MX: Unconstrained state dynamics. - c % casadi.SX or casadi.MX: Function used to define the feasible set $C = \{x | c(x) \ge 0\}$. - E % double: (EXPERIMENTAL) Projection matrix which can be used to adjust the projection used for the PDS. + f_x_unconstrained % casadi.SX|casadi.MX: Unconstrained system dynamics expression $f(x)$. + + c % casadi.SX|casadi.MX: The gap functions $c(x)$ used in the definition of the feasible set $\mathcal{C}$. + + E % double: Square matrix $E \in \mathbb{R}^{n_x\times n_x}$ that is used as the weighting matrix for the projection operator. end methods diff --git a/.gitignore b/.gitignore index 056249f0..c9f4a2c8 100644 --- a/.gitignore +++ b/.gitignore @@ -39,4 +39,5 @@ qt_temp* *.mat *~ -examples/*/*.pdf \ No newline at end of file +examples/*/*.pdf +docs/build \ No newline at end of file diff --git a/docs/Makefile b/docs/Makefile index f72dce2c..2ffd96f5 100644 --- a/docs/Makefile +++ b/docs/Makefile @@ -3,7 +3,7 @@ # You can set these variables from the command line, and also # from the environment for the first two. -SPHINXOPTS ?= -vvv -N +SPHINXOPTS ?= SPHINXBUILD ?= sphinx-build SOURCEDIR = source BUILDDIR = build diff --git a/docs/requirements.txt b/docs/requirements.txt index e7674a44..9200eb14 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,4 +1,4 @@ sphinx==7.3.7 sphinx-rtd-theme==1.3.0rc1 -sphinxcontrib-matlabdomain==0.21.5 +sphinxcontrib-matlabdomain @ git+https://github.com/apozharski/matlabdomain@master sphinx-math-dollar diff --git a/docs/source/api.rst b/docs/source/api.rst deleted file mode 100644 index ac439cdb..00000000 --- a/docs/source/api.rst +++ /dev/null @@ -1,5 +0,0 @@ -API -=== - -.. module:: +nosnoc.+model -.. autoclass:: Pds diff --git a/docs/source/conf.py b/docs/source/conf.py index fccc6103..ffe66312 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -15,11 +15,11 @@ extensions = [ + 'sphinx.ext.napoleon', 'sphinx.ext.autodoc', 'sphinx.ext.autosummary', 'sphinx.ext.intersphinx', 'sphinxcontrib.matlab', - 'sphinx.ext.napoleon', 'sphinx.ext.mathjax', 'sphinx_math_dollar', ] @@ -45,15 +45,10 @@ default_role = "obj" autodoc_member_order = 'bysource' -napoleon_numpy_docstring = False -napoleon_preprocess_types = True -napoleon_use_param = False -napoleon_attr_annotations = False - autodoc_default_options = {'members': True, 'show-inheritance': True} autosummary_generate = True -# -- magic to kill mathjax +# -- magic to prevent mathjax from overriding sphinx_math_dollar. mathjax3_config = { "tex": { diff --git a/docs/source/index.rst b/docs/source/index.rst index ac1eec87..8960b956 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -14,4 +14,4 @@ Contents .. toctree:: usage - api + models diff --git a/docs/source/models.rst b/docs/source/models.rst new file mode 100644 index 00000000..bd8f4c80 --- /dev/null +++ b/docs/source/models.rst @@ -0,0 +1,7 @@ +Models +====== +All `nosnoc` models are derived from :class:`+nosnoc.+model.Base` and share its functionality. + +.. module:: +nosnoc.+model +.. autoclass:: Base +.. autoclass:: Pds From a6bdf4671ae4cc01b3c528e179d918bf85c119f9 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 13:57:54 +0200 Subject: [PATCH 33/43] A lot of options documentation :) --- +nosnoc/+model/Pds.m | 2 +- +nosnoc/Options.m | 258 ++++++++++++++++++++++++---------------- docs/source/index.rst | 1 + docs/source/options.rst | 4 + 4 files changed, 161 insertions(+), 104 deletions(-) create mode 100644 docs/source/options.rst diff --git a/+nosnoc/+model/Pds.m b/+nosnoc/+model/Pds.m index 6e0058fe..8dcdc0b6 100644 --- a/+nosnoc/+model/Pds.m +++ b/+nosnoc/+model/Pds.m @@ -8,7 +8,7 @@ c % casadi.SX|casadi.MX: The gap functions $c(x)$ used in the definition of the feasible set $\mathcal{C}$. - E % double: Square matrix $E \in \mathbb{R}^{n_x\times n_x}$ that is used as the weighting matrix for the projection operator. + E % double: Square matrix $E \in \mathbb{R}^{n_x\times n_x}$ that is used as the weighting matrix for the projection operator. By default this is the identity matrix. end methods diff --git a/+nosnoc/Options.m b/+nosnoc/Options.m index 9c040c9c..096522a4 100644 --- a/+nosnoc/Options.m +++ b/+nosnoc/Options.m @@ -25,134 +25,186 @@ % This file is part of NOSNOC. classdef Options < handle +% The main options class for `nosnoc`. It contains the options for simulation and optimal control reformulations but *does not* +% contain settings for the MPCC solver. properties - % General - use_fesd(1,1) logical = 1 - casadi_symbolic_mode {mustBeMember(casadi_symbolic_mode,{'casadi.SX', 'casadi.MX'})} = 'casadi.SX' + % boolean: If true the FESD discretization is used, otherwise a direct time-stepping discretization is used + use_fesd(1,1) logical = 1; + + % string: Which casadi symbolics to use. Can either be `'casadi.SX'` or `'casadi.MX'.` + casadi_symbolic_mode {mustBeMember(casadi_symbolic_mode,{'casadi.SX', 'casadi.MX'})} = 'casadi.SX'; - % In case of simulation problem T_sim N_sim h_sim - h % Step size - h_k % Finite element step size - T % Terminal time % TODO: why is there also T_val, do we need both? + h % double: Step size + h_k % double: Finite element step size + T % double: Terminal time TODO: why is there also T_val, do we need both? + + N_stages(1,1) {mustBeInteger, mustBePositive} = 1; % int: Number of control stages. - % descritization - N_stages(1,1) {mustBeInteger, mustBePositive} = 1; + % int: Number of finite elements in each control stage. This can either be a scalar value + % in which case it is transformed into a vector for that value when :meth:`preprocess` is called. + % Alternatively you can pass a vector of size :attr:`N_stages`. N_finite_elements {mustBeInteger, mustBePositive} = 2; - % IRK and FESD Settings - n_s(1,1) {mustBeInteger} = 2 + n_s(1,1) {mustBeInteger} = 2 % int: Number of Stages in the Runge-Kutta scheme. + + % RKSchemes: Which Runge-Kutta scheme family to use. + % + % See Also: + % `RKSchemes` for more details as to the how to choose a Runge-Kutta Scheme and + % for differences between them. rk_scheme(1,1) RKSchemes = RKSchemes.RADAU_IIA + + % RKRepresentation: Which representation of Runge-Kutta discretization to use. + % + % See Also: + % `RKRepresentation` for a description of the representations. rk_representation RKRepresentation = RKRepresentation.integral; + % CrossCompMode: Which cross complementarity mode to use. + % + % See Also: + % `CrossCompMode` for a description of the representations. cross_comp_mode(1,1) CrossCompMode = CrossCompMode.FE_STAGE + + % double: Fraction in the range $\gamma_h \in [0,1]$ by which the step size is relaxed: + % $$(1-\gamma_h) h_0\le h \le (1+\gamma_h) h_0$$ gamma_h(1,1) double {mustBeReal} = 1 - dcs_mode DcsMode = DcsMode.Stewart - - % lift complementarities - lift_complementarities(1,1) logical = 0 - lower_bound_comp_lift(1,1) logical = 0 - - % Initialization - Step - initial_alpha(1,1) double {mustBeReal, mustBeFinite} = 1 - initial_beta(1,1) double {mustBeReal, mustBeFinite} = 1 - initial_theta_step(1,1) double {mustBeReal, mustBeFinite} = 1 - - % Step theta lifting - pss_lift_step_functions(1,1) logical = 0 - n_depth_step_lifting(1,1) {mustBeInteger, mustBeGreaterThanOrEqual(n_depth_step_lifting, 2)} = 2 - - % lift c in gcs - gcs_lift_gap_functions(1,1) logical = 0 - - % linear_complementarity M multiplier. for smaller step sizes - linear_complemtarity_M(1,1) double {mustBeReal, mustBePositive} = 1000 - - %General NLP/OCP Settings - g_path_at_fe(1,1) logical = 0 % evaluate nonlinear path constraint at every finte element boundary - g_path_at_stg(1,1) logical = 0 % evaluate nonlinear path constraint at every stage - - x_box_at_fe(1,1) logical = 1 % evaluate box constraint for diff states at every finite element boundary point - x_box_at_stg(1,1) logical = 1 % evaluate box constraint for diff states at every stage point. (is set to zero per default in differential irk mode, as it becomes a linear instead of box constraint) - time_optimal_problem(1,1) = 0 - - % Step equilibration - rho_h(1,1) double {mustBeNonnegative} = 1 - step_equilibration(1,1) StepEquilibrationMode = StepEquilibrationMode.heuristic_mean % heuristic_mean, l2_relaxed, l2_relaxed_scaled, direct, direct_homotopy, direct_homotopy_lift - step_equilibration_sigma(1,1) double {mustBePositive} = 0.1 % slope at zero in rescaling the indicator function, nu_ki_rescaled = tanh(nu_ki/step_equilibration_sigma) - - % Multiple shooting type discretization - equidistant_control_grid(1,1) logical = 1 - - % Time-Setting & Time-Freezing - time_freezing(1,1) logical = 0 - time_freezing_inelastic(1,1) logical = 0 - % for time optimal problems and equidistant control grids in physical time - use_speed_of_time_variables(1,1) logical = 0 - local_speed_of_time_variable(1,1) logical = 0 - stagewise_clock_constraint(1,1) logical = 1 - impose_terminal_phyisical_time(1,1) logical = 1 - s_sot0(1,1) double {mustBePositive} = 1 - s_sot_max(1,1) double {mustBePositive} = 25 - s_sot_min(1,1) double {mustBePositive} = 1 - S_sot_nominal(1,1) double {mustBePositive} = 1 - rho_sot(1,1) double {mustBeReal, mustBeNonnegative} = 0 - - T_final_max(1,1) double {mustBePositive} = 1e2 - T_final_min(1,1) double {mustBeReal, mustBeNonnegative} = 0 - time_freezing_reduced_model(1,1) logical = 0 % analytic reduction of lifter formulation, less algebraic variables (experimental) - time_freezing_hysteresis(1,1) logical = 0 % do not do automatic time freezing generation for hysteresis, it is not supported yet. - time_freezing_nonlinear_friction_cone(1,1) logical = 1 % 1 - use nonlienar friction cone, 0 - use polyhedral l_inf approximation. - - time_freezing_quadrature_state(1,1) logical = 0 % make a nonsmooth quadrature state to integrate only if physical time is running - time_freezing_lift_forces(1,1) logical = 0 % replace \dot{v} = M(q)^{-1}f(q,v,u) by dot{v} = z, M(q)z - f(q,v,u) = 0 - - % exerimental expert options - nonsmooth_switching_fun(1,1) logical = 0 % experimental: use c = max(c1,c2) insetad of c = [c1c2] - % expert mode: stabilize auxiliary dynamics in \nabla f_c(q) direction - stabilizing_q_dynamics(1,1) logical = 0 - kappa_stabilizing_q_dynamics(1,1) double {mustBePositive} = 1e-5 - % Verbose - print_level = 3 + dcs_mode DcsMode = DcsMode.Stewart % DcsMode: Which DCS to reformulate the problem into. + + lift_complementarities(1,1) logical = 0 % boolean: Whether complementarities are lifted. TODO(@anton) should this still live in MPCC generation? + lower_bound_comp_lift(1,1) logical = 0 % boolean: If true we add additional lower bounds to the lifted variables. + + initial_alpha(1,1) double {mustBeReal, mustBeFinite} = 1 % double: Initial value for $\alpha$ in the Heaviside step reformulation. + initial_beta(1,1) double {mustBeReal, mustBeFinite} = 1 % double: Initial value for $\beta$ when lifting is enabled in the Heaviside step reformulation. + initial_theta_step(1,1) double {mustBeReal, mustBeFinite} = 1 % double: Initial value for $\theta$ when lifting is enabled in the Heaviside step reformulation. + + % boolean: If true then the convex multiplier expressions are lifted in the Heaviside step reformulation. + % + % Warning: + % This is not currently implemented for generic Heaviside step DCS. + pss_lift_step_functions(1,1) logical = 0 + n_depth_step_lifting(1,1) {mustBeInteger, mustBeGreaterThanOrEqual(n_depth_step_lifting, 2)} = 2 % int: Depth to which the Heaviside step convex multipliers are lifted. + + gcs_lift_gap_functions(1,1) logical = 1 % boolean: If true the step functions $c(x)$ are lifted in the gradient complementarity system reformulation. - % Settings specific to CLS - friction_model (1,1) FrictionModel = FrictionModel.Conic; % use nonlinear friction ('Conic') or polyhedral approximation ('Polyhedral'); - conic_model_switch_handling (1,1) ConicModelSwitchHandling = ConicModelSwitchHandling.Abs; % How to treat switch detection in v_t. - kappa_friction_reg (1,1) double {mustBeReal, mustBeNonnegative} = 0; % reg. term in friction equations to avoid large multipliers if no contact happens. - lift_velocity_state(1,1) logical = 0; % define auxliary algebraic vairable, dot{v} = z_v, to avoid symbolic inversion of the inertia matrix; - eps_cls double = 1e-3 % constraint: enforce f_c at Euler step with h * eps_cls + linear_complemtarity_M(1,1) double {mustBeReal, mustBePositive} = 1000 % double: $M$ multiplier used in the linear complementarity step equilibration fromulation. Larger values alleviate infeasibility for smaller step sizes. + g_path_at_fe(1,1) logical = 0 % boolean: If true we evaluate nonlinear path constraint at every finte element boundary. + g_path_at_stg(1,1) logical = 0 % boolean: If true evaluate nonlinear path constraint at every stage. - % Relaxation of terminal constraint - relax_terminal_constraint(1,1) ConstraintRelaxationMode = ConstraintRelaxationMode.NONE; % If/how to relax the - relax_terminal_constraint_from_above(1,1) logical = 0; - rho_terminal(1,1) double {mustBePositive} = 1e2; - relax_terminal_constraint_homotopy(1,1) logical = 0; % terminal penalty is governed by homotopy parameter + x_box_at_fe(1,1) logical = 1 % boolean: If true we evaluate box constraint for diff states at every finite element boundary point. - % Relaxation of terminal (or stage for equidistant grids) numerical/phyisical time constraints - relax_terminal_numerical_time(1,1) logical = 0; % instead of imposing \sum_h = T, add it as ell1_penalty term - rho_terminal_numerical_time(1,1) double {mustBeNonnegative} = 1e2 - relax_terminal_numerical_time_homotopy (1,1) logical = 0; % us the homotopy parameter for the penalty - relax_terminal_physical_time(1,1) logical = 0; % instead of imposing \sum_h = T, add it as ell1_penalty term - rho_terminal_physical_time(1,1) double {mustBeNonnegative} = 1e2 - relax_terminal_physical_time_homotopy (1,1) logical = 0; % us the homotopy parameter for the penalty + % boolean: If true we evaluate box constraint for diff states at every stage point. + % + % Note: + % This is set to zero per default in differential rk mode, as it becomes a linear instead of box constraint. + x_box_at_stg(1,1) logical = 1 + time_optimal_problem(1,1) = 0 % boolean: If true for an OCP we automatically reformulate the problem to be time optimal. - cost_integration(1,1) logical = 1 + rho_h(1,1) double {mustBeNonnegative} = 1 % double: Weight used in heuristic or relaxed step equilibration modes. - % Experimental: - no_initial_impacts(1,1) logical = 0 + % StepEquilibrationMode: Which step equilibration mode to use. + % + % See Also: + % `StepEquilibrationMode` for more details on how each mode works. + step_equilibration(1,1) StepEquilibrationMode = StepEquilibrationMode.heuristic_mean + step_equilibration_sigma(1,1) double {mustBePositive} = 0.1 % double: Slope at zero for the sigmoid used to rescale the indicator function, nu_ki_rescaled = tanh(nu_ki/step_equilibration_sigma). + + equidistant_control_grid(1,1) logical = 1 % boolean: If true each control stage is fixed length. + + time_freezing(1,1) logical = 0 % boolean: Use a time freezing reformulation for the given model. + time_freezing_inelastic(1,1) logical = 0 % boolean: Use the specailized time freezing reformulation for systems with inelastic collisions and friction. + + use_speed_of_time_variables(1,1) logical = 0 % boolean: If true speed of time variables are used for the time freezing reformulation or time optimal problem + local_speed_of_time_variable(1,1) logical = 0 % boolean: If true then each control stage has a speed of time variable. Otherwise a single speed of time variable is used. + stagewise_clock_constraint(1,1) logical = 1 % boolean: If true the control grid is fixed with constraints for each control stage. + impose_terminal_phyisical_time(1,1) logical = 1 % boolean: If true the terminal physical time in a time freezing system is constrained to be exactly the desired horizon length. + s_sot0(1,1) double {mustBePositive} = 1 % double: Initial value for speed of time variables. + s_sot_max(1,1) double {mustBePositive} = 25 % double: Maximum for speed of time variables. + s_sot_min(1,1) double {mustBePositive} = 1 % double: Minimum for speed of time variables. + S_sot_nominal(1,1) double {mustBePositive} = 1 % double: Nominal speed of time used for regularizing the speed of time variables. + rho_sot(1,1) double {mustBeReal, mustBeNonnegative} = 0 % double: Weight used for the speed of time regularization. + + T_final_max(1,1) double {mustBePositive} = 1e2 % double: Maximum final time for a time optimal problem. + T_final_min(1,1) double {mustBeReal, mustBeNonnegative} = 0 % double: Minimum final time for a time optimal problem. + + + time_freezing_reduced_model(1,1) logical = 0 % boolean: Analytic reduction of lifter formulation, less algebraic variables (experimental). TODO(@armin) What was this supposed to be? + time_freezing_hysteresis(1,1) logical = 0 + time_freezing_nonlinear_friction_cone(1,1) logical = 1 % boolean: If true we use the nonlinear friction cone, otherwise use polyhedral l_inf approximation. + + time_freezing_quadrature_state(1,1) logical = 0 % boolean: If true make a nonsmooth quadrature state to integrate only if physical time is running. + time_freezing_lift_forces(1,1) logical = 0 % If true replace $\dot{v} = M(q)^{-1}f(q,v,u)$ by $dot{v} = z, M(q)z - f(q,v,u) = 0$. + + nonsmooth_switching_fun(1,1) logical = 0 % boolean: Experimental, use $c = \max(c1,c2)$ insetad of $c = c_1c_2$. + stabilizing_q_dynamics(1,1) logical = 0 % boolean: Experimental, stabilize auxiliary dynamics in \nabla f_c(q) direction. + kappa_stabilizing_q_dynamics(1,1) double {mustBePositive} = 1e-5 % double: Constant used for stabilizing auxiliary dynamics in \nabla f_c(q) direction. + + % int: Level of verbosity that the `nosnoc` reformulator uses. + % TODO: + % @anton, @armin document this better. + print_level = 3 - % Integrator - use_previous_solution_as_initial_guess(1,1) logical = 0 + % FrictionModel: Which Friction model to use for the Complementarity Lagrangian System. + % + % See Also: + % `FrictionModel` for more details as to the differences between the friction models. + friction_model (1,1) FrictionModel = FrictionModel.Conic; + + % ConicModelSwitchHandling: Which velocity switch handling mode to use when using the Conic friction model + % See Also: + % `ConicModelSwitchHandling` for more details as to the differences between the switch handling modes. + conic_model_switch_handling (1,1) ConicModelSwitchHandling = ConicModelSwitchHandling.Abs; + + kappa_friction_reg (1,1) double {mustBeReal, mustBeNonnegative} = 0; % double: Regularization term in friction equations to avoid large multipliers if no contact happens. + + lift_velocity_state(1,1) logical = 0; % boolean: If true define auxliary algebraic vairable, $dot{v} = z_v$, to avoid symbolic inversion of the inertia matrix. + eps_cls double = 1e-3 % double: enforce $f_c$ at Euler step with h * eps_cls + + % ConstraintRelaxationMode: What (if any) relaxation to apply to the terminal constraints. + % + % See Also: + % `ConstraintRelaxationMode` for a detailed description of the available relaxation modes. + relax_terminal_constraint(1,1) ConstraintRelaxationMode = ConstraintRelaxationMode.NONE; + relax_terminal_constraint_from_above(1,1) logical = 0; % boolean: If true we only relax the upper bound of the terminal constraint. TODO(@armin) do we still want this. I have never seen it be useful. + rho_terminal(1,1) double {mustBePositive} = 1e2; % double: Weight used to penalize terminal constraint violation. + + % boolean: If True the terminal constraint violation penalty is governed by homotopy parameter. + % + % Warning: + % This option is currently unimplemented. + relax_terminal_constraint_homotopy(1,1) logical = 0; + + relax_terminal_numerical_time(1,1) logical = 0; %boolean: If true instead of imposing $\sum h = T$, add it as $\ell_1$ penalty term. + rho_terminal_numerical_time(1,1) double {mustBeNonnegative} = 1e2 % double: Weight used to penalize terminal numerical time violation. + + % boolean: If True the terminal numerical time constraint violation penalty is governed by homotopy parameter + % + % Warning: + % This option is currently unimplemented + relax_terminal_numerical_time_homotopy (1,1) logical = 0; % us the homotopy parameter for the penalty. + relax_terminal_physical_time(1,1) logical = 0; % instead of imposing $t(T) = T$, add it as $\ell_1$ penalty term. + rho_terminal_physical_time(1,1) double {mustBeNonnegative} = 1e2 % double: Weight used to penalize terminal physical time violation. + + % boolean: If True the terminal physical time constraint violation penalty is governed by homotopy parameter. + % + % Warning: + % This option is currently unimplemented. + relax_terminal_physical_time_homotopy (1,1) logical = 0; + + cost_integration(1,1) logical = 1 % boolean: If true then the Lagrange term is integrated correctly, otherwise we only evaluate it at the ends of control stages. Setting this to false allows us to do parameter estimation with a nonlinear cost function. + + no_initial_impacts(1,1) logical = 0 % boolan: If true we disallow impulsive contacts at the beginning of the first control stage. + + use_previous_solution_as_initial_guess(1,1) logical = 0 % boolean: When simulating use the previous step as an initial guess for the current one. - % All MPCC parameters T_val(1,1) double {mustBePositive} = 1 p_val - % Butcher Tableu A_rk double B_rk double b_rk double diff --git a/docs/source/index.rst b/docs/source/index.rst index 8960b956..08b4b2cb 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -15,3 +15,4 @@ Contents usage models + options 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 From 3dc0e542b228f220836b73fec7738c4af3c3bf87 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 14:04:20 +0200 Subject: [PATCH 34/43] A few more fixes in docs --- +nosnoc/Options.m | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/+nosnoc/Options.m b/+nosnoc/Options.m index 096522a4..13293f02 100644 --- a/+nosnoc/Options.m +++ b/+nosnoc/Options.m @@ -28,7 +28,7 @@ % The main options class for `nosnoc`. It contains the options for simulation and optimal control reformulations but *does not* % contain settings for the MPCC solver. properties - % boolean: If true the FESD discretization is used, otherwise a direct time-stepping discretization is used + % boolean: If true the FESD discretization is used, otherwise a direct time-stepping discretization is used. use_fesd(1,1) logical = 1; % string: Which casadi symbolics to use. Can either be `'casadi.SX'` or `'casadi.MX'.` @@ -38,9 +38,9 @@ N_sim h_sim - h % double: Step size - h_k % double: Finite element step size - T % double: Terminal time TODO: why is there also T_val, do we need both? + h % double: Control stage Step size. + h_k % double: Finite element step size. + T % double: Terminal time. N_stages(1,1) {mustBeInteger, mustBePositive} = 1; % int: Number of control stages. @@ -145,6 +145,7 @@ kappa_stabilizing_q_dynamics(1,1) double {mustBePositive} = 1e-5 % double: Constant used for stabilizing auxiliary dynamics in \nabla f_c(q) direction. % int: Level of verbosity that the `nosnoc` reformulator uses. + % % TODO: % @anton, @armin document this better. print_level = 3 From 690fcaad18173731777cd948f45afef32753fedd Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 14:40:30 +0200 Subject: [PATCH 35/43] some docs improvements --- +nosnoc/+dcs/Base.m | 34 +++++++++++++++++++++------------- +nosnoc/+dcs/Gcs.m | 22 ++++++++++++++++------ +nosnoc/Options.m | 2 +- docs/source/conf.py | 1 + docs/source/dcs.rst | 6 ++++++ docs/source/index.rst | 3 ++- 6 files changed, 47 insertions(+), 21 deletions(-) create mode 100644 docs/source/dcs.rst 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 index 8996637a..7fdd5803 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -1,15 +1,25 @@ 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 or casadi.MX: Lagrange multipliers for the gradient complementarity system. - c_lift %casadi.SX or casadi.MX: Lift variables for the gap functions in :class:`nosnoc.model.Pds`. - + 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. + dims % struct: dimensions struct TODO document members - f_unconstrained_fun % casadi.Function: Unconstrained dynamics + f_unconstrained_fun % casadi.Function: Unconstrained dynamics. nabla_c_fun % casadi.Function: Gradient of the gap functions. - c_fun % casadi.Function: 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 diff --git a/+nosnoc/Options.m b/+nosnoc/Options.m index 13293f02..d883a460 100644 --- a/+nosnoc/Options.m +++ b/+nosnoc/Options.m @@ -146,7 +146,7 @@ % int: Level of verbosity that the `nosnoc` reformulator uses. % - % TODO: + % Todo: % @anton, @armin document this better. print_level = 3 diff --git a/docs/source/conf.py b/docs/source/conf.py index ffe66312..3032a1ed 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -17,6 +17,7 @@ extensions = [ 'sphinx.ext.napoleon', 'sphinx.ext.autodoc', + 'sphinx.ext.todo', 'sphinx.ext.autosummary', 'sphinx.ext.intersphinx', 'sphinxcontrib.matlab', diff --git a/docs/source/dcs.rst b/docs/source/dcs.rst new file mode 100644 index 00000000..ed44664e --- /dev/null +++ b/docs/source/dcs.rst @@ -0,0 +1,6 @@ +Dynamic Complementarity Systems +=============================== + +.. module:: +nosnoc.+dcs +.. autoclass:: Base +.. autoclass:: Gcs diff --git a/docs/source/index.rst b/docs/source/index.rst index 08b4b2cb..6d7cca51 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -14,5 +14,6 @@ Contents .. toctree:: usage - models options + models + dcs From 6c8d95139034b3fc3d57e6e8be82e2d28f367671 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 15:04:36 +0200 Subject: [PATCH 36/43] GCS comment improved and installation instructions --- +nosnoc/+dcs/Gcs.m | 2 +- docs/source/usage.rst | 21 +++++++++++++++++++++ 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/+nosnoc/+dcs/Gcs.m b/+nosnoc/+dcs/Gcs.m index 7fdd5803..4efcbdbb 100644 --- a/+nosnoc/+dcs/Gcs.m +++ b/+nosnoc/+dcs/Gcs.m @@ -1,5 +1,5 @@ classdef Gcs < nosnoc.dcs.Base -% A particular class of Dynamic Complementarity system called a Gradient complementarity system. +% A particular class of dynamic complementarity system called a gradient complementarity system. % It is defined by the dynamics: % % .. math:: 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. From 234e0fca0f366e59d8056806a8c262a486cca295 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 15:43:28 +0200 Subject: [PATCH 37/43] lots of minor docs fixes --- +nosnoc/+discrete_time_problem/Gcs.m | 4 ++-- +nosnoc/+model/Base.m | 2 +- docs/requirements.txt | 1 + docs/source/conf.py | 4 +++- docs/source/models.rst | 2 +- 5 files changed, 8 insertions(+), 5 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index 5d61070c..b88495be 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -87,8 +87,8 @@ function create_variables(obj) % 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}; - obj.w.z(0,0,opts.n_s) = {{'z', dims.n_z}, model.lbz, model.ubz, model.z0}; + 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}; obj.w.c_lift(0,0,opts.n_s) = {{['c_lift'], dims.n_c_lift},-inf,inf}; for ii=1:opts.N_stages diff --git a/+nosnoc/+model/Base.m b/+nosnoc/+model/Base.m index 01712576..d95facb1 100644 --- a/+nosnoc/+model/Base.m +++ b/+nosnoc/+model/Base.m @@ -1,5 +1,5 @@ classdef Base < matlab.mixin.Scalar & handle & matlab.mixin.CustomDisplay -% Base class for all nosnoc models. It contains shared properties such as the state, user algebraics, controls etc. +% Base class for all ``nosnoc`` models. It contains shared properties such as the state, user algebraics, controls etc. % It also contains the fields used to populate an Optimal Control problem such as Lagrange and Mayer cost terms, % an interface for least squares costs as well as non-box path and terminal constraints. properties diff --git a/docs/requirements.txt b/docs/requirements.txt index 9200eb14..176e5302 100644 --- a/docs/requirements.txt +++ b/docs/requirements.txt @@ -1,4 +1,5 @@ sphinx==7.3.7 sphinx-rtd-theme==1.3.0rc1 +furo==2024.05.06 sphinxcontrib-matlabdomain @ git+https://github.com/apozharski/matlabdomain@master sphinx-math-dollar diff --git a/docs/source/conf.py b/docs/source/conf.py index 3032a1ed..2a375d1d 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -35,7 +35,9 @@ # -- Options for HTML output -html_theme = 'sphinx_rtd_theme' +html_theme = 'furo' +pygments_style = "sphinx" +pygments_dark_style = "monokai" # -- Options for EPUB output epub_show_urls = 'footnote' diff --git a/docs/source/models.rst b/docs/source/models.rst index bd8f4c80..d0f8cff0 100644 --- a/docs/source/models.rst +++ b/docs/source/models.rst @@ -1,6 +1,6 @@ Models ====== -All `nosnoc` models are derived from :class:`+nosnoc.+model.Base` and share its functionality. +All ``nosnoc`` models are derived from :mat:class:`nosnoc.model.Base` and share its functionality. .. module:: +nosnoc.+model .. autoclass:: Base From 21226b9f748e315956090460b951ba7d30cd5c58 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 15:53:15 +0200 Subject: [PATCH 38/43] move basic pds example --- docs/source/index.rst | 2 +- .../new_design_pds.m => basic_pds/basic_pds.m} | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) rename examples/{new_design_examples/new_design_pds.m => basic_pds/basic_pds.m} (99%) diff --git a/docs/source/index.rst b/docs/source/index.rst index 6d7cca51..0de147e3 100644 --- a/docs/source/index.rst +++ b/docs/source/index.rst @@ -2,7 +2,7 @@ NOSNOC ============================================= Check out the :doc:`usage` section for further information, including -how to :ref:`installation` the project. +how to :ref:`install ` the project. .. note:: diff --git a/examples/new_design_examples/new_design_pds.m b/examples/basic_pds/basic_pds.m similarity index 99% rename from examples/new_design_examples/new_design_pds.m rename to examples/basic_pds/basic_pds.m index 5c8df341..77615799 100644 --- a/examples/new_design_examples/new_design_pds.m +++ b/examples/basic_pds/basic_pds.m @@ -1,4 +1,3 @@ - clear all clc close all From c4c3c91461d83b5e57a6eb076593132f2be78033 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 15:55:18 +0200 Subject: [PATCH 39/43] move moving set example --- examples/{new_design_examples => sweeping_set}/plot_moving_set.m | 0 .../new_design_moving_set.m => sweeping_set/sweeping_set.m} | 1 - 2 files changed, 1 deletion(-) rename examples/{new_design_examples => sweeping_set}/plot_moving_set.m (100%) rename examples/{new_design_examples/new_design_moving_set.m => sweeping_set/sweeping_set.m} (99%) diff --git a/examples/new_design_examples/plot_moving_set.m b/examples/sweeping_set/plot_moving_set.m similarity index 100% rename from examples/new_design_examples/plot_moving_set.m rename to examples/sweeping_set/plot_moving_set.m diff --git a/examples/new_design_examples/new_design_moving_set.m b/examples/sweeping_set/sweeping_set.m similarity index 99% rename from examples/new_design_examples/new_design_moving_set.m rename to examples/sweeping_set/sweeping_set.m index 74e50c3f..6e60fb24 100644 --- a/examples/new_design_examples/new_design_moving_set.m +++ b/examples/sweeping_set/sweeping_set.m @@ -1,4 +1,3 @@ - clear all clc close all From e65abcee6c19b257df44701e5c7986f52c26cf8f Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Wed, 3 Jul 2024 16:02:11 +0200 Subject: [PATCH 40/43] move msv example --- .../msv.m} | 3 --- 1 file changed, 3 deletions(-) rename examples/{new_design_examples/new_design_msv.m => mordukhovich2023_marine_surface_vehicle/msv.m} (95%) diff --git a/examples/new_design_examples/new_design_msv.m b/examples/mordukhovich2023_marine_surface_vehicle/msv.m similarity index 95% rename from examples/new_design_examples/new_design_msv.m rename to examples/mordukhovich2023_marine_surface_vehicle/msv.m index e6eb1b7f..37754165 100644 --- a/examples/new_design_examples/new_design_msv.m +++ b/examples/mordukhovich2023_marine_surface_vehicle/msv.m @@ -41,8 +41,6 @@ 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.lbu = [-100/sqrt(2);-100/sqrt(2);0;0]; -%model.ubu = [100/sqrt(2);100/sqrt(2);0;0]; model.u0 = model.ubu; model.c = [norm_2(x2-x1)-2*R]; model.f_x_unconstrained = [u1;u2]; @@ -70,7 +68,6 @@ 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,:))); -%plot_discs(h_res,x_res,[3.5,3.5], ["circle", "circle"]) v_res = u_rep + repmat(lambda_res(2:end),4,1).*nabla_c_res(:, 2:end); figure('Position', [0,0, 400. 400]) ax = subplot(3,1,1); From 9e7f880c4669ebe916de3d9e5fe243bab57469c7 Mon Sep 17 00:00:00 2001 From: Anton Pozharskiy Date: Wed, 3 Jul 2024 22:59:47 +0200 Subject: [PATCH 41/43] setting for lambda init --- +nosnoc/+discrete_time_problem/Gcs.m | 4 ++-- +nosnoc/Options.m | 1 + external/vdx | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index b88495be..d8fd0cd9 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -89,7 +89,7 @@ function create_variables(obj) % 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}; + 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}; @@ -98,7 +98,7 @@ function create_variables(obj) 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}; + 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 diff --git a/+nosnoc/Options.m b/+nosnoc/Options.m index d883a460..a0df38bf 100644 --- a/+nosnoc/Options.m +++ b/+nosnoc/Options.m @@ -81,6 +81,7 @@ initial_alpha(1,1) double {mustBeReal, mustBeFinite} = 1 % double: Initial value for $\alpha$ in the Heaviside step reformulation. initial_beta(1,1) double {mustBeReal, mustBeFinite} = 1 % double: Initial value for $\beta$ when lifting is enabled in the Heaviside step reformulation. initial_theta_step(1,1) double {mustBeReal, mustBeFinite} = 1 % double: Initial value for $\theta$ when lifting is enabled in the Heaviside step reformulation. + initial_lambda_gcs(1,1) double {mustBeReal, mustBeFinite} = 0 % double: Initial value for $\lambda$ in the Gradient Comlementarity System. % boolean: If true then the convex multiplier expressions are lifted in the Heaviside step reformulation. % diff --git a/external/vdx b/external/vdx index 693f8150..97915c77 160000 --- a/external/vdx +++ b/external/vdx @@ -1 +1 @@ -Subproject commit 693f8150890771ba7d5963c0ab02f7eb7a5cd3b6 +Subproject commit 97915c77fa07cebbc90766471d08a4711ab0dfd6 From 7e8db896a9d8e0f3f32a323668502d8192ac97ec Mon Sep 17 00:00:00 2001 From: "armin.nurkanovic" Date: Thu, 4 Jul 2024 10:18:33 +0200 Subject: [PATCH 42/43] polish examples --- examples/basic_pds/basic_pds.m | 23 ++++++++++++---- .../msv.m | 26 +++++++++---------- examples/sweeping_set/plot_moving_set.m | 4 +-- examples/sweeping_set/sweeping_set.m | 10 +++---- 4 files changed, 38 insertions(+), 25 deletions(-) rename examples/{mordukhovich2023_marine_surface_vehicle => marine_surface_vehicle}/msv.m (83%) diff --git a/examples/basic_pds/basic_pds.m b/examples/basic_pds/basic_pds.m index 77615799..fd8ff1f7 100644 --- a/examples/basic_pds/basic_pds.m +++ b/examples/basic_pds/basic_pds.m @@ -10,8 +10,6 @@ problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); problem_options.n_s = 3; -%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; -%problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; problem_options.rk_scheme = RKSchemes.RADAU_IIA; %problem_options.rk_representation= RKRepresentation.differential_lift_x; problem_options.rk_representation = RKRepresentation.integral; @@ -43,11 +41,26 @@ figure plot(x_res(1,:), x_res(2,:)) grid on -xlabel('$x_1(t)$','Interpreter','latex') -ylabel('$x_2(t)$','Interpreter','latex') +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'); -lam_full = integrator.get_full('lambda'); +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/mordukhovich2023_marine_surface_vehicle/msv.m b/examples/marine_surface_vehicle/msv.m similarity index 83% rename from examples/mordukhovich2023_marine_surface_vehicle/msv.m rename to examples/marine_surface_vehicle/msv.m index 37754165..cdd26b39 100644 --- a/examples/mordukhovich2023_marine_surface_vehicle/msv.m +++ b/examples/marine_surface_vehicle/msv.m @@ -7,13 +7,12 @@ %% Define projected system problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); -%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; -%problem_options.rk_scheme = RKSchemes.LOBATTO_IIIC; +% 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.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.cross_comp_mode = CrossCompMode.FE_FE; problem_options.N_finite_elements = 2; problem_options.n_s = 2; problem_options.N_stages = 25; @@ -21,7 +20,7 @@ problem_options.rho_h = 1e-4; problem_options.time_optimal_problem = true; problem_options.use_fesd = true; -problem_options.gcs_lift_gap_functions = false; +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; @@ -62,18 +61,19 @@ lambda_res = ocp_solver.get('lambda'); t_res = ocp_solver.get_time_grid(); if ~problem_options.use_fesd - t_res = t_res*T_final + 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('Position', [0,0, 400. 400]) + +figure ax = subplot(3,1,1); plot(t_res(1:end-1), v_res([1,3],:), "LineWidth", 2) -xlabel("$t$") -ylabel("$v_x$") +xlabel("$t$",'Interpreter','latex') +ylabel("$v_x$",'Interpreter','latex') ylim([40, 75]) xlim([0, T_final]) grid on @@ -81,8 +81,8 @@ ax.FontSize = fontsize; ax = subplot(3,1,2); plot(t_res(1:end-1), v_res([2,4],:), "LineWidth", 2) -xlabel("$t$") -ylabel("$v_y$") +xlabel("$t$",'Interpreter','latex') +ylabel("$v_y$",'Interpreter','latex') ax.FontSize = fontsize; ax.FontSize = fontsize; ylim([40, 75]) @@ -90,8 +90,8 @@ grid on ax = subplot(3,1,3); plot(t_res, c_res, "LineWidth", 2) -xlabel("$t$") -ylabel("$c(x)$") +xlabel("$t$",'Interpreter','latex') +ylabel("$c(x)$",'Interpreter','latex') ax.FontSize = fontsize; ax.FontSize = fontsize; ylim([-0.1, 10]) diff --git a/examples/sweeping_set/plot_moving_set.m b/examples/sweeping_set/plot_moving_set.m index 3dfc37cd..c46ba951 100644 --- a/examples/sweeping_set/plot_moving_set.m +++ b/examples/sweeping_set/plot_moving_set.m @@ -37,8 +37,8 @@ function plot_moving_set(h,pos,r,type,fig,vidname,export_frames) ylim([-3,3]) ax.XAxis.FontSize = 36; ax.YAxis.FontSize = 36; - xlabel("$c_x$", "fontsize", 32) - ylabel("$c_y$", "fontsize", 32) + xlabel("$c_x$", "fontsize", 32,'Interpreter','latex') + ylabel("$c_y$", "fontsize", 32,'Interpreter','latex') %axis off discs = {}; diff --git a/examples/sweeping_set/sweeping_set.m b/examples/sweeping_set/sweeping_set.m index 6e60fb24..1664f18f 100644 --- a/examples/sweeping_set/sweeping_set.m +++ b/examples/sweeping_set/sweeping_set.m @@ -9,11 +9,11 @@ %% NOSNOC settings problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); -problem_options.n_s = 3; +problem_options.n_s = 1; %problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; problem_options.rk_scheme = RKSchemes.RADAU_IIA; -%problem_options.rk_representation= 'differential_lift_x'; -problem_options.rk_representation= 'integral'; +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; @@ -37,8 +37,8 @@ figure plot(x_res(1,:), x_res(2,:)) grid on -xlabel('$x_1(t)$','Interpreter','latex') -ylabel('$x_2(t)$','Interpreter','latex') +xlabel('$x_1$','Interpreter','latex') +ylabel('$x_2$','Interpreter','latex') grid on fig = figure('Position', [10 10 1600 800]); From 1634cc635a4c061af377477f8532e298b0e4e693 Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Thu, 4 Jul 2024 10:53:36 +0200 Subject: [PATCH 43/43] fix bug in differential lift and bump vdx --- +nosnoc/+discrete_time_problem/Gcs.m | 6 +----- examples/sweeping_set/sweeping_set.m | 5 ++--- external/vdx | 2 +- 3 files changed, 4 insertions(+), 9 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Gcs.m b/+nosnoc/+discrete_time_problem/Gcs.m index d8fd0cd9..97455b45 100644 --- a/+nosnoc/+discrete_time_problem/Gcs.m +++ b/+nosnoc/+discrete_time_problem/Gcs.m @@ -231,6 +231,7 @@ function generate_direct_transcription_constraints(obj) 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); @@ -255,11 +256,6 @@ function generate_direct_transcription_constraints(obj) obj.f = obj.f + opts.b_rk(kk)*h*q_ijk; end end - if ~opts.right_boundary_point_explicit - error("not implemented"); - 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, u_i, v_global, p), model.lbg_path, model.ubg_path}; end diff --git a/examples/sweeping_set/sweeping_set.m b/examples/sweeping_set/sweeping_set.m index 1664f18f..ffdbe3c7 100644 --- a/examples/sweeping_set/sweeping_set.m +++ b/examples/sweeping_set/sweeping_set.m @@ -9,8 +9,7 @@ %% NOSNOC settings problem_options = nosnoc.Options(); solver_options = nosnoc.solver.Options(); -problem_options.n_s = 1; -%problem_options.rk_scheme = RKSchemes.GAUSS_LEGENDRE; +problem_options.n_s = 2; problem_options.rk_scheme = RKSchemes.RADAU_IIA; problem_options.rk_representation= 'differential_lift_x'; % problem_options.rk_representation= 'integral'; @@ -19,7 +18,7 @@ problem_options.T_sim = T_sim; %solver_options.homotopy_steering_strategy = 'ELL_INF'; -solver_options.print_level = 2; +solver_options.print_level = 3; model = nosnoc.model.Pds(); diff --git a/external/vdx b/external/vdx index 97915c77..693f8150 160000 --- a/external/vdx +++ b/external/vdx @@ -1 +1 @@ -Subproject commit 97915c77fa07cebbc90766471d08a4711ab0dfd6 +Subproject commit 693f8150890771ba7d5963c0ab02f7eb7a5cd3b6