From 87df77e9b041aecd3dcaf5e69eed126950df760c Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Fri, 11 Oct 2024 18:05:44 +0200 Subject: [PATCH] costfun to minimize v^2+b^2w^2 --- control_act.m | 20 ++++++++++++++------ plot_trajectory.m | 2 +- tesi.m | 4 ++-- video_all.m | 46 +++++++++++++++++++++++++++------------------- 4 files changed, 44 insertions(+), 28 deletions(-) diff --git a/control_act.m b/control_act.m index ecbb4a9..e780493 100644 --- a/control_act.m +++ b/control_act.m @@ -32,14 +32,22 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data) elseif eq(pred_hor, 1) if eq(sim_data.costfun, 1) - % minimize vcorr_r^2 + wcorr_l^2 - H = eye(2); + % 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); + % 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); + % 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); diff --git a/plot_trajectory.m b/plot_trajectory.m index 5ba7ebf..66dcf06 100644 --- a/plot_trajectory.m +++ b/plot_trajectory.m @@ -5,7 +5,7 @@ function tl = plot_trajectory(t, ref, x) hold on grid minor - label_interval = 1:1800:length(x); + label_interval = 1:5000:length(x); labels = strcat(num2str(round(t(label_interval),1)), "s"); %title("Reference trajectory / Robot position", "FontSize", 18) diff --git a/tesi.m b/tesi.m index 4cc9254..74d5c99 100644 --- a/tesi.m +++ b/tesi.m @@ -29,14 +29,14 @@ for i = 1:length(TESTS) [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data); sim_data.ref = ref; sim_data.dref = dref; - sim_data.costfun=2 + sim_data.costfun=4 sim_data.tc=0.05; % spawn a new worker for each controller % 1: track only % 2: track + 1step % 3: track + multistep - spmd (3) + spmd (2) worker_index = spmdIndex; % load controller-specific options data = load(['tests/' num2str(worker_index) '.mat']); diff --git a/video_all.m b/video_all.m index ed121aa..0219179 100644 --- a/video_all.m +++ b/video_all.m @@ -5,25 +5,33 @@ disp('Waiting 5s') pause(1) PLOT_TESTS = [ - "results-diffdrive/straightline/chill/11-09-2024-16-57-01"; - "results-diffdrive/straightline/chill_errortheta_pisixths/11-09-2024-16-57-43"; - "results-diffdrive/straightline/chill_errory/11-09-2024-16-59-04"; - "results-diffdrive/straightline/toofast/11-09-2024-16-58-24"; - "results-diffdrive/circle/start_center/11-09-2024-16-59-50"; - "results-diffdrive/square/11-09-2024-17-06-14"; - "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-diffdrive-costfun-ddronly/circle/start_center/ddr-minv-activeset-chattering" + "results-diffdrive-costfun-ddronly/circle/start_center/ddr-minw-activeset-chattering" + "results-diffdrive-costfun-ddronly/circle/start_center/09-10-2024-14-14-49" + "results-diffdrive-costfun/circle/start_center/ddr-minv-Rtinv-asymH-tc05" + "results-diffdrive-costfun/circle/start_center/ddr-minw-Rtinv-asymH-tc05" + "results-unicycle-costfun/circle/start_center/uni-minv-Rtinv-tc05" + "results-unicycle-costfun/circle/start_center/uni-minw-Rtinv-tc05" - "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-diffdrive/straightline/chill/11-09-2024-16-57-01"; + %"results-diffdrive/straightline/chill_errortheta_pisixths/11-09-2024-16-57-43"; + %"results-diffdrive/straightline/chill_errory/11-09-2024-16-59-04"; + %"results-diffdrive/straightline/toofast/11-09-2024-16-58-24"; + %"results-diffdrive/circle/start_center/11-09-2024-16-59-50"; + %"results-diffdrive/square/11-09-2024-17-06-14"; + %"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/toofast/11-09-2024-17-12-45"; + %"results-unicycle/figure8/toofast/11-09-2024-17-12-45"; ] s_ = size(PLOT_TESTS) @@ -37,9 +45,9 @@ for i = 1:s_(1) dir = ['videos-' ROBOT '/', sPLOT_TEST, '/'] 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{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("*.avi", dir);