EmaMaker
/
thesis
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

use active set algorithm to workaround singular hessian

diffdrive-costfun
EmaMaker 2024-10-06 14:54:24 +02:00
parent 8bf21fa218
commit 423a424be6
2 changed files with 26 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
control_act.m
View File

@ -30,13 +30,17 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
if eq(pred_hor, 0) if eq(pred_hor, 0)
return return
elseif eq(pred_hor, 1) elseif eq(pred_hor, 1)
% minimize wcorr_r^2 + wcorr_l^2
%H = eye(2);
if eq(sim_data.costfun, 1)
% minimize vcorr_r^2 + wcorr_l^2
H = eye(2);
elseif eq(sim_data.costfun, 2)
% ex1: minimize v=r(wr+wl)/2 % ex1: minimize v=r(wr+wl)/2
%H = sim_data.r*sim_data.r*0.5*ones(2,2); H = sim_data.r*sim_data.r*0.5*ones(2,2);
elseif eq(sim_data.costfun, 3)
% ex2: minimize w=r(wr-wl)/d % ex2: minimize w=r(wr-wl)/d
H = sim_data.r*sim_data.r*2*[1, -1; -1, 1]/(sim_data.d*sim_data.d); H = sim_data.r*sim_data.r*2*[1, -1; -1, 1]/(sim_data.d*sim_data.d);
end
f = zeros(2,1); f = zeros(2,1);
T_inv = decouple_matrix(q_act, sim_data); T_inv = decouple_matrix(q_act, sim_data);
@ -45,8 +49,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
d = T_inv*ut; d = T_inv*ut;
% solve qp problem % solve qp problem
options = optimoptions('quadprog', 'Display', 'off'); options = optimoptions('quadprog', 'Algorithm','active-set','Display','off');
u_corr = quadprog(H, f, [], [], [],[], -s_ - d, s_-d, [], options); u_corr = quadprog(H, f, [], [], [],[], -s_ - d, s_-d, zeros(2,1), options);
q_pred = q_act; q_pred = q_act;
U_corr_history(:,:,1) = u_corr; U_corr_history(:,:,1) = u_corr;
@ -127,16 +131,26 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
lb = [lb; -s_-d]; lb = [lb; -s_-d];
ub = [ub; s_-d]; ub = [ub; s_-d];
end end
if eq(sim_data.costfun, 1)
% minimize vcorr_r^2 + wcorr_l^2
% squared norm of u_corr. H must be identity, % squared norm of u_corr. H must be identity,
H = eye(pred_hor*2)*2; H = eye(pred_hor*2)*2;
elseif eq(sim_data.costfun, 2)
% ex1: minimize v=r(wr+wl)/2
H = kron(eye(pred_hor), sim_data.r*sim_data.r*0.5*ones(2,2));
elseif eq(sim_data.costfun, 3)
% ex2: minimize w=r(wr-wl)/d
H = kron(eye(pred_hor), sim_data.r*sim_data.r*2*[1, -1; -1, 1]/(sim_data.d*sim_data.d));
end
% no linear terms % no linear terms
f = zeros(pred_hor*2, 1); f = zeros(pred_hor*2, 1);
% solve qp problem % solve qp problem
options = optimoptions('quadprog', 'Display', 'off'); options = optimoptions('quadprog', 'Algorithm','active-set','Display','off');
U_corr = quadprog(H, f, [], [], [],[],lb,ub,[],options); U_corr = quadprog(H, f, [], [], [],[], lb, ub, zeros(2*pred_hor,1), options);
% reshape the vector of vectors to be an array, each element being % reshape the vector of vectors to be an array, each element being
% u_corr_j as a 2x1 vector % u_corr_j as a 2x1 vector
% and add the prediction at t_k+C % and add the prediction at t_k+C

4
tesi.m
View File

@ -29,12 +29,14 @@ for i = 1:length(TESTS)
[ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data); [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data);
sim_data.ref = ref; sim_data.ref = ref;
sim_data.dref = dref; sim_data.dref = dref;
sim_data.costfun=2
sim_data.tc=0.05;
% spawn a new worker for each controller % spawn a new worker for each controller
% 1: track only % 1: track only
% 2: track + 1step % 2: track + 1step
% 3: track + multistep % 3: track + multistep
spmd (2) spmd (3)
worker_index = spmdIndex; worker_index = spmdIndex;
% load controller-specific options % load controller-specific options
data = load(['tests/' num2str(worker_index) '.mat']); data = load(['tests/' num2str(worker_index) '.mat']);