Skip to content

Commit

Permalink
initial work
Browse files Browse the repository at this point in the history
  • Loading branch information
@apozharski
apozharski committed May 13, 2024
1 parent 748a360 commit a8b70dc
Show file tree
Hide file tree
Showing 5 changed files with 756 additions and 0 deletions.
9 changes: 9 additions & 0 deletions +nosnoc/+dcs/base.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
classdef base < matlab.mixin.Scalar
properties
end

methods(abstract)
generate_variables(obj, opts)
generate_equations(obj, opts)
end
end
92 changes: 92 additions & 0 deletions +nosnoc/+dcs/stewart.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
classdef stewart < nosnoc.dcs.base
properties
model nosnoc.model.pss

theta
theta_sys
lambda
lambda_sys
mu
mu_sys

f_x
g_Stewart

dims
end

methods
function obj = stewart(model)
obj.model = model;
dims = model.dims;
end

function generate_variables(obj, opts)
import casadi.*
dims = obj.dims;
% dimensions
dims.n_theta = sum(obj.dims.n_f_sys); % number of modes
dims.n_lambda = dims.n_theta;
for ii = 1:dims.n_sys
ii_str = num2str(ii);
% define theta (Filippov multiplers)
obj.theta_sys{ii} = define_casadi_symbolic(casadi_symbolic_mode,['theta_' ii_str],obj.dims.n_f_sys(ii));
obj.theta = [obj.theta;obj.theta_sys{ii}];
% define mu_i (Lagrange multipler of e'theta =1;)
obj.mu_sys{ii} = define_casadi_symbolic(casadi_symbolic_mode,['mu_' ii_str],1);
obj.mu = [obj.mu;obj.mu_sys{ii}];
% define lambda_i (Lagrange multipler of theta >= 0;)
obj.lambda_sys{ii} = define_casadi_symbolic(casadi_symbolic_mode,['lambda_' ii_str],obj.dims.n_f_sys(ii));
obj.lambda = [obj.lambda;obj.lambda_sys{ii}];
end

% symbolic variables z = [theta;lambda;mu_Stewart];
obj.z_all = [obj.theta;obj.lambda;obj.mu;obj.z];
end

function generate_equations(obj, opts)
import casadi.*
model = obj.model
dims = obj.dims;

obj.f_x = zeros(dims.n_x,1);
for ii = 1:dims.n_sys
obj.f_x = obj.f_x + obj.F{ii}*obj.theta_sys{ii};
obj.g_Stewart{ii} = -obj.S{ii}*obj.c{ii};
end

g_switching = []; % collects switching function algebraic equations, 0 = g_i(x) - \lambda_i - e \mu_i, 0 = c(x)-lambda_p+lambda_n
g_convex = []; % equation for the convex multiplers 1 = e' \theta

lambda00_expr =[];
for ii = 1:dims.n_sys
% basic algebraic equations and complementarity condtions of the DCS
% (Note that the cross complementarities are later defined when the discrete
% time variables for every IRK stage in the create_nlp_nosnoc function are defined.)
% g_ind_i - lambda_i + mu_i e_i = 0; for all i = 1,..., n_sys
% lambda_i'*theta_i = 0; for all i = 1,..., n_sys
% lambda_i >= 0; for all i = 1,..., n_sys
% theta_i >= 0; for all i = 1,..., n_sys
% Gradient of Lagrange Function of indicator LP
g_switching = [g_switching; obj.g_Stewart{ii} - obj.lambda_sys{ii}+obj.mu_sys{ii}*ones(dims.n_f_sys(ii),1)];
g_convex = [g_convex;ones(dims.n_f_sys(ii),1)'*obj.theta_sys{ii} - 1];
lambda00_expr = [lambda00_expr; obj.g_Stewart{ii} - min(obj.g_Stewart{ii})];
end
g_alg = [g_switching;g_convex];

obj.f_x_fun = Function('f_x', {obj.x, obj.z, obj.lambda, obj.theta, obj.mu, obj.u, obj.v_global, obj.p}, {obj.f_x, model.f_q});
obj.f_q_fun = Function('f_q', {obj.x, obj.z, obj.lambda, obj.theta, obj.mu, obj.u, obj.v_global, obj.p}, {model.f_q});
obj.g_z_fun = Function('g_z', {obj.x, obj.z, obj.u, obj.v_global, obj.p}, {model.g_z});
obj.g_alg_fun = Function('g_alg', {obj.x, obj.z, obj.lambda, obj.theta, obj.mu, obj.u, obj.v_global, obj.p}, {g_alg});
obj.g_Stewart_fun = Function('g_Stewart', {obj.x, obj.z, obj.v_global, obj.p}, {obj.g_Stewart{:}});
obj.lambda00_fun = Function('lambda00', {obj.x, obj.z, obj.v_global, obj.p_global}, {lambda00_expr});
obj.g_path_fun = Function('g_path', {obj.x, obj.z, obj.u, obj.v_global, obj.p}, {model.g_path}); % TODO(@anton) do dependence checking for spliting the path constriants
obj.g_comp_path_fun = Function('g_comp_path', {obj.x, obj.z, obj.u, obj.v_global, obj.p}, {obj.g_comp_path});
obj.g_terminal_fun = Function('g_terminal', {obj.x, obj.z, obj.v_global, obj.p_global}, {obj.g_terminal});
obj.f_q_T_fun = Function('f_q_T', {obj.x, obj.z, obj.v_global, obj.p}, {obj.f_q_T});
obj.f_lsq_x_fun = Function('f_lsq_x_fun',{obj.x,obj.x_ref,obj.p},{obj.f_lsq_x});
obj.f_lsq_u_fun = Function('f_lsq_u_fun',{obj.u,obj.u_ref,obj.p},{obj.f_lsq_u});
obj.f_lsq_T_fun = Function('f_lsq_T_fun',{obj.x,obj.x_ref_end,obj.p_global},{obj.f_lsq_T});
end
end
end
174 changes: 174 additions & 0 deletions +nosnoc/+discrete_problem/stewart.m
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,174 @@
classdef stewart < vdx.problems.Mpcc
properties
model
dcs
opts
end

methods
function obj = stewart(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;

obj.p.rho_h_p = {{'rho_h_p',1}, 1};
obj.p.T = {{'T',1}, opts.T};
obj.p.p_global = {model.p_global, model.p_global_val};

% other derived values
h0 = opts.h;

% 0d vars
obj.w.v_global = {{'v_global',dims.n_v_global}, model.lbv_global, model.ubv_global, model.v0_global};

% 1d vars
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 obj.opts.use_speed_of_time_variables
obj.w.sot(1:opts.N_stages) = {{'sot', 1}, opts.s_sot_min, opts.s_sot_max, opts.s_sot0};
end

% 2d vars
if obj.opts.use_fesd
obj.w.h(1:opts.N_stages,1:opts.N_finite_elements(1)) = {{'h', 1}, (1-opts.gamma_h)*h0, (1+opts.gamma_h)*h0, h0};
end
if (strcmp(obj.opts.step_equilibration,'linear')||..
strcmp(obj.opts.step_equilibration,'linear_tanh')||...
strcmp(obj.opts.step_equilibration,'linear_relaxed'))
obj.w.B_max(1:opts.N_stages,2:opts.N_finite_elements) = {{'B_max', dims.n_c},-inf,inf};
obj.w.pi_lambda(1:opts.N_stages,2:opts.N_finite_elements(1)) = {{'pi_lambda', dims.n_c},-inf,inf};
obj.w.pi_c(1:opts.N_stages,2:opts.N_finite_elements(1)) = {{'pi_c', dims.n_c},-inf,inf};
obj.w.lambda_lambda(1:opts.N_stages,2:opts.N_finite_elements(1)) = {{'lambda_lambda', dims.n_c},0,inf};
obj.w.lambda_c(1:opts.N_stages,2:opts.N_finite_elements(1)) = {{'lambda_c', dims.n_c},0,inf};
obj.w.eta(1:opts.N_stages,2:opts.N_finite_elements(1)) = {{'eta', dims.n_c},0,inf};
obj.w.nu(1:opts.N_stages,2:opts.N_finite_elements(1)) = {{'nu', 1},0,inf};
end

% 3d vars
obj.w.x(0,0,opts.n_s) = {{['x_0'], dims.n_x}};
obj.w.x(1:opts.N_stages,1:opts.N_finite_elements(1),1:opts.n_s) = {{'x', dims.n_x}, model.lbx, model.ubx, model.x0};
obj.w.z(1:opts.N_stages,1:opts.N_finite_elements(1),1:opts.n_s) = {{'z', dims.n_z}, model.lbz, model.ubz, model.z0};
obj.w.lambda(0,0,opts.n_s) = {{['lambda_0'], dims.n_lambda},0,inf};
obj.w.lambda(1:opts.N_stages,1:opts.N_finite_elements(1),1:opts.n_s) = {{'lambda', dims.n_lambda},0, inf};
obj.w.theta(1:opts.N_stages,1:opts.N_finite_elements(1),1:opts.n_s) = {{'theta', dims.n_theta},0, 1};
obj.w.mu(1:opts.N_stages,1:opts.N_finite_elements(1),1:opts.n_s) = {{'mu', dims.n_mu},0,inf};
end

function forward_sim_constraints(obj)
import casadi.*
model = obj.model;
opts = obj.opts;
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(1));
else
h0 = obj.p.T().val/(opts.N_stages*opts.N_finite_elements(1));
end

v_global = obj.w.v_global();
p_global = obj.p.p_global();

x_0 = obj.w.x(0,0,opts.n_s);
lambda_0 = obj.w.lambda(0,0,opts.n_s);

x_prev = obj.w.x(0,0,opts.n_s);
for ii=1:opts.N_stages
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
s_sot = obj.w.sot(ii);
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;
else
h = h0;
end
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);
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);
xk = opts.C_irk(1, kk+1) * x_prev;
for rr=1:opts.n_s % TODO(@anton) handle other modes.
x_ijr = obj.w.x(ii,jj,rr);
xk = xk + opts.C_irk(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.path(ii,jj,kk) = {dcs.g_path_fun(x_ijk, z_ijk, ui, v_global, p_global, p_stage), model.lbg_path, model.ubg_path};
obj.g.algebraic(ii,jj,kk) = {dcs.g_alg_fun(x_ijk, z_ijk, lambda_ijk, theta_ijk, mu_ijk, ui, v_global, p)};
% also integrate the objective
obj.f = obj.f + opts.B_irk(kk+1)*h*qj;
end
x_prev = obj.w.x(ii,jj,opts.n_s);
end
if obj.opts.use_fesd
obj.g.sum_h(ii) = {t_stage-sum_h};
end
if obj.opts.use_speed_of_time_variables
x_end = obj.w.x(ii,opts.N_finite_elements(end),opts.n_s);
x_start = obj.w.x(0,0,opts.n_s);
obj.g.g_equidistant_grid(ii) = {(x_end(end)-x_start(end)) - t_stage*ii};
end
end

% Terminal cost
obj.f = obj.f + model.f_q_T_fun(obj.w.x(ii,jj,kk), obj.w.z(ii,jj,kk), v_global, p_global);

% Terminal constraint
obj.g.terminal = {model.g_terminal_fun(obj.w.x(ii,jj,kk), obj.w.z(ii,jj,kk), v_global, p_global), model.lbg_terminal, model.ubg_terminal};
end

function generate_complementarities(obj)

end

function step_equilibration(obj)

end

function populate_problem(obj)
obj.create_variables();
obj.forward_sim_constraints();
obj.generate_complementarities();
obj.step_equilibration();

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
obj.w.sort_by_index();
obj.g.sort_by_index();

solver_options.assume_lower_bounds = true;

create_solver@vdx.problems.Mpcc(obj, solver_options, plugin);
end
end
end

0 comments on commit a8b70dc

Please sign in to comment.