also plot trajectory and error relative to output position

control_act.m

@@ -1,11 +1,12 @@
 function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data)   
     dc = decouple_matrix(q, sim_data);
     ut = utrack(t, q, sim_data);
-    ut = dc*ut;
-    
     [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
-    
-    uc = dc*uc;
+
+    ut = dc*ut;
+    %uc = dc*uc;
+    %uc = zeros(2,1);
+
     u = ut+uc;
     % saturation
     u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u));
@@ -29,47 +30,19 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
     if eq(pred_hor, 0)
         return
     elseif eq(pred_hor, 1)
+        H = eye(2);
+        f = zeros(2,1);
         T_inv = decouple_matrix(q_act, sim_data);
         ut = utrack(t, q_act, sim_data);
-        
-        if eq(sim_data.costfun,1)
-            % minimize v, unicycle and ddr
-            % actually minimize xdot^2 + ydot^2 = v
-            H = 2*eye(2);
-        elseif eq(sim_data.costfun,2)
-            % minimize vcorr^2 + wcorr^2, unicycle and ddr
-            H = T_inv;
-        end
-        if eq(sim_data.robot, 0)
-            if eq(sim_data.costfun, 3)
-                % ex3: unicycle, minimize v
-                H = T_inv' * [1 0; 0 0] * T_inv;
-            elseif eq(sim_data.costfun, 4)
-                % ex4: unicycle, minimize w
-                H = T_inv' * [0 0; 0 1] * T_inv;
-            end
-        else
-            if eq(sim_data.costfun, 3)
-                % ex1: ddr, minimize v. det(H) = 0 H not symmetric
-                R = [sim_data.r/2 sim_data.r/2]
-                H = T_inv' * R' * R * T_inv;
-            elseif eq(sim_data.costfun, 4)
-                % ex2: ddr, minimize w. det(H) = 0
-                R = [sim_data.r/sim_data.d -sim_data.r/sim_data.d]
-                H = T_inv' * R' * R * T_inv;
-            end
-        end
-
-        f = zeros(2,1);
-        A = [T_inv; -T_inv];        
-        %A = [eye(2); -eye(2)];
+        %A = [T_inv; -T_inv];        
+        A = [eye(2); -eye(2)];
 
         d = T_inv*ut;
         b = [s_-d;s_+d];
         
         % solve qp problem
-        options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set');
-        u_corr = quadprog(H, f, A, b, [],[],[],[],zeros(2,1),options);
+        options = optimoptions('quadprog', 'Display', 'off');
+        u_corr = quadprog(H, f, A, b, [],[],[],[],[],options);
         
         q_pred = q_act;
         U_corr_history(:,:,1) = u_corr;
@@ -97,7 +70,7 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
             
             T_inv = decouple_matrix(q_act, sim_data);
             % compute inputs (v, w)/(wr, wl)
-            u_ = T_inv * (u_track_ + u_corr_);
+            u_ = T_inv * u_track_ + u_corr_;
             
 
             % if needed, map (wr, wl) to (v, w) for unicicle
@@ -145,65 +118,28 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         % 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 = [];
-        H = [];
         for k=1:pred_hor
             T_inv = T_inv_pred(:,:,k);
             u_track = u_track_pred(:,:,k);
             d = T_inv*u_track;
-
-            A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
             b_deq = [b_deq; s_ - d; s_ + d];
-
-
-        if eq(sim_data.costfun,1)
-            % minimize v, unicycle and ddr
-            % actually minimize xdot^2 + ydot^2 = v
-            H = blkdiag(H, 2*eye(2));
-        elseif eq(sim_data.costfun,2)
-            % minimize vcorr^2 + wcorr^2, unicycle and ddr
-            H = blkdiag(H, T_inv);
-            H = kron(eye(pred_hor), T_inv(:, :, :));
-        end
-        if eq(sim_data.robot, 0)
-            if eq(sim_data.costfun, 3)
-                % ex3: unicycle, minimize v
-                H = blkdiag(H, T_inv' * [1 0; 0 0] * T_inv);
-            elseif eq(sim_data.costfun, 4)
-                % ex4: unicycle, minimize w
-                H = blkdiag(H, T_inv' * [0 0; 0 1] * T_inv);
-            end
-        else
-            if eq(sim_data.costfun, 3)
-                % ex1: ddr, minimize v. det(H) = 0 H not symmetric
-                R = [sim_data.r/2 sim_data.r/2];
-                H = blkdiag(H, T_inv' * (R') * R * T_inv);
-            elseif eq(sim_data.costfun, 4)
-                % ex2: ddr, minimize w. det(H) = 0
-                R = [sim_data.r/sim_data.d -sim_data.r/sim_data.d];
-                H = blkdiag(H, T_inv' * (R') * R * T_inv);
-            end
-        end
         end
         
-        %A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
+        A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
         %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;
-        %H = kron(eye(pred_hor), 2*ones(2,2));
+        H = eye(pred_hor*2)*2;
         % no linear terms
         f = zeros(pred_hor*2, 1);
-
-
     
         % solve qp problem
-        options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set');
-        U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],zeros(2*pred_hor, 1),options);
+        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

disaster.m

@@ -1,11 +1,13 @@
 clc
 clear all
-close allQQQ
+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('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);

plot_all.m

@@ -8,8 +8,25 @@ disp('Photos will start in 3s')
 pause(3)
 
 PLOT_TESTS = [
-    "results-diffdrive-costfun-ddronly/circle/start_center/ddr-minv-activeset";
-    "results-diffdrive-costfun-ddronly/circle/start_center/ddr-minw-activeset";
+    "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)

tesi.m

@@ -4,11 +4,10 @@ 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"]
+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 ROBOT
@@ -30,9 +29,6 @@ for i = 1:length(TESTS)
     [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data);
     sim_data.ref = ref;
     sim_data.dref = dref;
-    %sim_data.tfin = 15;
-    sim_data.costfun=4
-    sim_data.tc=0.05
     
     % spawn a new worker for each controller
     % 1: track only
@@ -68,7 +64,7 @@ for i = 1:length(TESTS)
     
     % save simulation data
     f1 = [ TEST '/'  char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible name
-    f = ['results-' ROBOT '-costfun/' f1];
+    f = ['results-' ROBOT '/' f1];
     mkdir(f)
     % save workspace
     dsave([f '/workspace_composite.mat']);

tesi_st.m

@@ -1,130 +0,0 @@
-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 = ["figure8/chill"]
-CONTROLLER = 3
-
-% main
-s_ = size(TESTS);
-for i = 1:length(TESTS)
-    clearvars -except i s_ TESTS ROBOT CONTROLLER
-    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 contzroller
-    % 1: track only
-    % 2: track + 1step
-    % 3: track + multistep
-    % load controller-specific options
-    data = load(['tests/' num2str(CONTROLLER) '.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, Q_pred, Prob] = simulate_discr(sim_data);
-    toc;
-
-    disp('Done')
-    
-    % save simulation data
-    f1 = [ TEST '/'  char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible name
-    f = ['results-' ROBOT '-costfun2-st/' f1];
-    mkdir(f)
-    % save workspace
-    dsave([f '/workspace_composite.mat']);
-    % save test file
-    copyfile(['tests/' TEST], f);
-    
-    % save figures + plot results
-    % plot results
-    h = figure('Name', [TEST ' ' num2str(CONTROLLER)] );
-    plot_results(t, q, ref_t, U, U_track, U_track);
-    % save figures
-    savefig(h, [f '/figure.fig']);
-end
-
-
-%% FUNCTION DECLARATIONS
-
-% Discrete-time simulation
-function [t, q, y, ref_t, U, U_track, Q_pred, Prob] = simulate_discr(sim_data)
-    tc = sim_data.tc;
-    steps = sim_data.tfin/tc
-    
-    q = sim_data.q0';
-    t = 0;
-
-    Q_pred = zeros(sim_data.PREDICTION_HORIZON,3, steps + 1);
-    Prob = cell(steps+1, 1);
-
-    [u_discr, u_track, q_pred, prob] = control_act(t(end), q(end, :), sim_data);
-    U = u_discr';
-    U_track = u_track';
-    Q_pred(:, :, 1) = q_pred;
-    Prob{1} = prob;
-
-    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
-        tspan = [(n-1)*tc n*tc];
-        z0 = q(end, :);
-        
-        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, q_pred, prob] = control_act(t(end), q(end, :), sim_data);
-        Prob{1+n} = prob;
-        U = [U; ones(length(v), 1)*u_discr'];        
-        U_track = [U_track; ones(length(v), 1)*u_track'];
-        Q_pred(:, :, 1+n) = q_pred;
-    end
-
-    y1 = q(:, 1) + sim_data.b * cos(q(:,3));
-    y2 = q(:, 2) + sim_data.b * sin(q(:,3));
-    y = [y1, y2];
-
-    ref_t = double(subs(sim_data.ref, t'))';
-end
-
-%%