abbandon correction, linearize around trajectory

try three different approaches, with questionable results

control_act.m

@@ -1,154 +1,134 @@
-function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data)   
+function [u, ut, 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);
-    [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
+        u = dc*ut;
         
-    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 = [];
+        q_pred = [];
+        return
     end
     
-function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
-    pred_hor = sim_data.PREDICTION_HORIZON;
+    % mpc
     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)];
 
-        d = T_inv*ut;
-        b = [s_-d;s_+d];
+    % prediction
+    %T_invs = zeros(2,2, pred_hor);
+    %Qs = zeros(3,1,pred_hor);
+    %drefs = zeros(2,1, pred_hor);
+    %refs = zeros(2,1, pred_hor);
 
-        % solve qp problem
-        options = optimoptions('quadprog', 'Display', 'off');
-        u_corr = quadprog(H, f, A, b, [],[],[],[],[],options);
+    % optim problem
+    prob = optimproblem('ObjectiveSense', 'minimize');
+    % objective
+    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
     
-        %disp('start of simulation')
-        % for each step in the prediction horizon, integrate the system to
-        % predict its future state
+    % linearization around robot trajectory
+    % only needs to be calculated once
+    theta = q(3);
+    st = sin(theta);
+    ct = cos(theta);       
+    T_inv = decouple_matrix(q, sim_data);
+    
 
     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
+        % reference trajectory and derivative
+        ref_s = double(subs(sim_data.ref, t_));
+        dref_s = double(subs(sim_data.dref, t_));
 
         %{
-            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
+        % linearization around trajectory trajectory, hybrid approach
+        % theta from reference position and velocity
+        if eq(k, 1)
+            % only for the first step, theta from the current state
+            theta = q(3);
+        else
+            % then linearize around reference trajectory
+            % proper way
+            theta = mod( atan2(dref_s(2), dref_s(1)) + 2*pi, 2*pi);
+            % or, derivative using difference. Seem to give the same
+            % results
+            %ref_s1 = double(subs(sim_data.ref, t_ + tc));
+            %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
-        % 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)
-        b_deq = [];
-        for k=1:pred_hor
-            T_inv = T_inv_pred(:,:,k);
-            u_track = u_track_pred(:,:,k);
-            d = T_inv*u_track;
-            b_deq = [b_deq; s_ - d; s_ + d];
+        %{
+        % linearization around trajectory trajectory, "correct" approach
+        theta = mod( atan2(dref_s(2), dref_s(1)) + 2*pi, 2*pi);
+        st = sin(theta);
+        ct = cos(theta);       
+        T_inv = decouple_matrix([ref_s(1); ref_s(2); theta], sim_data);
+        %}
+                    
+        
+        % not at the end of the horizon
+        if k < pred_hor - 1
+            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
 
-        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;
-        % 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);
+            % evolution constraints
+            c = Q(:, k+1) == Q(:, k) + [v*tc*ct; v*tc*st; w*tc];
+            prob.Constraints.evo = [prob.Constraints.evo; c];
         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,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,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/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-unicycle-costfun2-soltraj/circle/start_center/21-09-2024-15-29-40"
+    "results-diffdrive-costfun2-soltraj/circle/start_center/21-09-2024-12-16-00"
     ]
 
 s_ = size(PLOT_TESTS)
@@ -37,7 +20,7 @@ for i = 1:s_(1)
     PLOT_TEST = [sPLOT_TEST, '/workspace_composite.mat']
     load(PLOT_TEST)
 
-    dir = ['images-', ROBOT, '/', sPLOT_TEST, '/']
+    dir = ['images-', ROBOT, '-costfun2-soltraj/', sPLOT_TEST, '/']
     mkdir(dir);
     
     for n=1:3
@@ -50,14 +33,6 @@ 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'])
@@ -75,12 +50,6 @@ 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,5 +20,7 @@ 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,9 +3,10 @@ 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 = ["circle/start_center"]
+ROBOT = 'diffdrive'
+%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"]
 
 % main
 s_ = size(TESTS);
@@ -29,6 +30,7 @@ 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
@@ -50,13 +52,17 @@ 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, U_corr, U_corr_pred_history, Q_pred] = simulate_discr(sim_data);
+        [t, q, y, ref_t, U, U_track, Q_pred] = simulate_discr(sim_data);
         toc;
     
         disp('Done')
@@ -64,7 +70,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 '-costfun2-soltraj/' f1];
     mkdir(f)
     % save workspace
     dsave([f '/workspace_composite.mat']);
@@ -77,16 +83,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_corr{n});
+        plot_results(t{n}, q{n}, ref_t{n}, U{n}, U_track{n}, U_track{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_corr{2}(:, 1) - U_corr{3}(:, 1))
+    plot(t{2}, U{2}(:, 1) - U{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_corr{2}(:, 2) - U_corr{3}(:, 2))
+    plot(t{2}, U{2}(:, 2) - U{3}(:, 2))
     xlabel('t')
     ylabel(['difference on ' sim_data{1}.input2_name ' between 1-step and multistep'])
     % save figures
@@ -98,25 +104,23 @@ for i = 1:length(TESTS)
 end
 
 
+
 %% FUNCTION DECLARATIONS
 
 % Discrete-time simulation
-function [t, q, y, 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, 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);
 
-    [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;
+    Q_pred = zeros(sim_data.PREDICTION_HORIZON,3, steps + 1);
+
+    [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);
@@ -125,10 +129,6 @@ function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = sim
     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,21 +138,16 @@ function [t, q, y, ref_t, U, U_track, U_corr, U_corr_pred_history, Q_pred] = sim
         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_discr, u_track, q_pred] = control_act(t(end), q(end, :), sim_data);
         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;
-	
-	    U_corr_pred_history(:,:,n) = permute(U_corr_history, [3, 1, 2]);
+    end
 
     y1 = q(:, 1) + sim_data.b * cos(q(:,3));
     y2 = q(:, 2) + sim_data.b * sin(q(:,3));
-        y = [y; [y1, y2]];
-    end
+    y = [y1, y2];
 
     ref_t = double(subs(sim_data.ref, t'))';
 end
 
-%%