Skip to content

Commit

Permalink
horrible hacks to get CLS extraction to work :)
Browse files Browse the repository at this point in the history
  • Loading branch information
@apozharski
apozharski committed Jun 17, 2024
1 parent 80b1e54 commit 9aa2acb
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 29 deletions.
5 changes: 2 additions & 3 deletions +nosnoc/+dcs/Cls.m
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,5 @@
classdef Cls < nosnoc.dcs.Base
properties
model

% CLS stage vars
lambda_normal
y_gap
Expand Down Expand Up @@ -205,7 +203,8 @@ function generate_equations(obj, opts)
obj.g_alg_fun = Function('g_alg', {model.x, model.z, z_alg, model.v_global, model.p}, {g_alg});
obj.g_impulse_fun = Function('g_impulse', {model.q, v_post_impact, v_pre_impact, z_impulse, model.v_global, model.p}, {g_impulse}); % TODO (@anton) user algebraics pre and post impact?
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_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});
Expand Down
33 changes: 20 additions & 13 deletions +nosnoc/+discrete_time_problem/Cls.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -330,10 +330,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
Expand Down Expand Up @@ -474,12 +475,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, ui, v_global, p);
end
Expand Down Expand Up @@ -515,9 +520,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
Expand Down
36 changes: 23 additions & 13 deletions +nosnoc/Integrator.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,30 +112,40 @@
ii, opts.N_sim, t_current, opts.T_sim, solver_stats.cpu_time_total);
end
obj.w_all = [obj.w_all,obj.discrete_time_problem.w.res];
x_step = obj.discrete_time_problem.w.x(:,:,opts.n_s).res;
x_step_full = obj.discrete_time_problem.w.x(:,:,:).res;
x_res = [x_res, x_step(:,2:end)];
x_res_full = [x_res_full, x_step_full(:,2:end)];

if ~strcmp(class(obj.discrete_time_problem), 'CLS')
if obj.opts.dcs_mode ~= DcsMode.CLS
x_step = obj.discrete_time_problem.w.x(:,:,opts.n_s).res;
x_step_full = obj.discrete_time_problem.w.x(:,:,:).res;
x_res = [x_res, x_step(:,2:end)];
x_res_full = [x_res_full, x_step_full(:,2:end)];
if opts.use_fesd
h = obj.discrete_time_problem.w.h(:,:).res;
else
h = ones(1,opts.N_finite_elements) * obj.discrete_time_problem.p.T().val/opts.N_finite_elements;
end
t_grid = [t_grid, t_grid(end) + cumsum(h)];
for ii = 1:length(h)
for jj = 1:opts.n_s
t_grid_full = [t_grid_full; t_grid_full(end) + opts.c_rk(jj)*h(ii)];
end
end
else
x_step = obj.discrete_time_problem.w.x(:,:,[0,opts.n_s]).res;
x_step_full = obj.discrete_time_problem.w.x(:,:,:).res;
x_res = [x_res, x_step(:,(2+opts.no_initial_impacts):end)];
x_res_full = [x_res_full, x_step_full(:,(2+opts.no_initial_impacts):end)];
h = obj.discrete_time_problem.w.h(:,:).res;
for hi=h
t_grid = [t_grid, t_grid(end), t_grid(end)+hi];
end
end
t_grid = [t_grid, t_grid(end) + cumsum(h)];
for ii = 1:length(h)
for jj = 1:opts.n_s
t_grid_full = [t_grid_full; t_grid_full(end) + opts.c_rk(jj)*h(ii)];
for ii=1:length(h)
hi = h(ii);
if opts.no_initial_impacts && ii==1
t_grid = [t_grid, t_grid(end)+hi];
else
t_grid = [t_grid, t_grid(end), t_grid(end)+hi];
end
end
% TODO(@anton) do full grid
end

%obj.discrete_time_problem.w.init = w0;%obj.discrete_time_problem.w.res;
obj.set_x0(x_step(:,end));
end
Expand Down

0 comments on commit 9aa2acb

Please sign in to comment.