diff --git a/control_act.m b/control_act.m index 99bf8fb..f655203 100644 --- a/control_act.m +++ b/control_act.m @@ -69,10 +69,14 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data) u_track_ = utrack(t_, q_act, sim_data); T_inv = decouple_matrix(q_act, sim_data); - % compute inputs (wr, wl) + % compute inputs (v, w)/(wr, wl) u_ = T_inv * u_track_ + u_corr_; - % map (wr, wl) to (v, w) for unicicle - u_ = diffdrive_to_uni(u_, sim_data); + + + % if needed, map (wr, wl) to (v, w) for unicicle + if eq(sim_data.robot, 1) + u_ = diffdrive_to_uni(u_, sim_data); + end % integrate unicycle theta_new = q_act(3) + tc*u_(2); @@ -175,14 +179,15 @@ function T_inv = decouple_matrix(q, sim_data) ct = cos(theta); b = sim_data.b; - r = sim_data.r; - d = sim_data.d; - %a1 = sim_data.r*0.5; - %a2 = sim_data.b*sim_data.r/sim_data.d; - %det_inv = -sim_data.d/(sim_data.b*sim_data.r*sim_data.r);0 - %T_inv = det_inv * [ a1*st - a2*ct, -a1*ct - a2*st;-a1*st-a2*ct , a1*ct - a2*st]; - T_inv = [2*b*ct - d*st, d*ct + 2*b*st ; 2*b*ct + d*st, -d*ct+2*b*st] / (2*b*r); + if eq(sim_data.robot, 0) + T_inv = [ct, st; -st/b, ct/b]; + elseif eq(sim_data.robot, 1) + r = sim_data.r; + d = sim_data.d; + + T_inv = [2*b*ct - d*st, d*ct + 2*b*st ; 2*b*ct + d*st, -d*ct+2*b*st] / (2*b*r); + end end diff --git a/sistema_discr.m b/diffdrive.m similarity index 77% rename from sistema_discr.m rename to diffdrive.m index 244db8c..c4c6e49 100644 --- a/sistema_discr.m +++ b/diffdrive.m @@ -1,4 +1,4 @@ -function dq = sistema_discr(t, q, u_discr, sim_data) +function dq = diffdrive(t, q, u_discr, sim_data) theta = q(3); %dq = T(q)*W*u diff --git a/disaster.m b/disaster.m new file mode 100644 index 0000000..689bb2e --- /dev/null +++ b/disaster.m @@ -0,0 +1,92 @@ +clc +clear all +close all + +%load('results-diffdrive/circle/start_center/10-09-2024 13-27-12/workspace_composite.mat') +%load('results-diffdrive/circle/start_center/10-09-2024 15-33-08/workspace_composite.mat') +%load('/home/emamaker/documents/Università/tesi/tesi-sim/results-diffdrive/square/10-09-2024 13-53-35/workspace_composite.mat') + +load('results-diffdrive/figure8/toofast/10-09-2024-22-35-17/workspace_composite.mat') + + + +y = cell(1,3); +for n = 1:3 + y{n} = [q{n}(:,1) + sim_data{n}.b*cos(q{n}(:,3)), q{n}(:,2) + sim_data{n}.b*sin(q{n}(:,3))]; +end + + +figure(1) +hold on +plot(ref_t{n}(:, 1), ref_t{n}(:, 2), "DisplayName", "Ref", "LineStyle", "--", 'LineWidth', 2) +for n=1:3 + plot(y{n}(:, 1), y{n}(:, 2), "DisplayName", ['state' num2str(n)], 'LineWidth', 2) +end +legend() +hold off + +%U_corr{1} = U{1} - U_track{1} +plot_input(2, t, U, 'sat') +plot_input(3, t, U_track, 'track') +plot_input(4, t, U_corr, 'corr') + +plot_traj(t, y, ref_t) + +function plot_input(nfig, t, input, name) + figure(nfig) + subplot(2,1,1) + hold on + plot(t{1}, input{1}(:,1), "DisplayName", ['w_r ' num2str(1) name], 'LineWidth', 2, 'LineStyle', '-.') + plot(t{2}, input{2}(:,1), "DisplayName", ['w_r ' num2str(2) name], 'LineWidth', 4, 'LineStyle', ':') + plot(t{3}, input{3}(:,1), "DisplayName", ['w_r ' num2str(3) name], 'LineWidth', 2, 'LineStyle', '--') + + subplot(2,1,2) + hold on + plot(t{1}, input{1}(:,2), "DisplayName", ['w_l ' num2str(1) name], 'LineWidth', 2, 'LineStyle', '-.') + plot(t{2}, input{2}(:,2), "DisplayName", ['w_l ' num2str(2) name], 'LineWidth', 4, 'LineStyle', ':') + plot(t{3}, input{3}(:,2), "DisplayName", ['w_l ' num2str(3) name], 'LineWidth', 2, 'LineStyle', '--') %{ + + %{ + for n=1:3 + subplot(2,1,1) + hold on + plot(t{n}, input{n}(:,1), "DisplayName", ['w_r ' num2str(n) name], 'LineWidth', 2, 'LineStyle', '-.') + subplot(2,1,2) + hold on + plot(t{n}, input{n}(:,2), "DisplayName", ['w_l ' num2str(n) name], 'LineWidth', 2, 'LineStyle', '-.') + + end + %} + subplot(2,1,1) + legend() + subplot(2,1,2) + legend + hold off +end + +function plot_traj(t, q, ref_t) + figure(10) + for n=1:3 + ref = ref_t{n}; + x = q{n}; + ex = ref(:, 1) - x(:, 1); + ey = ref(:, 2) - x(:, 2); + + subplot(2,2,1) + hold on + plot(t{n}, x(:,1), 'DisplayName', ['x ' num2str(n)], 'LineWidth', 2) + legend() + subplot(2,2,2) + hold on + plot(t{n}, x(:,2), 'DisplayName', ['y ' num2str(n)], 'LineWidth', 2) + legend() + subplot(2,2,3) + hold on + plot(t{n}, ex, 'DisplayName', ['errx ' num2str(n)], 'LineWidth', 2) + legend() + subplot(2,2,4) + hold on + plot(t{n}, ey, 'DisplayName', ['erry ' num2str(n)], 'LineWidth', 2) + legend() + end +end \ No newline at end of file diff --git a/disaster_circle.fig b/disaster_circle.fig new file mode 100644 index 0000000..0be4296 Binary files /dev/null and b/disaster_circle.fig differ diff --git a/plot_all.m b/plot_all.m index a544441..c318485 100644 --- a/plot_all.m +++ b/plot_all.m @@ -1,4 +1,4 @@ -<\clc +clc clear all close all @@ -8,14 +8,27 @@ disp('Photos will start in 3s') pause(3) PLOT_TESTS = [ - "results-diffdrive/straightline/chill/10-09-2024-22-30-28"; - "results-diffdrive/straightline/chill_errortheta_pisixths/10-09-2024-22-31-10"; - "results-diffdrive/straightline/chill_errory/10-09-2024-22-32-34"; - "results-diffdrive/circle/start_center/10-09-2024-22-33-21"; - "results-diffdrive/square/10-09-2024-22-40-01"; - "results-diffdrive/figure8/chill/10-09-2024-22-34-26"; - "results-diffdrive/figure8/fancyreps/10-09-2024-21-45-28"; - "results-diffdrive/figure8/toofast/10-09-2024-22-35-17"; + %{ + "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"; ] s_ = size(PLOT_TESTS) @@ -26,56 +39,61 @@ for i = 1:s_(1) PLOT_TEST = [sPLOT_TEST, '/workspace_composite.mat'] load(PLOT_TEST) - dir = ['images-diffdrive/', sPLOT_TEST, '/'] + dir = ['images-', ROBOT, '/', sPLOT_TEST, '/'] mkdir(dir); - for n=1:3 - clf; plot_trajectory(t{n}, ref_t{n}, q{n}) - export_fig(gcf, '-transparent', [dir, num2str(n), '_trajectory.png']) - clf; plot_error(t{n}, ref_t{n}, q{n}) - export_fig(gcf, '-transparent', [dir, num2str(n), '_error.png']) - clf; plot_doubleinput(t{n}, sim_data{n}.SATURATION, U_track{n}, U_corr{n}, 0) - export_fig(gcf, '-transparent', [dir, num2str(n), '_double_input_1x2.png']) - clf; plot_doubleinput(t{n}, sim_data{n}.SATURATION, U_track{n}, U_corr{n}, 1) - export_fig(gcf, '-transparent', [dir, num2str(n), '_double_input_2x1.png']) - clf; plot_tripleinput(t{n}, sim_data{n}.SATURATION, U{n}, U_track{n}, U_corr{n}, 0) - export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_1x2.png']) - clf; plot_tripleinput(t{n}, sim_data{n}.SATURATION, U{n}, U_track{n}, U_corr{n}, 1) - export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_2x1.png']) + in1 = sim_data{n}.input1_name; + in2 = sim_data{n}.input2_name; + m1 = sim_data{n}.m1; + m2 = sim_data{n}.m2; + + pause(1); clf; plot_trajectory(t{n}, ref_t{n}, q{n}) + pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_trajectory.png']) + pause(1); clf; plot_error(t{n}, ref_t{n}, q{n}) + pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_error.png']) + pause(1); clf; plot_doubleinput(t{n}, sim_data{n}.SATURATION, U_track{n}, U_corr{n}, 0, in1, in2, m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_double_input_1x2.png']) + pause(1); clf; plot_doubleinput(t{n}, sim_data{n}.SATURATION, U_track{n}, U_corr{n}, 1, in1, in2, m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_double_input_2x1.png']) + pause(1); clf; plot_tripleinput(t{n}, sim_data{n}.SATURATION, U{n}, U_track{n}, U_corr{n}, 0, in1, in2, m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_1x2.png']) + pause(1); clf; plot_tripleinput(t{n}, sim_data{n}.SATURATION, U{n}, U_track{n}, U_corr{n}, 1, in1, in2, m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_triple_input_2x1.png']) - %clf; plot_input(t{n}, sim_data{n}.SATURATION, U_track{n}, 'track') - %export_fig(gcf, '-transparent', [dir, num2str(n), '_track_input.png']) - %clf; plot_input(t{n}, sim_data{n}.SATURATION, U_corr{n}, 'corr') - %export_fig(gcf, '-transparent', [dir, num2str(n), '_corr_input.png']) + %pause(1); clf; plot_input(t{n}, sim_data{n}.SATURATION, U_track{n}, 'track') + %pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_track_input.png']) + %pause(1); clf; plot_input(t{n}, sim_data{n}.SATURATION, U_corr{n}, 'corr') + %pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_corr_input.png']) - clf; plot_input(t{n},sim_data{n}.SATURATION, U{n}, '', 0) - export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input_1x2.png']) - clf; plot_input(t{n},sim_data{n}.SATURATION, U{n}, '', 1) - export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input_2x1.png']) + pause(1); clf; plot_input(t{n},sim_data{n}.SATURATION, U{n}, 0, '', in1, in2, m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input_1x2.png']) + pause(1); clf; plot_input(t{n},sim_data{n}.SATURATION, U{n}, 1, '', in1, in2, m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input_2x1.png']) end % correction difference (multistep, 1-step) - clf; plot_input_diff(t{3}, t{2}, U_corr{3}, U_corr{2}, 0, '\textbf{$$\omega_r^{corr, multistep}$$}', '\textbf{$$\omega_r^{corr, 1step}$$}', '\textbf{$$\omega_l^{corr, multistep}$$}', '\textbf{$$\omega_l^{corr, 1step}$$}') - export_fig(gcf, '-transparent', [dir, 'corr_input_diff_1x2.eps']) - clf; plot_input_diff(t{3}, t{2}, U_corr{3}, U_corr{2}, 1, '\textbf{$$\omega_r^{corr, multistep}$$}', '\textbf{$$\omega_r^{corr, 1step}$$}', '\textbf{$$\omega_l^{corr, multistep}$$}', '\textbf{$$\omega_l^{corr, 1step}$$}') - export_fig(gcf, '-transparent', [dir, 'corr_input_diff_2x1.eps']) - - % input difference (saturated track only, 1step) - clf; plot_input_diff(t{1}, t{2}, U{1}, U{2}, 0, '\textbf{$$\omega_r^{trackonly_sat}$$}', '\textbf{$$\omega_r^{1step}$$}', '\textbf{$$\omega_l^{trackonly-sat}$$}', '\textbf{$$\omega_l^{1step}$$}') - export_fig(gcf, '-transparent', [dir, 'input_diff_track_1step_1x2.eps']) - clf; plot_input_diff(t{1}, t{2}, U{1}, U{2}, 1, '\textbf{$$\omega_r^{trackonly_sat}$$}', '\textbf{$$\omega_r^{1step}$$}', '\textbf{$$\omega_l^{trackonly-sat}$$}', '\textbf{$$\omega_l^{1step}$$}') - export_fig(gcf, '-transparent', [dir, 'input_diff_track_1step_2x1.eps']) + pause(1); clf; plot_input_diff(t{3}, t{2}, U_corr{3}, U_corr{2}, 0, ['\textbf{$$' in1 '^{corr, multistep}$$}'], ['\textbf{$$' in1 '^{corr, 1step}$$}'], ['\textbf{$$' in2 '^{corr, multistep}$$}'], ['\textbf{$$' in2 '^{corr, 1step}$$}'], m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, 'corr_input_diff_1x2.png']) + pause(1); clf; plot_input_diff(t{3}, t{2}, U_corr{3}, U_corr{2}, 1, ['\textbf{$$' in1 '^{corr, multistep}$$}'], ['\textbf{$$' in1 '^{corr, 1step}$$}'], ['\textbf{$$' in2 '^{corr, multistep}$$}'], ['\textbf{$$' in2 '^{corr, 1step}$$}'], m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, 'corr_input_diff_2x1.png']) + % input difference (saturated track only, 1-step) + pause(1); clf; plot_input_diff(t{1}, t{2}, U{1}, U{2}, 0, ['\textbf{$$' in1 '^{trackonly-sat}$$}'], ['\textbf{$$' in1 '^{1step}$$}'], ['\textbf{$$' in2 '^{trackonly-sat}$$}'], ['\textbf{$$' in2 '^{1step}$$}'], m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_track_1step_1x2.png']) + pause(1); clf; plot_input_diff(t{1}, t{2}, U{1}, U{2}, 1, ['\textbf{$$' in1 '^{trackonly-sat}$$}'], ['\textbf{$$' in1 '^{1step}$$}'], ['\textbf{$$' in2 '^{trackonly-sat}$$}'], ['\textbf{$$' in2 '^{1step}$$}'], m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_track_1step_2x1.png']) + % input difference (saturated track only, multistep) - clf; plot_input_diff(t{1}, t{3}, U{1}, U{3}, 0, '\textbf{$$\omega_r^{trackonly_sat}$$}', '\textbf{$$\omega_r^{multistep}$$}', '\textbf{$$\omega_l^{1step}$$}', '\textbf{$$\omega_l^{multistep}$$}') - export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_1x2.eps']) - clf; plot_input_diff(t{1}, t{3}, U{1}, U{3}, 1, '\textbf{$$\omega_r^{trackonly_sat}$$}', '\textbf{$$\omega_r^{multistep}$$}', '\textbf{$$\omega_l^{1step}$$}', '\textbf{$$\omega_l^{multistep}$$}') - export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_2x1.eps']) - + pause(1); clf; plot_input_diff(t{1}, t{3}, U{1}, U{3}, 0, ['\textbf{$$' in1 '^{trackonly-sat}$$}'], ['\textbf{$$' in1 '^{multistep}$$}'], ['\textbf{$$' in2 '^{trackonly-sat}$$}'], ['\textbf{$$' in2 '^{multistep}$$}'], m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_track_multistep_1x2.png']) + pause(1); clf; plot_input_diff(t{1}, t{3}, U{1}, U{3}, 1, ['\textbf{$$' in1 '^{trackonly-sat}$$}'], ['\textbf{$$' in1 '^{multistep}$$}'], ['\textbf{$$' in2 '^{trackonly-sat}$$}'], ['\textbf{$$' in2 '^{multistep}$$}'], m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_track_multistep_2x1.png']) + % input difference (1-step, multistep) - clf; plot_input_diff(t{2}, t{3}, U{2}, U{3}, 0, '\textbf{$$\omega_r^{1step}$$}', '\textbf{$$\omega_r^{multistep}$$}', '\textbf{$$\omega_l^{1step}$$}', '\textbf{$$\omega_l^{multistep}$$}') - export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_1x2.eps']) - clf; plot_input_diff(t{2}, t{3}, U{2}, U{3}, 1, '\textbf{$$\omega_r^{1step}$$}', '\textbf{$$\omega_r^{multistep}$$}', '\textbf{$$\omega_l^{1step}$$}', '\textbf{$$\omega_l^{multistep}$$}') - export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_2x1.eps']) + pause(1); clf; plot_input_diff(t{2}, t{3}, U{2}, U{3}, 0, ['\textbf{$$' in1 '^{1step}$$}'], ['\textbf{$$' in1 '^{multistep}$$}'], ['\textbf{$$' in2 '^{1step}$$}'], ['\textbf{$$' in2 '^{multistep}$$}'], m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_1x2.png']) + pause(1); clf; plot_input_diff(t{2}, t{3}, U{2}, U{3}, 1, ['\textbf{$$' in1 '^{1step}$$}'], ['\textbf{$$' in1 '^{multistep}$$}'], ['\textbf{$$' in2 '^{1step}$$}'], ['\textbf{$$' in2 '^{multistep}$$}'], m1, m2) + pause(0.5); export_fig(gcf, '-transparent', [dir, 'input_diff_1step_multistep_2x1.png']) + end \ No newline at end of file diff --git a/plot_doubleinput.m b/plot_doubleinput.m index d348900..7bf5d86 100644 --- a/plot_doubleinput.m +++ b/plot_doubleinput.m @@ -1,9 +1,10 @@ -function plot_doubleinput(t, sat, U_track, U_corr, type) +function plot_doubleinput(t, sat, U_track, U_corr, type, in1, in2, m1, m2) tiledlayout(1,1,'Padding','tight', 'TileSpacing','compact') nexttile -ylim_ = 7; +ylim1_ = sat(1)*2.5; +ylim2_ = sat(2)*2.5; lw = 6; if type == 0 @@ -13,14 +14,16 @@ subplot(2,1,1); end hold on -plot(t, U_track(:, 1), 'Linewidth', lw, 'DisplayName', '\omega_r^{track}'); -plot(t, U_corr(:, 1), 'Linewidth', lw, 'DisplayName', '\omega_r^{corr}'); +plot(t, U_track(:, 1), 'Linewidth', lw-1, 'DisplayName', [in1 '^{track}'], 'Color', 'cyan'); +plot(t, U_corr(:, 1), 'Linewidth', lw-1, 'DisplayName', [in1 '^{corr}'], 'Color', 'green'); legend('FontSize', 18, 'Location', 'northeast', 'AutoUpdate','off') -plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); -plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); +plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); +plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); xlabel('\textbf{t[s]}', 'FontSize', 22, 'Interpreter','latex'); +ylabel(['\textbf{[', m1 ']}'], 'Interpreter','latex'); + hold off @@ -29,9 +32,7 @@ Axes.FontSize=22; Axes.FontWeight='bold'; grid minor; Axes.PlotBoxAspectRatio = [1 1 1]; -ylim([-ylim_, ylim_]) - - +ylim([-ylim1_, ylim1_]) if type == 0 subplot(1,2,2); @@ -40,21 +41,22 @@ subplot(2,1,2); end hold on -plot(t, U_track(:, 2), 'Linewidth', lw, 'DisplayName', '\omega_l^{track}'); -plot(t, U_corr(:, 2), 'Linewidth', lw, 'DisplayName', '\omega_l^{corr}'); +plot(t, U_track(:, 2), 'Linewidth', lw-1, 'DisplayName', [in2 '^{track}'], 'Color', 'cyan'); +plot(t, U_corr(:, 2), 'Linewidth', lw-1, 'DisplayName', [in2 '^{corr}'], 'Color', 'green'); legend('FontSize', 18, 'Location', 'northeast', 'AutoUpdate','off') -plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); -plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); +plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); +plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); xlabel('\textbf{t[s]}', 'Interpreter','latex'); +ylabel(['\textbf{[', m2 ']}'], 'Interpreter','latex'); Axes = gca; Axes.FontSize=22; Axes.FontWeight='bold'; grid minor; Axes.PlotBoxAspectRatio = [1 1 1]; -ylim([-ylim_, ylim_]) +ylim([-ylim2_, ylim2_]) hold off diff --git a/plot_input.m b/plot_input.m index fb8e9bf..8fa72ad 100644 --- a/plot_input.m +++ b/plot_input.m @@ -1,4 +1,4 @@ -function plot_input(t, sat, U, type, orient) +function plot_input(t, sat, U, orient, type, in1, in2, m1, m2) tiledlayout(1,1,'Padding','tight', 'TileSpacing','compact') @@ -10,17 +10,17 @@ else subplot(2,1,1); end - -ylim_ = 7; +ylim1_ = sat(1)*2.5; +ylim2_ = sat(2)*2.5; hold on plot(t, U(:, 1), 'Linewidth', 6); -plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--'); -plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--'); +plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); +plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); xlabel('\textbf{t[s]}', 'Interpreter','latex'); -ylabel(['\textbf{$$\omega_r^{' type '}$$[rad/s]}'], 'Interpreter','latex'); +ylabel(['\textbf{$$' in1 '^{' type '}$$ [' m1 ']}'], 'Interpreter','latex'); hold off @@ -29,8 +29,7 @@ Axes.FontSize=26; Axes.FontWeight='bold'; grid minor; Axes.PlotBoxAspectRatio = [1 1 1]; -ylim([-ylim_, ylim_]) - +ylim([-ylim1_, ylim1_]) if orient == 0 subplot(1,2,2); @@ -41,11 +40,11 @@ end hold on plot(t, U(:, 2), 'Linewidth', 6); -plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); -plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); +plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); +plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); xlabel('\textbf{t[s]}', 'Interpreter','latex'); -ylabel(['\textbf{$$\omega_l^{' type '}$$[rad/s]}'], 'Interpreter','latex'); +ylabel(['\textbf{$$' in2 '^{' type '}$$ [' m2 ']}'], 'Interpreter','latex'); hold off Axes = gca; @@ -53,7 +52,6 @@ Axes.FontSize=26; Axes.FontWeight='bold'; grid minor; Axes.PlotBoxAspectRatio = [1 1 1]; -ylim([-ylim_, ylim_]) - +ylim([-ylim2_, ylim2_]) end \ No newline at end of file diff --git a/plot_input_diff.m b/plot_input_diff.m index 4dee5d9..af8d0f4 100644 --- a/plot_input_diff.m +++ b/plot_input_diff.m @@ -1,4 +1,4 @@ -function plot_input_diff(t1, t2, U_corr1, U_corr2, type, name1_1, name1_2, name2_1, name2_2) +function plot_input_diff(t1, t2, U_corr1, U_corr2, type, name1_1, name1_2, name2_1, name2_2, m1, m2) l1 = length(U_corr1); l2 = length(U_corr2) @@ -22,7 +22,7 @@ subplot(2,1,1); end plot(t, U_corr1(:, 1) - U_corr2(:,1), 'Linewidth', 5); xlabel('\textbf{t[s]}', 'Interpreter','latex'); -ylabel([name1_1 '-' name1_2], 'Interpreter','latex'); +ylabel([name1_1 '-' name1_2 ' [' m1 ']'], 'Interpreter','latex'); Axes = gca; Axes.FontSize=26; @@ -38,7 +38,7 @@ subplot(2,1,2); end plot(t, U_corr1(:, 2) - U_corr2(:,2), 'Linewidth', 5); xlabel('\textbf{t[s]}', 'Interpreter','latex'); -ylabel([name2_1 '-' name2_2], 'Interpreter','latex'); +ylabel([name2_1 '-' name2_2 ' [' m2 ']'], 'Interpreter','latex'); Axes = gca; Axes.FontSize=26; diff --git a/plot_tripleinput.m b/plot_tripleinput.m index ba0bf16..e900870 100644 --- a/plot_tripleinput.m +++ b/plot_tripleinput.m @@ -1,9 +1,10 @@ -function plot_tripleinput(t, sat, U, U_track, U_corr, type) +function plot_tripleinput(t, sat, U, U_track, U_corr, type, in1, in2, m1, m2) %tiledlayout(1,1,'Padding','tight', 'TileSpacing','compact') %nexttile -ylim_ = 7; +ylim1_ = sat(1)*2.5; +ylim2_ = sat(2)*2.5; lw = 6; if type == 0 @@ -13,15 +14,16 @@ subplot(2,1,1); end hold on -plot(t, U(:, 1), 'Linewidth', lw, 'DisplayName', '\omega_r'); -plot(t, U_track(:, 1), 'Linewidth', lw, 'DisplayName', '\omega_r^{track}', 'Linestyle', ':'); -plot(t, U_corr(:, 1), 'Linewidth', lw, 'DisplayName', '\omega_r^{corr}', 'Linestyle', ':'); +plot(t, U_track(:, 1), 'Linewidth', lw-1, 'DisplayName', [in1 '^{track}'], 'Color', 'cyan'); +plot(t, U_corr(:, 1), 'Linewidth', lw-1, 'DisplayName', [in1 '^{corr}'], 'Color', 'green'); +plot(t, U(:, 1), 'Linewidth', lw, 'DisplayName', in1, 'Color', 'red'); legend('FontSize', 18, 'Location', 'northeast', 'AutoUpdate','off') xlabel('\textbf{t[s]}', 'Interpreter','latex'); +ylabel(['\textbf{[', m1 ']}'], 'Interpreter','latex'); -plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); -plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); +plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); +plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); hold off @@ -31,7 +33,7 @@ Axes.FontSize=22; Axes.FontWeight='bold'; grid minor; Axes.PlotBoxAspectRatio = [1 1 1]; -ylim([-ylim_, ylim_]) +ylim([-ylim1_, ylim1_]) if type == 0 @@ -41,15 +43,16 @@ subplot(2,1,2); end hold on -plot(t, U(:, 2), 'Linewidth', lw, 'DisplayName', '\omega_l'); -plot(t, U_track(:, 2), 'Linewidth', lw, 'DisplayName', '\omega_l^{track}', 'Linestyle', ':'); -plot(t, U_corr(:, 2), 'Linewidth', lw, 'DisplayName', '\omega_l^{corr}', 'Linestyle', ':'); +plot(t, U_track(:, 2), 'Linewidth', lw-1, 'DisplayName', [in2 '^{track}'], 'Color', 'cyan'); +plot(t, U_corr(:, 2), 'Linewidth', lw-1, 'DisplayName', [in2 '^{corr}'], 'Color', 'green'); +plot(t, U(:, 2), 'Linewidth', lw, 'DisplayName', in2, 'Color', 'red'); legend('FontSize', 18, 'Location', 'northeast', 'AutoUpdate','off') xlabel('\textbf{t[s]}', 'Interpreter','latex'); +ylabel(['\textbf{[', m2 ']}'], 'Interpreter','latex'); -plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); -plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2, 'Color', 'black'); +plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); +plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black'); Axes = gca; @@ -57,7 +60,7 @@ Axes.FontSize=22; Axes.FontWeight='bold'; grid minor; Axes.PlotBoxAspectRatio = [1 1 1]; -ylim([-ylim_, ylim_]) +ylim([-ylim2_, ylim2_]) hold off end \ No newline at end of file diff --git a/tesi.m b/tesi.m index f294b1d..bfc4537 100644 --- a/tesi.m +++ b/tesi.m @@ -2,39 +2,59 @@ clc clear all close all +% options +ROBOT = 'unicycle' TESTS = ["straightline/chill", "straightline/chill_errortheta_pisixths", "straightline/toofast", "straightline/chill_errory", "circle/start_center", "figure8/chill", "figure8/toofast", "square"] +%TESTS = ["circle/start_center"] +% main s_ = size(TESTS); - for i = 1:length(TESTS) - clearvars -except i s_ TESTS + clearvars -except i s_ TESTS ROBOT close all + % load simulation parameters common to all robots and all tests sim_data = load(["tests/robot_common.mat"]); TEST = convertStringsToChars(TESTS(i)) + % load test data (trajectory, etc) test_data = load(['tests/' TEST '/common.mat']); for fn = fieldnames(test_data)' sim_data.(fn{1}) = test_data.(fn{1}); end + % set trajectory and starting conditions sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS); [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data); sim_data.ref = ref; sim_data.dref = dref; - + + % spawn a new worker for each controller + % 1: track only + % 2: track + 1step + % 3: track + multistep spmd (3) worker_index = spmdIndex; + % load controller-specific options data = load(['tests/' num2str(worker_index) '.mat']); - + for fn = fieldnames(data)' + sim_data.(fn{1}) = data.(fn{1}); + end + + % load robot-specific options + % put here to overwrite any parameter value left over in the tests + % .mat files, just in case + data = load(['tests/' ROBOT '.mat']); for fn = fieldnames(data)' sim_data.(fn{1}) = data.(fn{1}); end + % initialize prediction horizon sim_data.U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON); sim_data + % simulate robot tic; [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simulate_discr(sim_data); toc; @@ -43,8 +63,8 @@ for i = 1:length(TESTS) end % save simulation data - f1 = [ TEST '/' char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible - f = ['results-diffdrive/' f1]; + f1 = [ TEST '/' char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible name + f = ['results-' ROBOT '/' f1]; mkdir(f) % save workspace dsave([f '/workspace_composite.mat']); @@ -64,11 +84,11 @@ for i = 1:length(TESTS) subplot(2,1,1) plot(t{2}, U_corr{2}(:, 1) - U_corr{3}(:, 1)) xlabel('t') - ylabel('difference on w_r between 1-step and multistep') + ylabel(['difference on ' sim_data{1}.input1_name ' between 1-step and multistep']) subplot(2,1,2) plot(t{2}, U_corr{2}(:, 2) - U_corr{3}(:, 2)) xlabel('t') - ylabel('difference on w_l between 1-step and multistep') + ylabel(['difference on ' sim_data{1}.input2_name ' between 1-step and multistep']) % save figures savefig(h, [f '/figure.fig']); @@ -97,6 +117,12 @@ function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = sim U_track = u_track'; Q_pred(:, :, 1) = q_pred; y = []; + + if eq(sim_data.robot, 0) + fun = @(t, q, u_discr, sim_data) unicycle(t, q, u_discr, sim_data); + elseif eq(sim_data.robot, 1) + fun = @(t, q, u_discr, sim_data) diffdrive(t, q, u_discr, sim_data); + end for n = 1:steps sim_data.old_u_corr = u_corr; @@ -107,7 +133,7 @@ function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = sim z0 = q(end, :); opt = odeset('MaxStep', 0.005); - [v, z] = ode45(@(v, z) sistema_discr(v, z, u_discr, sim_data), tspan, z0, opt); + [v, z] = ode45(@(v, z) fun(v, z, u_discr, sim_data), tspan, z0, opt); q = [q; z]; t = [t; v]; diff --git a/tests/diffdrive.mat b/tests/diffdrive.mat new file mode 100644 index 0000000..c34fd8a Binary files /dev/null and b/tests/diffdrive.mat differ diff --git a/tests/robot_common.mat b/tests/robot_common.mat index 6d63e8e..cd8432a 100644 Binary files a/tests/robot_common.mat and b/tests/robot_common.mat differ diff --git a/tests/unicycle.mat b/tests/unicycle.mat new file mode 100644 index 0000000..ef454d7 Binary files /dev/null and b/tests/unicycle.mat differ diff --git a/unicycle.m b/unicycle.m new file mode 100644 index 0000000..78911e3 --- /dev/null +++ b/unicycle.m @@ -0,0 +1,8 @@ +function dq = unicycle(t, q, u_discr, sim_data) + theta = q(3); + %dq = T(q)*W*u + + % T_q (unicycle) + T_q = [cos(theta), 0; sin(theta), 0; 0, 1]; + dq = T_q*u_discr; +end diff --git a/video_all.m b/video_all.m index b541425..ed121aa 100644 --- a/video_all.m +++ b/video_all.m @@ -5,14 +5,25 @@ disp('Waiting 5s') pause(1) PLOT_TESTS = [ - "results-diffdrive/straightline/chill/10-09-2024-22-30-28"; - "results-diffdrive/straightline/chill_errortheta_pisixths/10-09-2024-22-31-10"; - "results-diffdrive/straightline/chill_errory/10-09-2024-22-32-34"; - "results-diffdrive/circle/start_center/10-09-2024-22-33-21"; - "results-diffdrive/square/10-09-2024-22-40-01"; - "results-diffdrive/figure8/chill/10-09-2024-22-34-26"; - "results-diffdrive/figure8/fancyreps/10-09-2024-21-45-28"; - "results-diffdrive/figure8/toofast/10-09-2024-22-35-17"; + "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"; ] s_ = size(PLOT_TESTS) @@ -23,7 +34,7 @@ for i = 1:s_(1) PLOT_TEST = [sPLOT_TEST, '/workspace_composite.mat'] load(PLOT_TEST) - dir = ['videos-diffdrive/', sPLOT_TEST, '/'] + 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");