big refactor of branches into a single codebase
using flags to switch between tests and robot typesmaster
parent
f5a4bc0c51
commit
a9bf86cda8
|
|
@ -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
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -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
|
||||
Binary file not shown.
118
plot_all.m
118
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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
24
plot_input.m
24
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
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
44
tesi.m
44
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];
|
||||
|
|
|
|||
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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
|
||||
29
video_all.m
29
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");
|
||||
|
|
|
|||
Loading…
Reference in New Issue