activeset

control_act.m

@@ -32,26 +32,32 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         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
+            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;
+                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;
+                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]
+                R = [sim_data.r/2 sim_data.r/2]
-            %H = T_inv' * R' * R * T_inv;
+                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]
+                R = [sim_data.r/sim_data.d -sim_data.r/sim_data.d]
-            %H = T_inv' * R' * R * T_inv;
+                H = T_inv' * R' * R * T_inv;
+            end
         end
 
         f = zeros(2,1);
@@ -62,8 +68,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         b = [s_-d;s_+d];
         
         % solve qp problem
-        options = optimoptions('quadprog', 'Display', 'off');
+        options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set');
-        u_corr = quadprog(H, f, A, b, [],[],[],[],[],options);
+        u_corr = quadprog(H, f, A, b, [],[],[],[],zeros(2,1),options);
         
         q_pred = q_act;
         U_corr_history(:,:,1) = u_corr;
@@ -141,6 +147,7 @@ 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 = [];
+        H = [];
         for k=1:pred_hor
             T_inv = T_inv_pred(:,:,k);
             u_track = u_track_pred(:,:,k);
@@ -148,6 +155,36 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
 
             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)]);
@@ -158,14 +195,15 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
         % 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*eye(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');
+        options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set');
-        U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],[],options);
+        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

plot_all.m

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

@@ -31,12 +31,14 @@ for i = 1:length(TESTS)
     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
     % 2: track + 1step
     % 3: track + multistep
-    spmd (2)
+    spmd (3)
         worker_index = spmdIndex;
         % load controller-specific options
         data = load(['tests/' num2str(worker_index) '.mat']);

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
+
+%%