also plot trajectory and error relative to output position

control_act.m

@@ -1,134 +1,154 @@
-function [u, ut, q_pred] = control_act(t, q, sim_data)   
+function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data)   
-    pred_hor = sim_data.PREDICTION_HORIZON;
-    
-    % track only
-    if eq(pred_hor, 0)
-
     dc = decouple_matrix(q, sim_data);
     ut = utrack(t, q, sim_data);
-        u = dc*ut;
+    [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
 
+    ut = dc*ut;
+    %uc = dc*uc;
+    %uc = zeros(2,1);
+
+    u = ut+uc;
     % saturation
     u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u));
-        prob = [];
+end
-        q_pred = [];
-        return
-    end
 
-    % mpc
+function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
+    pred_hor = sim_data.PREDICTION_HORIZON;
     SATURATION = sim_data.SATURATION;
     PREDICTION_SATURATION_TOLERANCE = sim_data.PREDICTION_SATURATION_TOLERANCE;
     tc = sim_data.tc;
+
+    u_corr = zeros(2,1);
+    U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
+    q_act = q;
+    u_track_pred=zeros(2,1, pred_hor);
+    T_inv_pred=zeros(2,2, pred_hor);
+
+    q_pred = [];
+
     s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
+    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)];
 
-    % prediction
+        d = T_inv*ut;
-    %T_invs = zeros(2,2, pred_hor);
+        b = [s_-d;s_+d];
-    %Qs = zeros(3,1,pred_hor);
-    %drefs = zeros(2,1, pred_hor);
-    %refs = zeros(2,1, pred_hor);
         
-    % optim problem
+        % solve qp problem
-    prob = optimproblem('ObjectiveSense', 'minimize');
+        options = optimoptions('quadprog', 'Display', 'off');
-    % objective
+        u_corr = quadprog(H, f, A, b, [],[],[],[],[],options);
-    obj = 0;
-    % decision vars
-    ss_ = repmat(s_, [1,1, pred_hor]);
-    ucorr = optimvar('ucorr', 2, pred_hor,'LowerBound', -ss_, 'UpperBound', ss_);
-    % state vars
-    Q = optimvar('state', 3, pred_hor);
-    % initial conditions
-    prob.Constraints.evo = Q(:, 1) == q';
         
+        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
     
-    % linearization around robot trajectory
+        %disp('start of simulation')
-    % only needs to be calculated once
+        % for each step in the prediction horizon, integrate the system to
-    theta = q(3);
+        % predict its future state
-    st = sin(theta);
-    ct = cos(theta);       
-    T_inv = decouple_matrix(q, sim_data);
     
+        for k = 1:pred_hor
+            % start from the old (known) state
     
-    for k=1:pred_hor
+            % 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);
             
-        % reference trajectory and derivative
+            T_inv = decouple_matrix(q_act, sim_data);
-        ref_s = double(subs(sim_data.ref, t_));
+            % compute inputs (v, w)/(wr, wl)
-        dref_s = double(subs(sim_data.dref, t_));
+            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
     
         %{
-        % linearization around trajectory trajectory, hybrid approach
+            Now setup the qp problem
-        % theta from reference position and velocity
+            It needs:
-        if eq(k, 1)
+            - Unknowns, u_corr at each timestep. Will be encoded as a vector of
-            % only for the first step, theta from the current state
+            vectors, in which every two elements is a u_j
-            theta = q(3);
+            i.e. (u_1; u_2; u_3; ...; u_C) = (v_1; w_1; v_2, w_2; v_3, w_3; ...
-        else
+            ; v_C, w_C)
-            % then linearize around reference trajectory
+            It is essential that the vector stays a column, so that u'u is the
-            % proper way
+            sum of the squared norms of each u_j
-            theta = mod( atan2(dref_s(2), dref_s(1)) + 2*pi, 2*pi);
+    
-            % or, derivative using difference. Seem to give the same
+            - Box constraints: a constraint for each timestep in the horizon.
-            % results
+            Calculated using the predicted state and inputs. They need to be
-            %ref_s1 = double(subs(sim_data.ref, t_ + tc));
+            put in matrix (Ax <= b) form
-            %theta = atan2(ref_s1(2)-ref_s(2), ref_s1(1)-ref_s(1));
-        end
-        st = sin(theta);
-        ct = cos(theta);       
-        T_inv = decouple_matrix([ref_s(1); ref_s(2); theta], sim_data);
         %}
     
-        %{
+        % box constrains
-        % linearization around trajectory trajectory, "correct" approach
+        % A becomes sort of block-diagonal
-        theta = mod( atan2(dref_s(2), dref_s(1)) + 2*pi, 2*pi);
+        % A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2:
-        st = sin(theta);
+        % accounting for T_inv and -T_inv) by PREDICTION_HORIZON (number of
-        ct = cos(theta);       
+        % vectors in u_corr times the number of elements [2] in each vector)
-        T_inv = decouple_matrix([ref_s(1); ref_s(2); theta], sim_data);
+        b_deq = [];
-        %}
+        for k=1:pred_hor
-                    
+            T_inv = T_inv_pred(:,:,k);
-        
+            u_track = u_track_pred(:,:,k);
-        % not at the end of the horizon
+            d = T_inv*u_track;
-        if k < pred_hor - 1
+            b_deq = [b_deq; s_ - d; s_ + d];
-            if eq(sim_data.robot, 0)
-                % inputs for unicycle
-                v = ucorr(1,k);
-                w = ucorr(2,k);
-            else
-                % inputs for differential drive
-                v = sim_data.r * (ucorr(1,k) + ucorr(2,k)) / 2;
-                w = sim_data.r * (ucorr(1,k) - ucorr(2,k)) / sim_data.d;
         end
         
-            % evolution constraints
+        A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
-            c = Q(:, k+1) == Q(:, k) + [v*tc*ct; v*tc*st; w*tc];
+        %A_deq
-            prob.Constraints.evo = [prob.Constraints.evo; c];
+        %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
-
-        % objective
-        % sum of squared norms of u-q^d
-
-        % feedback + tracking input
-        % cannot use the utrack function, or the current formulation makes
-        % the problem become non linear
-        err = ref_s - [Q(1, k) + sim_data.b*ct; Q(2, k) + sim_data.b*st ];
-        ut_ = dref_s + sim_data.K*err;
-        %ut = utrack(t_, Q(k, :), sim_data);
-        qd = T_inv*ut_;
-        ud = ucorr(:, k)-qd;
-        obj = obj + (ud')*ud;
-    end
-    % end linearization around trajectory
-
-
-    prob.Objective = obj;
-    %show(prob)
-    %disp("to struct")
-    %prob2struct(prob);
-    
-    opts=optimoptions(@quadprog,'Display','off');
-    sol = solve(prob, 'Options',opts);
-    u = sol.ucorr(:, 1);
-    q_pred = sol.state';
-
-    % ideal tracking for the predicted state
-    ut = decouple_matrix(q_pred, sim_data)*utrack(t, q_pred, sim_data);
 end
 
 function u_track = utrack(t, q, sim_data)    

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-unicycle-costfun2-soltraj/circle/start_center/21-09-2024-15-29-40"
+    "results-diffdrive/straightline/chill/11-09-2024-16-57-01";
-    "results-diffdrive-costfun2-soltraj/circle/start_center/21-09-2024-12-16-00"
+    "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)
@@ -20,7 +37,7 @@ for i = 1:s_(1)
     PLOT_TEST = [sPLOT_TEST, '/workspace_composite.mat']
     load(PLOT_TEST)
 
-    dir = ['images-', ROBOT, '-costfun2-soltraj/', sPLOT_TEST, '/']
+    dir = ['images-', ROBOT, '/', sPLOT_TEST, '/']
     mkdir(dir);
     
     for n=1:3
@@ -33,6 +50,14 @@ for i = 1:s_(1)
         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'])
@@ -50,6 +75,12 @@ for i = 1:s_(1)
         pause(0.5); export_fig(gcf, '-transparent', [dir, num2str(n), '_error_out.png'])
     end
     
+    % correction difference (multistep, 1-step)
+    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'])

set_initial_conditions.m

@@ -20,7 +20,5 @@ switch i
         q0 = [0;0;pi/2];
     case 9
         q0 = [2.5; 0; pi/2];
-    case 10
-        q0 = [0;0;deg2rad(3)];
 end
 end

tesi.m

@@ -3,10 +3,9 @@ clear all
 close all
 
 % options
-ROBOT = 'diffdrive'
+ROBOT = 'unicycle'
-%TESTS = ["straightline/chill", "straightline/chill_errortheta_pisixths", "straightline/toofast", "straightline/chill_errory", "circle/start_center", "figure8/chill", "figure8/toofast", "square"]
+TESTS = ["straightline/chill", "straightline/chill_errortheta_pisixths", "straightline/toofast", "straightline/chill_errory", "circle/start_center", "figure8/chill", "figure8/toofast", "square"]
-%TESTS = ["straightline/chill", "straightline/chill_errortheta_3deg", "circle/start_center", "square", "figure8/chill"]
+%TESTS = ["circle/start_center"]
-TESTS = ["circle/start_center"]
 
 % main
 s_ = size(TESTS);
@@ -30,7 +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;
     
     % spawn a new worker for each controller
     % 1: track only
@@ -52,17 +50,13 @@ for i = 1:length(TESTS)
             sim_data.(fn{1}) = data.(fn{1});
         end
         
-        if sim_data.PREDICTION_HORIZON > 1
-            sim_data.PREDICITON_HORIZON = 40
-        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] = simulate_discr(sim_data);
+        [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = simulate_discr(sim_data);
         toc;
     
         disp('Done')
@@ -70,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 '-costfun2-soltraj/' f1];
+    f = ['results-' ROBOT '/' f1];
     mkdir(f)
     % save workspace
     dsave([f '/workspace_composite.mat']);
@@ -83,16 +77,16 @@ for i = 1:length(TESTS)
     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_track{n});
+        plot_results(t{n}, q{n}, ref_t{n}, U{n}, U_track{n}, U_corr{n});
     end
     % 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{2}(:, 1) - U{3}(:, 1))
+    plot(t{2}, U_corr{2}(:, 1) - U_corr{3}(:, 1))
     xlabel('t')
     ylabel(['difference on ' sim_data{1}.input1_name ' between 1-step and multistep'])
     subplot(2,1,2)
-    plot(t{2}, U{2}(:, 2) - U{3}(:, 2))
+    plot(t{2}, U_corr{2}(:, 2) - U_corr{3}(:, 2))
     xlabel('t')
     ylabel(['difference on ' sim_data{1}.input2_name ' between 1-step and multistep'])
     % save figures
@@ -104,23 +98,25 @@ for i = 1:length(TESTS)
 end
 
 
-
 %% FUNCTION DECLARATIONS
 
 % Discrete-time simulation
-function [t, q, y, ref_t, U, U_track, 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
     
     q = sim_data.q0';
     t = 0;
+    Q_pred = zeros(sim_data.PREDICTION_HORIZON,3,sim_data.tfin/sim_data.tc + 1);
+    U_corr_pred_history=zeros(sim_data.PREDICTION_HORIZON,2,steps);
 
-    Q_pred = zeros(sim_data.PREDICTION_HORIZON,3, steps + 1);
+    [u_discr, u_track, u_corr, U_corr_history, q_pred] = control_act(t, q, sim_data);
-
+    sim_data.U_corr_history = U_corr_history;
-    [u_discr, u_track, q_pred] = control_act(t(end), q(end, :), sim_data);
     U = u_discr';
+    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);
@@ -129,6 +125,10 @@ function [t, q, y, ref_t, U, U_track, Q_pred] = simulate_discr(sim_data)
     end
     
     for n = 1:steps
+        sim_data.old_u_corr = u_corr;
+        sim_data.old_u_track = u_track;
+        sim_data.old_u = u_discr;
+
         tspan = [(n-1)*tc n*tc];
         z0 = q(end, :);
         
@@ -138,16 +138,21 @@ function [t, q, y, ref_t, U, U_track, Q_pred] = simulate_discr(sim_data)
         q = [q; z];
         t = [t; v];
                 
-        [u_discr, u_track, q_pred] = control_act(t(end), q(end, :), sim_data);
+        [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'];
+        U_corr = [U_corr; ones(length(v), 1)*u_corr'];
         U_track = [U_track; ones(length(v), 1)*u_track'];
         Q_pred(:, :, 1+n) = q_pred;
-    end
+	
+	    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 = [y1, y2];
+        y = [y; [y1, y2]];
+    end
 
     ref_t = double(subs(sim_data.ref, t'))';
 end
 
+%%