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

Compare commits

merge into: EmaMaker:master
Branches Tags
EmaMaker:diffdrive-costfun
EmaMaker:master-costfun
EmaMaker:master-costfun2
EmaMaker:master
...
pull from: EmaMaker:diffdrive-costfun
Branches Tags
EmaMaker:diffdrive-costfun
EmaMaker:master-costfun
EmaMaker:master-costfun2
EmaMaker:master

4 Commits
master ... diffdrive-

Author SHA1 Message Date
EmaMaker 87df77e9b0 costfun to minimize v^2+b^2w^2 2024-10-11 18:05:44 +02:00
EmaMaker 423a424be6 use active set algorithm to workaround singular hessian 2024-10-06 19:35:57 +02:00
EmaMaker 8bf21fa218 minimize v,w via w_r, w_l 2024-09-29 10:47:37 +02:00
EmaMaker 6dd6af2b8c switch to ub, lb constraints 
since reformulating the constraints A and b are no longer needed, this is easier
 2024-09-14 13:35:04 +02:00
4 changed files with 77 additions and 50 deletions
Show Stats Expand all files Collapse all files

63
control_act.m
View File

@ -30,19 +30,35 @@ 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)
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
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
H = sim_data.r*sim_data.r*2*[1, -1; -1, 1]/(sim_data.d*sim_data.d);
elseif eq(sim_data.costfun, 4)
rr = sim_data.r*sim_data.r;
dd = sim_data.d*sim_data.d;
bb = sim_data.b*sim_data.b;
H1 = rr*ones(2)/4;
H2 = bb*rr*[1 -1; -1 1]/dd;
H = 2 * (H1 + H2);
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);
ut = utrack(t, q_act, sim_data); ut = utrack(t, q_act, sim_data);
%A = [T_inv; -T_inv];
A = [eye(2); -eye(2)];
d = T_inv*ut; d = T_inv*ut;
b = [s_-d;s_+d];
% solve qp problem % solve qp problem
options = optimoptions('quadprog', 'Display', 'off'); options = optimoptions('quadprog', 'Algorithm','active-set','Display','off');
u_corr = quadprog(H, f, A, b, [],[],[],[],[],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;
@ -113,35 +129,36 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
put in matrix (Ax <= b) form put in matrix (Ax <= b) form
%} %}
% box constrains % box constraints
% A becomes sort of block-diagonal lb = [];
% A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2: ub = [];
% accounting for T_inv and -T_inv) by PREDICTION_HORIZON (number of
% vectors in u_corr times the number of elements [2] in each vector)
b_deq = [];
for k=1:pred_hor for k=1:pred_hor
T_inv = T_inv_pred(:,:,k); T_inv = T_inv_pred(:,:,k);
u_track = u_track_pred(:,:,k); u_track = u_track_pred(:,:,k);
d = T_inv*u_track; d = T_inv*u_track;
b_deq = [b_deq; s_ - d; s_ + d]; lb = [lb; -s_-d];
ub = [ub; s_-d];
end end
A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]); if eq(sim_data.costfun, 1)
%A_deq % minimize vcorr_r^2 + wcorr_l^2
%b_deq
% unknowns
% squared norm of u_corr. H must be identity, % squared norm of u_corr. H must be identity,
% PREDICTION_HORIZON*size(u_corr)
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, A_deq, b_deq, [],[],[],[],[],options); U_corr = quadprog(H, f, [], [], [],[], lb, ub, zeros(2*pred_hor,1), options);
%U_corr = lsqnonlin(@(pred_hor) ones(pred_hor, 1), U_corr_history(:,:,1), [], [], A_deq, b_deq, [], []);
% 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

2
plot_trajectory.m
View File

@ -5,7 +5,7 @@ function tl = plot_trajectory(t, ref, x)
hold on hold on
grid minor grid minor
label_interval = 1:1800:length(x); label_interval = 1:5000:length(x);
labels = strcat(num2str(round(t(label_interval),1)), "s"); labels = strcat(num2str(round(t(label_interval),1)), "s");
%title("Reference trajectory / Robot position", "FontSize", 18) %title("Reference trajectory / Robot position", "FontSize", 18)

12
tesi.m
View File

@ -3,9 +3,9 @@ clear all
close all close all
% options % options
ROBOT = 'unicycle' ROBOT = 'diffdrive'
TESTS = ["straightline/chill", "straightline/chill_errortheta_pisixths", "straightline/toofast", "straightline/chill_errory", "circle/start_center", "figure8/chill", "figure8/toofast", "square"] %TESTS = ["straightline/chill", "straightline/chill_errortheta_pisixths", "straightline/toofast", "straightline/chill_errory", "circle/start_center", "figure8/chill", "figure8/toofast", "square"]
%TESTS = ["circle/start_center"] TESTS = ["circle/start_center"]
% main % main
s_ = size(TESTS); s_ = size(TESTS);
@ -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=4
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 (3) spmd (2)
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']);
@ -64,7 +66,7 @@ for i = 1:length(TESTS)
% save simulation data % save simulation data
f1 = [ TEST '/' char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible name f1 = [ TEST '/' char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible name
f = ['results-' ROBOT '/' f1]; f = ['results-' ROBOT '-costfun-ddronly/' f1];
mkdir(f) mkdir(f)
% save workspace % save workspace
dsave([f '/workspace_composite.mat']); dsave([f '/workspace_composite.mat']);

46
video_all.m
View File

@ -5,25 +5,33 @@ disp('Waiting 5s')
pause(1) pause(1)
PLOT_TESTS = [ PLOT_TESTS = [
"results-diffdrive/straightline/chill/11-09-2024-16-57-01"; "results-diffdrive-costfun-ddronly/circle/start_center/ddr-minv-activeset-chattering"
"results-diffdrive/straightline/chill_errortheta_pisixths/11-09-2024-16-57-43"; "results-diffdrive-costfun-ddronly/circle/start_center/ddr-minw-activeset-chattering"
"results-diffdrive/straightline/chill_errory/11-09-2024-16-59-04"; "results-diffdrive-costfun-ddronly/circle/start_center/09-10-2024-14-14-49"
"results-diffdrive/straightline/toofast/11-09-2024-16-58-24"; "results-diffdrive-costfun/circle/start_center/ddr-minv-Rtinv-asymH-tc05"
"results-diffdrive/circle/start_center/11-09-2024-16-59-50"; "results-diffdrive-costfun/circle/start_center/ddr-minw-Rtinv-asymH-tc05"
"results-diffdrive/square/11-09-2024-17-06-14"; "results-unicycle-costfun/circle/start_center/uni-minv-Rtinv-tc05"
"results-diffdrive/figure8/chill/11-09-2024-17-00-53"; "results-unicycle-costfun/circle/start_center/uni-minw-Rtinv-tc05"
%"results-diffdrive/figure8/fancyreps/11-09-2024--45-28";
"results-diffdrive/figure8/toofast/11-09-2024-17-01-43";
"results-unicycle/straightline/chill/11-09-2024-17-07-51"; %"results-diffdrive/straightline/chill/11-09-2024-16-57-01";
"results-unicycle/straightline/chill_errortheta_pisixths/11-09-2024-17-08-35"; %"results-diffdrive/straightline/chill_errortheta_pisixths/11-09-2024-16-57-43";
"results-unicycle/straightline/chill_errory/11-09-2024-17-10-00"; %"results-diffdrive/straightline/chill_errory/11-09-2024-16-59-04";
"results-unicycle/straightline/toofast/11-09-2024-17-09-18"; %"results-diffdrive/straightline/toofast/11-09-2024-16-58-24";
"results-unicycle/circle/start_center/11-09-2024-17-10-48"; %"results-diffdrive/circle/start_center/11-09-2024-16-59-50";
"results-unicycle/square/11-09-2024-17-17-21"; %"results-diffdrive/square/11-09-2024-17-06-14";
"results-unicycle/figure8/chill/11-09-2024-17-11-53"; %"results-diffdrive/figure8/chill/11-09-2024-17-00-53";
%"results-diffdrive/figure8/fancyreps/11-09-2024--45-28";
%"results-diffdrive/figure8/toofast/11-09-2024-17-01-43";
%"results-unicycle/straightline/chill/11-09-2024-17-07-51";
%"results-unicycle/straightline/chill_errortheta_pisixths/11-09-2024-17-08-35";
%"results-unicycle/straightline/chill_errory/11-09-2024-17-10-00";
%"results-unicycle/straightline/toofast/11-09-2024-17-09-18";
%"results-unicycle/circle/start_center/11-09-2024-17-10-48";
%"results-unicycle/square/11-09-2024-17-17-21";
%"results-unicycle/figure8/chill/11-09-2024-17-11-53";
%"results-unicycle/figure8/fancyreps/11-09-2024--45-28"; %"results-unicycle/figure8/fancyreps/11-09-2024--45-28";
"results-unicycle/figure8/toofast/11-09-2024-17-12-45"; %"results-unicycle/figure8/toofast/11-09-2024-17-12-45";
] ]
s_ = size(PLOT_TESTS) s_ = size(PLOT_TESTS)
@ -37,9 +45,9 @@ for i = 1:s_(1)
dir = ['videos-' ROBOT '/', sPLOT_TEST, '/'] dir = ['videos-' ROBOT '/', sPLOT_TEST, '/']
mkdir(dir); mkdir(dir);
close all; pause(2); video(q{1}', ref_t{1}', Q_pred{1}, U_track{1}, U_corr{1}, 0, 0.12, t{1}, 4, sim_data{1}.tc*0.25, "track only"); %close all; pause(2); video(q{1}', ref_t{1}', Q_pred{1}, U_track{1}, U_corr{1}, 0, 0.12, t{1}, 4, sim_data{1}.tc*0.25, "track only");
close all; pause(2); video(q{2}', ref_t{2}', Q_pred{2}, U_track{2}, U_corr{2}, 1, 0.12, t{2}, 4, sim_data{2}.tc*0.25, "track and 1-step"); close all; pause(2); video(q{2}', ref_t{2}', Q_pred{2}, U_track{2}, U_corr{2}, 1, 0.12, t{2}, 4, sim_data{2}.tc*0.25, "track and 1-step");
close all; pause(2); video(q{3}', ref_t{3}', Q_pred{3}, U_track{3}, U_corr{3}, 3, 0.12, t{3}, 4, sim_data{3}.tc*0.25, "track and multistep"); %close all; pause(2); video(q{3}', ref_t{3}', Q_pred{3}, U_track{3}, U_corr{3}, 3, 0.12, t{3}, 4, sim_data{3}.tc*0.25, "track and multistep");
%movefile("*.gif", dir); %movefile("*.gif", dir);
movefile("*.avi", dir); movefile("*.avi", dir);