trajectory: add square

+ plot u_track and u_corr

.gitignore

@@ -1 +1,3 @@
 *.asv
+tests/**
+results/*+

control_act.m

@@ -1,81 +1,82 @@
-function u = control_act(t, q)
-    global SATURATION
-    
-    dc = decouple_matrix(q);
-    ut = utrack(t,q);
-    uc = ucorr(t,q);
+function [u, ut, uc, U_corr_history] = control_act(t, q, sim_data)   
+    dc = decouple_matrix(q, sim_data.b);
+    ut = utrack(t, q, sim_data);
+
+    [uc, U_corr_history] = ucorr(t, q, sim_data);
     u = dc * (ut + uc);
     % saturation
-    u = min(SATURATION, max(-SATURATION, u));
+    u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u));
 end
 
-function u_corr = ucorr(t,q)
-    global SATURATION PREDICTION_SATURATION_TOLERANCE PREDICTION_HORIZON tc
-    
-    if eq(PREDICTION_HORIZON, 0)
-        u_corr = zeros(2,1);
+function [u_corr, U_corr_history] = 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);
+
+    if eq(pred_hor, 0)
         return
     end
-
-    persistent U_corr_history;
-    if isempty(U_corr_history)
-        U_corr_history = zeros(2, 1, PREDICTION_HORIZON);
-    end
     
+    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')
-    q_prec = q;
-    %q_pred = [];
-    %u_track_pred = [];
-    %t_inv_pred = [];
-    q_pred=zeros(3,1, PREDICTION_HORIZON);
-    u_track_pred=zeros(2,1, PREDICTION_HORIZON+1);
-    T_inv_pred=zeros(2,2, PREDICTION_HORIZON+1);
+    q_act = q;
+    q_pred=zeros(3,1, pred_hor);
+    u_track_pred=zeros(2,1, pred_hor);
+    T_inv_pred=zeros(2,2, pred_hor);
     % for each step in the prediction horizon, integrate the system to
     % predict its future state
 
     % the first step takes in q_k-1 and calculates q_new = q_k
-    % this means that u_track_pred will contain u_track_k-1 and will not
+    % this means that u_track_pred(:,:,1) will contain u_track_k-1 and will not
     % contain u_track_k+C
-    for k = 1:PREDICTION_HORIZON
+    for k = 1:pred_hor
         % start from the old (known) state
 
-        % calculate the inputs, based on the old 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 calculated from q
-        t_ = t + tc*(k-1);
-        u_track_ = utrack(t_, q_prec);
+        % 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_prec);
+        T_inv = decouple_matrix(q_act, sim_data.b);
         u_ = T_inv * (u_corr_ + u_track_);
         
-        % calc the state integrating with euler
-        x_new = q_prec(1) + tc*u_(1) * cos(q_prec(3));
-        y_new = q_prec(2) + tc*u_(1) * sin(q_prec(3));
-        theta_new = q_prec(3) + tc*u_(2);
+        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(:,:,k) = q_new;
+        %q_pred(:,:,k) = q_new;
         u_track_pred(:,:,k) = u_track_;
         T_inv_pred(:,:,k) = T_inv;
 
         % Prepare old state for next iteration
-        q_prec = q_new;
+        q_act = q_new;
     end
 
     %disp('end of simulation')
     %q_prec
-    
-    % calculate u_track_k+C
-    u_track_pred(:,:,PREDICTION_HORIZON+1) = utrack(t+tc*PREDICTION_HORIZON, q_prec);
-    % remove u_track_k-1
-    u_track_pred = u_track_pred(:,:,2:end);
 
-    T_inv_pred(:,:,PREDICTION_HORIZON+1) = decouple_matrix(q_prec);
-    T_inv_pred = T_inv_pred(:,:,2:end);
+    
+    % calculate u_track_k+C  and T_inv_k+C, needed for constraints
+    % u_track_pred(:,:,pred_hor) = utrack(t+tc*pred_hor, q_act, sim_data);
+    % T_inv_pred(:,:,pred_hor) = decouple_matrix(q_act, sim_data.b);
 
     %disp('end of patching data up')
 
@@ -99,11 +100,12 @@ function u_corr = ucorr(t,q)
     % 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*2 (number of
     % vectors in u_corr times the number of elements [2] in each vector)
-    A_max_elems = PREDICTION_HORIZON * 2 * 2;
+    A_max_elems = pred_hor * 2 * 2;
     A_deq = [];
     b_deq = [];
 
-    for k=1:PREDICTION_HORIZON
+    s_ = SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE;
+    for k=1:pred_hor
         T_inv = T_inv_pred(:,:,k);
         u_track = u_track_pred(:,:,k);
 
@@ -116,19 +118,18 @@ function u_corr = ucorr(t,q)
         A_deq = [A_deq, column];
 
         d = T_inv*u_track;
-        b_deq = [b_deq; SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE - d;
-            SATURATION - ones(2,1)*PREDICTION_SATURATION_TOLERANCE + d];
+        b_deq = [b_deq; s_ - d; s_ + d];
     end
     
     %A_deq
     %b_deq
     % unknowns
-    
+   
     % squared norm of u_corr. H must be identity,
     % PREDICTION_HORIZON*size(u_corr)
-    H = eye(PREDICTION_HORIZON*2)*2;
+    H = eye(pred_hor*2)*2;
     % no linear terms
-    f = zeros(PREDICTION_HORIZON*2, 1);
+    f = zeros(pred_hor*2, 1);
 
     % solve qp problem
     options = optimoptions('quadprog', 'Display', 'off');
@@ -136,30 +137,28 @@ function u_corr = ucorr(t,q)
 
     % reshape the vector of vectors to be an array, each element being
     % u_corr_j as a 2x1 vector
-    U_corr_history = reshape(U_corr, [2,1,PREDICTION_HORIZON]);    
-
+    % and add the prediction at t_k+C
+    U_corr_history = reshape(U_corr, [2,1,pred_hor]);
+    %sim_data.U_corr_history = U_corr_history;
+    % first result is what to do now
     u_corr=U_corr_history(:,:, 1);
     
 end
 
-function u_track = utrack(t, q)    
-    global ref dref K
-    ref_s = double(subs(ref, t));
-    dref_s = double(subs(dref, t));
+function u_track = utrack(t, q, sim_data)    
+    ref_s = double(subs(sim_data.ref, t));
+    dref_s = double(subs(sim_data.dref, t));
     
-    f = feedback(q);
+    f = feedback(q, sim_data.b);
     err = ref_s - f;
-    u_track = dref_s + K*err;
+    u_track = dref_s + sim_data.K*err;
 end
 
-function q_track = feedback(q)
-    global b
+function q_track = feedback(q, b)
     q_track = [q(1) + b*cos(q(3)); q(2) + b*sin(q(3))  ];
 end
 
-function T_inv = decouple_matrix(q)
-    global b
-
+function T_inv = decouple_matrix(q, b)
     theta = q(3);
     st = sin(theta);
     ct = cos(theta);

set_initial_conditions.m

@@ -1,10 +1,14 @@
-function x0 = set_initial_conditions(i)
+function q0 = set_initial_conditions(i)
 switch i
     case 0
-        x0 = [0; 0; 0];
+        q0 = [0; 0; 0];
     case 1
-        x0 = [0; 0; pi];
+        q0 = [0; 0; pi];
     case 2
-        x0 = [0, 0; pi/6];
+        q0 = [0; 0; pi/6];
+    case 3
+        q0 = [1; 0; 0];
+    case 4
+        q0 = [1; 0; -pi/6];
 end
 end

set_trajectory.m

@@ -19,17 +19,44 @@ switch i
         xref = 5 + 10*s;
         yref = 0;
     case 4
-        xref = 2.5*cos(s);
-        yref = 2.5*sin(s);
+        xref = cos(s);
+        yref = sin(s);
     case 5
         xref = 15*cos(s);
         yref = 15*sin(s);
     case 6
         xref = 0.4*s; 
         yref = cos(0.4*s);
+        %xref = 0.6*s; 
+        %yref = cos(0.6*s);
     case 7
         xref = 5*cos(0.05*s);
         yref = 5*sin(0.05*s);
+    case 8
+        xref = 0.5*s;
+        yref = 1;
+    case 9
+        xref = 0.9*s;
+        yref = 0.5*cos(0.65*s);
+    case 10
+        xref = cos(0.5*s);
+        yref = 0.5 * sin(s);
+    case 11
+        % cardioid
+        a = 0.5;
+        xref = 2*a*(1-cos(0.5*s))*sin(0.5*s);
+        %xref = 4*a*(0.2-cos(0.5*s))*sin(0.5*s);
+        yref = 2*a*(1-cos(0.5*s))*cos(0.5*s);
+    case 12
+        % square! in parametic form! https://math.stackexchange.com/questions/978486/parametric-form-of-square
+        speed = 0.3;
+        side = 2;
+        s_ = speed * s;
+        %xref = side * cos(s) / max(abs(cos(speed*s)), abs(sin(speed*s))) ;
+        %yref = side * sin(s) / max(abs(cos(speed*s)), abs(sin(speed+s))) ;
+        p = (sqrt(2) * (abs(sin(s_ + pi/4)) + abs(cos(s_ + pi/4))));
+        xref = side * cos(s_) / p;
+        yref = side * sin(s_) / p;
 end
 
 ref = [xref; yref];

sistema.m

@@ -1,3 +1,3 @@
-function q = sistema(t, q)
-    q = unicycle(t, q, control_act(t, q));
+function q = sistema(t, q, sim_data)
+    q = unicycle(t, q, control_act(t, q, sim_data));
 end

sistema_discr.m

@@ -1,4 +1,3 @@
-function x = sistema_discr(t, q)
-    global u_discr
-    q = unicycle(t, q, u_discr);
+function dq = sistema_discr(t, q, u_discr)
+    dq = unicycle(t, q, u_discr);
 end

tesiema.m

@@ -2,92 +2,105 @@ clc
 clear all
 close all
 
-%% global variables
-global q0 ref dref b tc K SATURATION tc tfin USE_PREDICTION PREDICTION_HORIZON PREDICTION_SATURATION_TOLERANCE;
+%TESTS = ["sin_faster", "sin", "circle", "straightline", "reverse_straightline"]
+TESTS = ["straightline"]
 
-%% variables
-TRAJECTORY = 6
-INITIAL_CONDITIONS = 1
-USE_PREDICTION = false
-PREDICTION_HORIZON = 5
-% distance from the center of the unicycle to the point being tracked
-% ATTENZIONE! CI SARA' SEMPRE UN ERRORE COSTANTE DOVUTO A b. Minore b,
-% minore l'errore
-b = 0.2
-% proportional gain
-K = eye(2)*2
+s_ = size(TESTS);
 
-tc = 0.1
-tfin=30
+for i = 1:s_(2)    
+    clear data sim_data
+    close all
+    
+%for i = 1:1
+    TEST = convertStringsToChars(TESTS(i))
+    
+    sim_data = load(['tests/' TEST '/common.mat']);
+    
+    sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS);
+    [ref dref] = set_trajectory(sim_data.TRAJECTORY);
+    sim_data.ref = ref;
+    sim_data.dref = dref;
 
-% saturation
-% HYP: a diff. drive robot with motors spinning at 100rpm -> 15.7 rad/s.
-% Radius of wheels 10cm. Wheels distanced 15cm from each other
-% applying transformation, v
-% saturation = [1.57, 20];
-SATURATION = [1; 1];
-PREDICTION_SATURATION_TOLERANCE = 0.0;
 
-%% launch simulation
-% initial state
-% In order, [x, y, theta]
-q0 = set_initial_conditions(INITIAL_CONDITIONS)
-% trajectory to track
-[ref, dref] = set_trajectory(TRAJECTORY)
+    spmd (3)
+        worker_index = spmdIndex;
+        data = load(['tests/' TEST '/' num2str(spmdIndex) '.mat']);
+    
+        sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON;
+        sim_data.U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
+        sim_data
+    
+        [t, q, ref_t, U, U_track, U_corr] = simulate_discr(sim_data);
+    
+        disp('Done')
+    end
+    
+    h = [];
+    s1_ = size(worker_index);
+    for n = 1:s1_(2)
+        h_ = figure('Name', [TEST ' ' num2str(n)] );
+        h = [h, h_];
+        plot_results(t{n}, q{n}, ref_t{n}, U{n}, U_track{n}, U_corr{n});
+    end
 
-global tu uu
+    f1 = [ TEST '-'  datestr(datetime)];
+    f = ['results/' f1];
+    mkdir(f)
+    savefig(h, [f '/' f1 '.fig']);
+    save([f '/workspace.mat']);
 
-figure(1)
-PREDICTION_HORIZON = 0;
-[t, q, ref_t, U] = simulate_discr(tfin);
-plot_results(t, q, ref_t, U);
+    copyfile(['tests/' TEST], f);
+end
 
-figure(2)
-PREDICTION_HORIZON = 1;
-[t1, q1, ref_t1, U1] = simulate_discr(tfin);
-plot_results(t1, q1, ref_t1, U1);
-
-figure(3)
-PREDICTION_HORIZON = 2;
-[t2, q2, ref_t2, U2] = simulate_discr(tfin);
-plot_results(t2, q2, ref_t2, U2);
-
-%figure(3)
-%subplot(1, 2, 1)
-%plot(tu, uu(1, :))
-%subplot(1, 2, 2)
-%plot(tu, uu(2, :))
-%plot_results(t, x-x1, ref_t-ref_t1, U-U1);
+%% 
 
+% data = load(['tests/' TEST '/' num2str(2) '.mat']);
+% sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON;
+% sim_data.U_corr_history = zeros(2,1,sim_data.PREDICTION_HORIZON);
+% sim_data
+% [t, q, ref_t, U, U_track, U_corr] = simulate_discr(sim_data);
+% 
+% %% 
+% plot_results(t,q,ref_t,U, U_track, U_corr);
 
 
 %% FUNCTION DECLARATIONS
 
 % Discrete-time simulation
-function [t, q, ref_t, U] = simulate_discr(tfin)
-    global ref q0 u_discr tc
-
-    steps = tfin/tc
+function [t, q, ref_t, U, U_track, U_corr] = simulate_discr(sim_data)
+    tc = sim_data.tc;
+    steps = sim_data.tfin/tc
     
-    q = q0';
+    q = sim_data.q0';
     t = 0;
-    u_discr = control_act(t, q0);
+    [u_discr, u_track, u_corr, U_corr_history] = control_act(t, q, sim_data);
+    sim_data.U_corr_history = U_corr_history;
     U = u_discr';
+    U_corr = u_corr';
+    U_track = u_track';
     
     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, :);
         
-        [v, z] = ode45(@sistema, tspan, z0);
+        %[v, z] = ode45(@sistema_discr, tspan, z0, u_discr);
+        [v, z] = ode45(@(v, z) sistema_discr(v, z, u_discr), tspan, z0);
 
         q = [q; z];
         t = [t; v];
         
-        u_discr = control_act(t(end), q(end, :));
+        [u_discr, u_track, u_corr, U_corr_history] = 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'];
     end
 
-    ref_t = double(subs(ref, t'))';
+    ref_t = double(subs(sim_data.ref, t'))';
 end
 
 
@@ -111,27 +124,48 @@ function [t, q, ref, U] = simulate_cont(tfin)
     % plot results
     ref = double(subs(ref, t'))';
 end
+%% 
 
 % Plots
-function plot_results(t, x, ref, U)
-    subplot(2,2,1)
+function plot_results(t, x, ref, U, U_track, U_corr)
+    subplot(4,2,1)
     hold on
+    title("trajectory / state")
     plot(ref(:, 1), ref(:, 2), "DisplayName", "Ref")
     plot(x(:, 1), x(:, 2), "DisplayName", "state")
     xlabel('x')
     ylabel('y')
     legend()
-    subplot(2,2,3)
+    subplot(4,2,3)
     plot(t, U(:, 1))
     xlabel('t')
     ylabel('input v')
-    subplot(2,2,4)
+    subplot(4,2,4)
     plot(t, U(:, 2))
     xlabel('t')
     ylabel('input w')
+
+    subplot(4,2,5)
+    plot(t, U_corr(:, 1))
+    xlabel('t')
+    ylabel('input correction v')
+    subplot(4,2,6)
+    plot(t, U_corr(:, 2))
+    xlabel('t')
+    ylabel('input correction w')
     
     
-    subplot(4,4,3)
+    subplot(4,2,7)
+    plot(t, U_track(:, 1))
+    xlabel('t')
+    ylabel('tracking input v')
+    subplot(4,2,8)
+    plot(t, U_track(:, 2))
+    xlabel('t')
+    ylabel('tracking input w')
+    
+    
+    subplot(8,4,3)
     hold on
     xlabel('t')
     ylabel('x')
@@ -140,12 +174,12 @@ function plot_results(t, x, ref, U)
     legend()
     hold off
     
-    subplot(4,4,4)
+    subplot(8,4,4)
     plot(t, ref(:, 1) - x(:, 1));
     xlabel('t')
     ylabel('x error')
     
-    subplot(4,4,7)
+    subplot(8,4,7)
     hold on
     xlabel('t')
     ylabel('y')
@@ -154,8 +188,9 @@ function plot_results(t, x, ref, U)
     legend()
     hold off
     
-    subplot(4,4,8)
+    subplot(8,4,8)
     plot(t, ref(:, 2) - x(:, 2));
     xlabel('t')
     ylabel('y error')
+   
 end

unicycle.m

@@ -1,7 +1,7 @@
-function dx = unicycle(t, x, u)
+function dq = unicycle(t, q, u)
     % u is (v;w)
     % x is (x; y; theta)
-    theta = x(3);
-    G_x = [cos(theta), 0; sin(theta), 0; 0, 1];
-    dx = G_x*u;
+    theta = q(3);
+    G_q = [cos(theta), 0; sin(theta), 0; 0, 1];
+    dq = G_q*u;
 end