activeset

control_act.m

@@ -1,12 +1,11 @@
 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);
-    [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
-
     ut = dc*ut;
-    %uc = dc*uc;
-    %uc = zeros(2,1);
-
+    
+    [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
+    
+    uc = dc*uc;
     u = ut+uc;
     % saturation
     u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u));
@@ -30,19 +29,47 @@ 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);
-        %A = [T_inv; -T_inv];        
-        A = [eye(2); -eye(2)];
+        
+        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)];
 
         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);
+        options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set');
+        u_corr = quadprog(H, f, A, b, [],[],[],[],zeros(2,1),options);
         
         q_pred = q_act;
         U_corr_history(:,:,1) = u_corr;
@@ -70,7 +97,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
@@ -120,34 +147,63 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         % 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;
+        H = [];
         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;
+
+            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
         %b_deq
         % unknowns
        
         % squared norm of u_corr. H must be identity,
         % PREDICTION_HORIZON*size(u_corr)
-        H = eye(pred_hor*2)*2;
+        %H = eye(pred_hor*2)*2;
+        %H = kron(eye(pred_hor), 2*ones(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);
+        options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set');
+        U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],zeros(2*pred_hor, 1),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,13 +1,11 @@
 clc
 clear all
-close all
+close allQQQ
 
 %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,27 +8,8 @@ disp('Photos will start in 3s')
 pause(3)
 
 PLOT_TESTS = [
-    %{
-    "results-diffdrive/straightline/chill/11-09-2024-16-57-01";
-    "results-diffdrive/straightline/chill_errortheta_pisixths/11-09-2024-16-57-43";
-    "results-diffdrive/straightline/chill_errory/11-09-2024-16-59-04";
-    "results-diffdrive/straightline/toofast/11-09-2024-16-58-24";
-    "results-diffdrive/circle/start_center/11-09-2024-16-59-50";
-    "results-diffdrive/square/11-09-2024-17-06-14";
-    "results-diffdrive/figure8/chill/11-09-2024-17-00-53";
-    %"results-diffdrive/figure8/fancyreps/11-09-2024--45-28";
-    "results-diffdrive/figure8/toofast/11-09-2024-17-01-43";
-    %}
-
-    %"results-unicycle/straightline/chill/11-09-2024-17-07-51";
-    %"results-unicycle/straightline/chill_errortheta_pisixths/11-09-2024-17-08-35";
-    %"results-unicycle/straightline/chill_errory/11-09-2024-17-10-00";
-    %"results-unicycle/straightline/toofast/11-09-2024-17-09-18";
-    %"results-unicycle/circle/start_center/11-09-2024-17-10-48";
-    "results-unicycle/square/11-09-2024-17-17-21";
-    %"results-unicycle/figure8/chill/11-09-2024-17-11-53";
-    %"results-unicycle/figure8/fancyreps/11-09-2024--45-28";
-    %"results-unicycle/figure8/toofast/11-09-2024-17-12-45";
+    "results-diffdrive-costfun-ddronly/circle/start_center/ddr-minv-activeset";
+    "results-diffdrive-costfun-ddronly/circle/start_center/ddr-minw-activeset";
     ]
 
 s_ = size(PLOT_TESTS)
@@ -60,7 +41,7 @@ for i = 1:s_(1)
         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')
@@ -70,6 +51,11 @@ for i = 1:s_(1)
         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)
@@ -95,5 +81,4 @@ for i = 1:s_(1)
     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

tesi.m

@@ -4,10 +4,11 @@ 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
@@ -29,6 +30,9 @@ 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
@@ -64,7 +68,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 '/' f1];
+    f = ['results-' ROBOT '-costfun/' f1];
     mkdir(f)
     % save workspace
     dsave([f '/workspace_composite.mat']);
@@ -149,7 +153,7 @@ function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = sim
         
         y1 = q(:, 1) + sim_data.b * cos(q(:,3));
         y2 = q(:, 2) + sim_data.b * sin(q(:,3));
-        y = [y; y1, y2];
+        y = [y; [y1, y2]];
     end
 
     ref_t = double(subs(sim_data.ref, t'))';

tesi_st.m

@@ -0,0 +1,130 @@
+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
+
+%%