also plot trajectory and error relative to output position

using flags to switch between tests and robot types

control_act.m

@@ -4,7 +4,8 @@ function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data)
     [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
 
     ut = dc*ut;
-    uc = dc*uc;
+    %uc = dc*uc;
+    %uc = zeros(2,1);
 
     u = ut+uc;
     % saturation
@@ -25,113 +26,137 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
 
     q_pred = [];
 
+    s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
     if eq(pred_hor, 0)
         return
-    end
-    
-    if pred_hor > 1
-        % move the horizon over 1 step and add trailing zeroes
-        U_corr_history = cat(3, sim_data.U_corr_history(:,:, 2:end), zeros(2,1));
-    end
-
-    %disp('start of simulation')
-    % for each step in the prediction horizon, integrate the system to
-    % predict its future state
-
-    for k = 1:pred_hor
-        % start from the old (known) state
-
-        % compute the inputs, based on the old state
-
-        % u_corr is the prediction done at some time in the past, as found in U_corr_history
-        u_corr_ = U_corr_history(:, :, k);
-        % u_track can be computed from q
-        t_ = t + tc * (k-1);
-        u_track_ = utrack(t_, q_act, sim_data);
-        
+    elseif eq(pred_hor, 1)
+        H = eye(2);
+        f = zeros(2,1);
         T_inv = decouple_matrix(q_act, sim_data);
-        % compute inputs (wr, wl)
-        u_ = T_inv * (u_corr_ + u_track_);
-        % map (wr, wl) to (v, w) for unicicle
-        u_ = diffdrive_to_uni(u_, sim_data);
+        ut = utrack(t, q_act, sim_data);
+        %A = [T_inv; -T_inv];        
+        A = [eye(2); -eye(2)];
 
-        % integrate unicycle
-        theta_new = q_act(3) + tc*u_(2);
-        % compute the state integrating with euler
-        %x_new = q_act(1) + tc*u_(1) * cos(q_act(3));
-        %y_new = q_act(2) + tc*u_(1) * sin(q_act(3));
-        % compute the state integrating via runge-kutta
-        x_new = q_act(1) + tc*u_(1) * cos(q_act(3) + 0.5*tc*u_(2));
-        y_new = q_act(2) + tc*u_(1) * sin(q_act(3) + 0.5*tc*u_(2));
-
-        q_new = [x_new; y_new; theta_new];
-
-        % save history
-        q_pred = [q_pred; q_new'];
-        u_track_pred(:,:,k) = u_track_;
-        T_inv_pred(:,:,k) = T_inv;
-
-        % Prepare old state for next iteration
-        q_act = q_new;
-    end
-
-    %{
-        Now setup the qp problem
-        It needs:
-        - Unknowns, u_corr at each timestep. Will be encoded as a vector of
-        vectors, in which every two elements is a u_j
-        i.e. (u_1; u_2; u_3; ...; u_C) = (v_1; w_1; v_2, w_2; v_3, w_3; ...
-        ; v_C, w_C)
-        It is essential that the vector stays a column, so that u'u is the
-        sum of the squared norms of each u_j
-
-        - Box constraints: a constraint for each timestep in the horizon.
-        Calculated using the predicted state and inputs. They need to be
-        put in matrix (Ax <= b) form
-    %}
-
-    % box constrains
-    % A becomes sort of block-diagonal
-    % A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2:
-    % 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)
-    A_deq = [];
-    b_deq = [];
-
-    s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
-    for k=1:pred_hor
-        T_inv = T_inv_pred(:,:,k);
-        u_track = u_track_pred(:,:,k);
-
-        % [T_inv; -T_inv] is a 4x2 matrix
-        A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
-
-        d = T_inv*u_track;
-        b_deq = [b_deq; s_ - d; s_ + d];
-    end
+        d = T_inv*ut;
+        b = [s_-d;s_+d];
+        
+        % solve qp problem
+        options = optimoptions('quadprog', 'Display', 'off');
+        u_corr = quadprog(H, f, A, b, [],[],[],[],[],options);
+        
+        q_pred = q_act;
+        U_corr_history(:,:,1) = u_corr;
+        return
+    else
+        %if pred_hor > 1
+            % move the horizon over 1 step and add trailing zeroes
+            U_corr_history = cat(3, sim_data.U_corr_history(:,:, 2:end), zeros(2,1));
+        %end
     
-    %A_deq
-    %b_deq
-    % unknowns
-   
-    % squared norm of u_corr. H must be identity,
-    % PREDICTION_HORIZON*size(u_corr)
-    H = eye(pred_hor*2);
-    % no linear terms
-    f = zeros(pred_hor*2, 1);
-
-    % solve qp problem
-    options = optimoptions('quadprog', 'Display', 'off');
-    U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],[],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
-    % u_corr_j as a 2x1 vector
-    % and add the prediction at t_k+C
-    U_corr_history = reshape(U_corr, [2,1,pred_hor]);
-    % first result is what to do now
-    u_corr=U_corr_history(:,:, 1);
+        %disp('start of simulation')
+        % for each step in the prediction horizon, integrate the system to
+        % predict its future state
     
+        for k = 1:pred_hor
+            % start from the old (known) state
+    
+            % compute the inputs, based on the old state
+    
+            % u_corr is the prediction done at some time in the past, as found in U_corr_history
+            u_corr_ = U_corr_history(:, :, k);
+            % u_track can be computed from q
+            t_ = t + tc * (k-1);
+            u_track_ = utrack(t_, q_act, sim_data);
+            
+            T_inv = decouple_matrix(q_act, sim_data);
+            % compute inputs (v, w)/(wr, wl)
+            u_ = T_inv * u_track_ + u_corr_;
+            
+
+            % 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);
+            % compute the state integrating with euler
+            %x_new = q_act(1) + tc*u_(1) * cos(q_act(3));
+            %y_new = q_act(2) + tc*u_(1) * sin(q_act(3));
+            % compute the state integrating via runge-kutta
+            x_new = q_act(1) + tc*u_(1) * cos(q_act(3) + 0.5*tc*u_(2));
+            y_new = q_act(2) + tc*u_(1) * sin(q_act(3) + 0.5*tc*u_(2));
+    
+            q_new = [x_new; y_new; theta_new];
+    
+            % save history
+            q_pred = [q_pred; q_new'];
+            u_track_pred(:,:,k) = u_track_;
+            T_inv_pred(:,:,k) = T_inv;
+    
+            % Prepare old state for next iteration
+            q_act = q_new;
+        end
+    
+        %{
+            Now setup the qp problem
+            It needs:
+            - Unknowns, u_corr at each timestep. Will be encoded as a vector of
+            vectors, in which every two elements is a u_j
+            i.e. (u_1; u_2; u_3; ...; u_C) = (v_1; w_1; v_2, w_2; v_3, w_3; ...
+            ; v_C, w_C)
+            It is essential that the vector stays a column, so that u'u is the
+            sum of the squared norms of each u_j
+    
+            - Box constraints: a constraint for each timestep in the horizon.
+            Calculated using the predicted state and inputs. They need to be
+            put in matrix (Ax <= b) form
+        %}
+    
+        % box constrains
+        % A becomes sort of block-diagonal
+        % A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2:
+        % 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)
+        A_deq = [];
+        b_deq = [];
+    
+        s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
+        for k=1:pred_hor
+            T_inv = T_inv_pred(:,:,k);
+            u_track = u_track_pred(:,:,k);
+    
+            % [T_inv; -T_inv] is a 4x2 matrix
+            %A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
+            A_deq = blkdiag(A_deq, [eye(2); -eye(2)]);
+
+    
+            d = T_inv*u_track;
+            b_deq = [b_deq; s_ - d; s_ + d];
+        end
+        
+        %A_deq
+        %b_deq
+        % unknowns
+       
+        % squared norm of u_corr. H must be identity,
+        % PREDICTION_HORIZON*size(u_corr)
+        H = eye(pred_hor*2)*2;
+        % no linear terms
+        f = zeros(pred_hor*2, 1);
+    
+        % solve qp problem
+        options = optimoptions('quadprog', 'Display', 'off');
+        U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],[],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
+        % u_corr_j as a 2x1 vector
+        % and add the prediction at t_k+C
+        U_corr_history = reshape(U_corr, [2,1,pred_hor]);
+        % first result is what to do now
+        u_corr=U_corr_history(:,:, 1);
+    end
 end
 
 function u_track = utrack(t, q, sim_data)    
@@ -154,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
 
 

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
 

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

plot_all.m

@@ -8,21 +8,25 @@ disp('Photos will start in 3s')
 pause(3)
 
 PLOT_TESTS = [
-    "results-diffdrive/straightline/chill/01-Aug-2024 15-34-03";
-    "results-diffdrive/straightline/chill_errortheta_pisixths/01-Aug-2024 15-56-36";
-    "results-diffdrive/square/01-Aug-2024 16-18-51";
-    "results-diffdrive/circle/start_center/01-Aug-2024 16-46-41";
-    "results-diffdrive/circle/start_tangent/01-Aug-2024 16-55-09";
-    "results-diffdrive/circle/toofast/01-Aug-2024 17-35-25";
-    "results-diffdrive/straightline/toofast/01-Aug-2024 15-37-48";
-    "results-diffdrive/figure8/chill/15-Aug-2024 09-16-21";
-    "results-diffdrive/figure8/toofast/15-Aug-2024 09-10-32";
-    "results-diffdrive/straightline/abrupt_stop_chill/27-Aug-2024 10-15-43";
-    "results-diffdrive/straightline/abrupt_stop_toofast/27-Aug-2024 10-44-35";
-    "results-diffdrive/sin/no_start_error/27-Aug-2024 19-28-17";
-    "results-diffdrive/sin/no_start_error/27-Aug-2024 19-29-42";
-    "results-diffdrive/sin/no_start_error/27-Aug-2024 19-31-17";
-    "results-diffdrive/sin/no_start_error/27-Aug-2024 19-38-03";
+    "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)
@@ -33,54 +37,65 @@ 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.eps'])
-        clf; plot_error(t{n}, ref_t{n}, q{n})
-        export_fig(gcf, '-transparent', [dir, num2str(n), '_error.eps'])
-        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.eps'])
-        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.eps'])
-        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.eps'])
-        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.eps'])
-        
-        %print([dir, num2str(n), '_error.png'], '-dpng')
-        %clf; plot_input(t{n}, sim_data{n}.SATURATION, U_track{n}, 'track')
-        %export_fig(gcf, '-transparent', [dir, num2str(n), '_track_input.eps'])
-        %print([dir, num2str(n), '_track_input.png'], '-dpng')
-        %clf; plot_input(t{n}, sim_data{n}.SATURATION, U_corr{n}, 'corr')
-        %export_fig(gcf, '-transparent', [dir, num2str(n), '_corr_input.eps'])
-        %print([dir, num2str(n), '_corr_input.png'], '-dpng')
+        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'])
+       
+        %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}, '')
-        %export_fig(gcf, '-transparent', [dir, num2str(n), '_total_input.eps'])
-        %print([dir, num2str(n), '_total_input.png'], '-dpng')
+        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'])
+
+        pause(1); clf; plot_trajectory(t{n}, ref_t{n}, y{n})
+        pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_trajectory_out.png'])
+        pause(1); clf; plot_error(t{n}, ref_t{n}, y{n})
+        pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_error_out.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{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{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)
+    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

plot_doubleinput.m

@@ -1,8 +1,11 @@
-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
 
+ylim1_ = sat(1)*2.5;
+ylim2_ = sat(2)*2.5;
+lw = 6;
 
 if type == 0
 subplot(1,2,1);
@@ -11,21 +14,25 @@ subplot(2,1,1);
 end
 
 hold on
-plot(t, U_track(:, 1), 'Linewidth', 5, 'DisplayName', '\omega_r^{track}');
-plot(t, U_corr(:, 1), 'Linewidth', 5, 'DisplayName', '\omega_r^{corr}');
-legend('FontSize', 12, 'Location', 'east', 'AutoUpdate','off')
-plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--');
-plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--');
-xlabel('\textbf{t[s]}', 'FontSize', 18, 'Interpreter','latex');
+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.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
 
 
 Axes = gca;
-Axes.FontSize=18;
+Axes.FontSize=22;
 Axes.FontWeight='bold';
 grid minor;
 Axes.PlotBoxAspectRatio = [1 1 1];
-
+ylim([-ylim1_, ylim1_])
 
 if type == 0
 subplot(1,2,2);
@@ -34,18 +41,23 @@ subplot(2,1,2);
 end
 
 hold on
-plot(t, U_track(:, 2), 'Linewidth', 5, 'DisplayName', '\omega_l^{track}');
-plot(t, U_corr(:, 2), 'Linewidth', 5, 'DisplayName', '\omega_l^{corr}');
-legend('FontSize', 12, 'Location', 'east', 'AutoUpdate','off')
-plot(t, ones(1,length(t))*sat(2), 'Linewidth', 2.5, 'LineStyle', '--');
-plot(t, -ones(1,length(t))*sat(2), 'Linewidth', 2.5, 'LineStyle', '--');
-xlabel('\textbf{t[s]}', 'FontSize', 18, 'Interpreter','latex');
+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.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=18;
+Axes.FontSize=22;
 Axes.FontWeight='bold';
 grid minor;
 Axes.PlotBoxAspectRatio = [1 1 1];
+ylim([-ylim2_, ylim2_])
+
 
 hold off
 end

plot_error.m

@@ -5,20 +5,20 @@ function plot_error(t,ref,x)
     ey = ref(:, 2) - x(:, 2);
 
     error_norm = sqrt(ex.*ex + ey.*ey);
-    plot(t, error_norm, 'Linewidth', 5, 'DisplayName', 'norm of error [m]');
+    plot(t, error_norm, 'Linewidth', 8, 'DisplayName', 'norm of error [m]');
 
     avg = ones(1, length(error_norm)) * error_norm / length(error_norm);
-    plot(t, avg*ones(1, length(error_norm)), 'DisplayName', 'average error [m]', 'LineWidth', 3);
+    plot(t, avg*ones(1, length(error_norm)), 'DisplayName', 'average error [m]', 'LineWidth', 4);
 
     Axes = gca;
-    Axes.FontSize=18;
+    Axes.FontSize=22;
     Axes.FontWeight='bold';
     Axes.PlotBoxAspectRatio = [1 1 1];
     grid minor;
     
-    legend('FontSize', 12, 'Location', 'northeast', 'AutoUpdate','off')
-    xlabel("\textbf{t [s]}", FontSize=18, Interpreter="latex")
-    ylabel("\textbf{tracking error}", FontSize=18, Interpreter="latex")
+    legend('FontSize', 18, 'Location', 'northeast', 'AutoUpdate','off')
+    xlabel("\textbf{t [s]}", Interpreter="latex")
+    ylabel("\textbf{tracking error}", Interpreter="latex")
 
     hold off
 end

plot_input.m

@@ -1,36 +1,57 @@
-function plot_input(t, sat, U, type)
+function plot_input(t, sat, U, orient, type, in1, in2, m1, m2)
+
 
 tiledlayout(1,1,'Padding','tight', 'TileSpacing','compact')
 nexttile
 
+if orient == 0
+subplot(1,2,1);
+else
 subplot(2,1,1);
+end
+
+ylim1_ = sat(1)*2.5;
+ylim2_ = sat(2)*2.5;
+
 hold on
-plot(t, U(:, 1), 'Linewidth', 5);
-plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--');
-plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--');
-xlabel('\textbf{t[s]}', 'FontSize', 18, 'Interpreter','latex');
-ylabel(['\textbf{$$w_r^{' type '}$$[rad/s]}'], 'FontSize', 18, 'Interpreter','latex');
+plot(t, U(:, 1), 'Linewidth', 6);
+
+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{$$' in1 '^{' type '}$$ [' m1 ']}'], 'Interpreter','latex');
 hold off
 
 
 Axes = gca;
-Axes.FontSize=18;
+Axes.FontSize=26;
 Axes.FontWeight='bold';
 grid minor;
 Axes.PlotBoxAspectRatio = [1 1 1];
+ylim([-ylim1_, ylim1_])
 
+if orient == 0
+subplot(1,2,2);
+else
 subplot(2,1,2);
+end
+
 hold on
-plot(t, U(:, 2), 'Linewidth', 5);
-plot(t, ones(1,length(t))*sat(2), 'Linewidth', 2.5, 'LineStyle', '--');
-plot(t, -ones(1,length(t))*sat(2), 'Linewidth', 2.5, 'LineStyle', '--');
-xlabel('\textbf{t[s]}', 'FontSize', 18, 'Interpreter','latex');
-ylabel(['\textbf{$$w_l^{' type '}$$[rad/s]}'], 'FontSize', 18, 'Interpreter','latex');
+plot(t, U(:, 2), 'Linewidth', 6);
+
+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{$$' in2 '^{' type '}$$ [' m2 ']}'], 'Interpreter','latex');
 hold off
 
 Axes = gca;
-Axes.FontSize=18;
+Axes.FontSize=26;
 Axes.FontWeight='bold';
 grid minor;
 Axes.PlotBoxAspectRatio = [1 1 1];
+ylim([-ylim2_, ylim2_])
+
 end

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)
@@ -21,11 +21,11 @@ else
 subplot(2,1,1);
 end
 plot(t, U_corr1(:, 1) - U_corr2(:,1), 'Linewidth', 5);
-xlabel('\textbf{t[s]}', 'FontSize', 24, 'Interpreter','latex');
-ylabel([name1_1 '-' name1_2], 'FontSize', 18, 'Interpreter','latex');
+xlabel('\textbf{t[s]}', 'Interpreter','latex');
+ylabel([name1_1 '-' name1_2 ' [' m1 ']'], 'Interpreter','latex');
 
 Axes = gca;
-Axes.FontSize=18;
+Axes.FontSize=26;
 Axes.FontWeight='bold';
 grid minor;
 Axes.PlotBoxAspectRatio = [1 1 1];
@@ -37,11 +37,11 @@ else
 subplot(2,1,2);
 end
 plot(t, U_corr1(:, 2) - U_corr2(:,2), 'Linewidth', 5);
-xlabel('\textbf{t[s]}', 'FontSize', 24, 'Interpreter','latex');
-ylabel([name2_1 '-' name2_2], 'FontSize', 18, 'Interpreter','latex');
+xlabel('\textbf{t[s]}', 'Interpreter','latex');
+ylabel([name2_1 '-' name2_2 ' [' m2 ']'], 'Interpreter','latex');
 
 Axes = gca;
-Axes.FontSize=18;
+Axes.FontSize=26;
 Axes.FontWeight='bold';
 grid minor;
 Axes.PlotBoxAspectRatio = [1 1 1];

plot_trajectory.m

@@ -5,26 +5,27 @@ function tl = plot_trajectory(t, ref, x)
 
     hold on
     grid minor  
-    label_interval = 1:1200:length(x);
+    label_interval = 1:1800:length(x);
     labels = strcat(num2str(round(t(label_interval),1)), "s");
 
     %title("Reference trajectory / Robot position", "FontSize", 18)
     
-    plot(ref(:, 1), ref(:, 2), "DisplayName", "Reference trajectory", 'Color', 'blue', 'Linewidth', 12);
+    plot(ref(:, 1), ref(:, 2), "DisplayName", "Reference trajectory", 'Color', 'blue', 'Linewidth', 4, 'Linestyle', ':');
     %plot(x(:, 1), x(:, 2), "DisplayName", "Robot position", 'Color', 'green', 'Linewidth', 6);
-    [x_arr, y_arr] = arrowed_line(x(:, 1), x(:, 2), 600, 50, 50);
+    [x_arr, y_arr] = arrowed_line(x(:, 1), x(:, 2), 1200, 65, 65);
     plot(x_arr, y_arr, "DisplayName", "Robot position", 'Color', 'green', 'Linewidth', 4);
-    labelpoints(x(label_interval, 1), x(label_interval, 2), labels, 'N', 0.3, 'FontWeight', 'bold', 'FontSize', 15);
+    labelpoints(x(label_interval, 1), x(label_interval, 2), labels, 'N', 0.35, 'FontWeight', 'bold', 'FontSize', 14, 'Color', 'k', 'BackgroundColor', 'w');
 
     Axes = gca;
-    Axes.FontSize=18;
+    Axes.FontSize=22;
     Axes.FontWeight='bold';
+    Axes.PlotBoxAspectRatio = [1 1 1];
 
-    xlabel('\textbf{x [m]}', 'FontSize', 18, 'Interpreter', 'latex');
-    ylabel('\textbf{y [m]}', 'FontSize', 18, 'Interpreter', 'latex');
-    legend('FontSize', 12, 'Location', 'southeast', 'AutoUpdate','off')
-
+    xlabel('\textbf{x [m]}', 'Interpreter', 'latex');
+    ylabel('\textbf{y [m]}', 'Interpreter', 'latex');
+    legend('FontSize', 18, 'Location', 'southeast', 'AutoUpdate','off')
 
+    %{
     xlim = Axes.XLim;
     ylim = Axes.YLim;
     xl = xlim(2) - xlim(1);
@@ -37,6 +38,7 @@ function tl = plot_trajectory(t, ref, x)
     Axes.PlotBoxAspectRatio = [1 1 1];
     Axes.Units='normalized' 
     Axes.OuterPosition = [0 0 1.25 1.25];
+    %}
 
 
     hold off

plot_tripleinput.m

@@ -1,8 +1,11 @@
-function plot_doubleinput(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
+%tiledlayout(1,1,'Padding','tight', 'TileSpacing','compact')
+%nexttile
 
+ylim1_ = sat(1)*2.5;
+ylim2_ = sat(2)*2.5;
+lw = 6;
 
 if type == 0
 subplot(1,2,1);
@@ -11,21 +14,26 @@ subplot(2,1,1);
 end
 
 hold on
-plot(t, U_track(:, 1), 'Linewidth', 5, 'DisplayName', '\omega_r^{track}');
-plot(t, U_corr(:, 1), 'Linewidth', 5, 'DisplayName', '\omega_r^{corr}');
-plot(t, U(:, 1), 'Linewidth', 2.8, 'DisplayName', '\omega');
-legend('FontSize', 12, 'Location', 'northeast', 'AutoUpdate','off')
-plot(t, ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--');
-plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--');
-xlabel('\textbf{t[s]}', 'FontSize', 18, 'Interpreter','latex');
+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.5, 'LineStyle', '--', 'Color', 'black');
+plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black');
+
 hold off
 
 
 Axes = gca;
-Axes.FontSize=18;
+Axes.FontSize=22;
 Axes.FontWeight='bold';
 grid minor;
 Axes.PlotBoxAspectRatio = [1 1 1];
+ylim([-ylim1_, ylim1_])
 
 
 if type == 0
@@ -35,19 +43,24 @@ subplot(2,1,2);
 end
 
 hold on
-plot(t, U_track(:, 2), 'Linewidth', 5, 'DisplayName', '\omega_l^{track}');
-plot(t, U_corr(:, 2), 'Linewidth', 5, 'DisplayName', '\omega_l^{corr}');
-plot(t, U(:, 2), 'Linewidth', 2.8, 'DisplayName', '\omega_l');
-legend('FontSize', 12, 'Location', 'northeast', 'AutoUpdate','off')
-plot(t, ones(1,length(t))*sat(2), 'Linewidth', 2.5, 'LineStyle', '--');
-plot(t, -ones(1,length(t))*sat(2), 'Linewidth', 2.5, 'LineStyle', '--');
-xlabel('\textbf{t[s]}', 'FontSize', 18, 'Interpreter','latex');
+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.5, 'LineStyle', '--', 'Color', 'black');
+plot(t, -ones(1,length(t))*sat(1), 'Linewidth', 2.5, 'LineStyle', '--', 'Color', 'black');
+
 
 Axes = gca;
-Axes.FontSize=18;
+Axes.FontSize=22;
 Axes.FontWeight='bold';
 grid minor;
 Axes.PlotBoxAspectRatio = [1 1 1];
+ylim([-ylim2_, ylim2_])
 
 hold off
 end

tesi.m

@@ -2,73 +2,96 @@ clc
 clear all
 close all
 
-%TESTS = ["sin_faster", "sin", "circle", "straightline", "reverse_straightline"]
-TESTS = ["figure8/fancyreps"]
+% options
+ROBOT = 'diffdrive'
+%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:s_(1)    
-    clear data sim_data
+for i = 1:length(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;
-
-    spmd (3)
-        worker_index = spmdIndex;
-        data = load(['tests/' num2str(worker_index) '.mat']);
+    %sim_data.tfin = 15;
     
+    % spawn a new worker for each controller
+    % 1: track only
+    % 2: track + 1step
+    % 3: track + multistep
+    spmd (2)
+        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
 
-        [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simulate_discr(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;
     
         disp('Done')
     end
     
+    % save simulation data
+    f1 = [ TEST '/'  char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible name
+    f = ['results-' ROBOT '-costfun/' f1];
+    mkdir(f)
+    % save workspace
+    dsave([f '/workspace_composite.mat']);
+    % save test file
+    copyfile(['tests/' TEST], f);
+    
+    % save figures + plot results
     h = [];
+    % plot results
     s1_ = size(worker_index);
     for n = 1:s1_(2)
         h = [h, figure('Name', [TEST ' ' num2str(n)] )];
         plot_results(t{n}, q{n}, ref_t{n}, U{n}, U_track{n}, U_corr{n});
     end
-
-    % windows compatible
-    f1 = [ TEST '/'  char(datetime, 'dd-MM-yyyy HH-mm-ss')];
-    f = ['results-diffdrive/' f1];
-    mkdir(f)
-    savefig(h, [f '/figure.fig']);
-
-    h = [h, figure('Name', 'difference between 1step and multistep')]
+    % plot correction different between 1-step and multistep
+    h = [h, figure('Name', 'difference between 1step and multistep')];
     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')
-
-    clear h
-    dsave([f '/workspace_composite.mat']);
-
-    copyfile(['tests/' TEST], f);
+    ylabel(['difference on ' sim_data{1}.input2_name ' between 1-step and multistep'])
+    % save figures
+    savefig(h, [f '/figure.fig']);
 
     %video(q{1}', ref_t{1}', 0.1, t{1}, 2, sim_data{1}.tc*0.05, "aa");
     %video(q{2}', ref_t{2}', 0.1, t{2}, 2, sim_data{1}.tc*0.05, "aa");
@@ -79,7 +102,7 @@ end
 %% FUNCTION DECLARATIONS
 
 % Discrete-time simulation
-function [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simulate_discr(sim_data)
+function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simulate_discr(sim_data)
     tc = sim_data.tc;
     steps = sim_data.tfin/tc
     
@@ -94,6 +117,13 @@ function [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simula
     U_corr = u_corr';
     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;
@@ -103,12 +133,12 @@ function [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simula
         tspan = [(n-1)*tc n*tc];
         z0 = q(end, :);
         
-        %[v, z] = ode45(@sistema_discr, tspan, z0, u_discr);
-        [v, z] = ode45(@(v, z) sistema_discr(v, z, u_discr, sim_data), tspan, z0);
+        opt = odeset('MaxStep', 0.005);
+        [v, z] = ode45(@(v, z) fun(v, z, u_discr, sim_data), tspan, z0, opt);
 
         q = [q; z];
         t = [t; v];
-        
+                
         [u_discr, u_track, u_corr, U_corr_history, q_pred] = control_act(t(end), q(end, :), sim_data);
         sim_data.U_corr_history = U_corr_history;
         U = [U; ones(length(v), 1)*u_discr'];
@@ -116,7 +146,11 @@ function [t, q, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simula
         U_track = [U_track; ones(length(v), 1)*u_track'];
         Q_pred(:, :, 1+n) = q_pred;
 	
-	U_corr_pred_history(:,:,n) = permute(U_corr_history, [3, 1, 2]);
+	    U_corr_pred_history(:,:,n) = permute(U_corr_history, [3, 1, 2]);
+        
+        y1 = q(:, 1) + sim_data.b * cos(q(:,3));
+        y2 = q(:, 2) + sim_data.b * sin(q(:,3));
+        y = [y; [y1, y2]];
     end
 
     ref_t = double(subs(sim_data.ref, t'))';

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

video_all.m

@@ -5,21 +5,25 @@ disp('Waiting 5s')
 pause(1)
 
 PLOT_TESTS = [
-    % "results-diffdrive/straightline/chill/01-Aug-2024 15-34-03";
-    % "results-diffdrive/straightline/chill_errortheta_pisixths/01-Aug-2024 15-56-36";
-    % "results-diffdrive/square/01-Aug-2024 16-18-51";
-    % "results-diffdrive/circle/start_center/01-Aug-2024 16-46-41";
-    % "results-diffdrive/circle/start_tangent/01-Aug-2024 16-55-09";
-    % "results-diffdrive/circle/toofast/01-Aug-2024 17-35-25";
-    % "results-diffdrive/straightline/toofast/01-Aug-2024 15-37-48";
-    % "results-diffdrive/figure8/chill/15-Aug-2024 09-16-21";
-    % "results-diffdrive/figure8/toofast/15-Aug-2024 09-10-32";
-    "results-diffdrive/straightline/abrupt_stop_chill/27-Aug-2024 10-15-43";
-    % "results-diffdrive/straightline/abrupt_stop_toofast/27-Aug-2024 10-44-35";
-    % "results-diffdrive/sin/no_start_error/27-Aug-2024 19-28-17";
-    % "results-diffdrive/sin/no_start_error/27-Aug-2024 19-29-42";
-    % "results-diffdrive/sin/no_start_error/27-Aug-2024 19-31-17";
-    % "results-diffdrive/sin/no_start_error/27-Aug-2024 19-38-03";
+    "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)
@@ -30,7 +34,7 @@ for i = 1:s_(1)
     PLOT_TEST = [sPLOT_TEST, '/workspace_composite.mat']
     load(PLOT_TEST)
 
-    dir = ['gifs-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");
@@ -38,7 +42,7 @@ for i = 1:s_(1)
     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);
-    movefile("*.mp4", dir);
+    movefile("*.avi", dir);
+    %movefile("*.mp4", dir);
 
 end