restructure code to allow execution in parallel

these commits are so messy

control_act.m

@@ -1,42 +1,40 @@
-function u = control_act(t, q)
+function u = control_act(t, q, sim_data)   
-    global SATURATION
+    dc = decouple_matrix(q, sim_data.b);
-    
+    ut = utrack(t, q, sim_data);
-    dc = decouple_matrix(q);
+    uc = ucorr(t, q, sim_data);
-    ut = utrack(t,q);
-    uc = ucorr(t,q);
     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)
+function u_corr = ucorr(t, q, sim_data)
-    global SATURATION PREDICTION_SATURATION_TOLERANCE PREDICTION_HORIZON tc
+    pred_hor = sim_data.PREDICTION_HORIZON;
-    
+    SATURATION = sim_data.SATURATION;
-    if eq(PREDICTION_HORIZON, 0)
+    PREDICTION_SATURATION_TOLERANCE = sim_data.PREDICTION_SATURATION_TOLERANCE;
+    tc = sim_data.tc;
+
+    if eq(pred_hor, 0)
         u_corr = zeros(2,1);
         return
     end
 
     persistent U_corr_history;
     if isempty(U_corr_history)
-        U_corr_history = zeros(2, 1, PREDICTION_HORIZON);
+        U_corr_history = zeros(2, 1, pred_hor);
     end
     
     %disp('start of simulation')
     q_prec = q;
-    %q_pred = [];
+    q_pred=zeros(3,1, pred_hor);
-    %u_track_pred = [];
+    u_track_pred=zeros(2,1, pred_hor+1);
-    %t_inv_pred = [];
+    T_inv_pred=zeros(2,2, pred_hor+1);
-    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);
     % 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
     % 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
@@ -45,9 +43,9 @@ function u_corr = ucorr(t,q)
         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_ = utrack(t_, q_prec, sim_data);
         
-        T_inv = decouple_matrix(q_prec);
+        T_inv = decouple_matrix(q_prec, sim_data.b);
         u_ = T_inv * (u_corr_ + u_track_);
         
         % calc the state integrating with euler
@@ -70,11 +68,11 @@ function u_corr = ucorr(t,q)
     %q_prec
     
     % calculate u_track_k+C
-    u_track_pred(:,:,PREDICTION_HORIZON+1) = utrack(t+tc*PREDICTION_HORIZON, q_prec);
+    u_track_pred(:,:,pred_hor+1) = utrack(t+tc*pred_hor, q_prec, sim_data);
     % 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(:,:,pred_hor+1) = decouple_matrix(q_prec, sim_data.b);
     T_inv_pred = T_inv_pred(:,:,2:end);
 
     %disp('end of patching data up')
@@ -99,11 +97,11 @@ 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
+    for k=1:pred_hor
         T_inv = T_inv_pred(:,:,k);
         u_track = u_track_pred(:,:,k);
 
@@ -126,9 +124,9 @@ function u_corr = ucorr(t,q)
     
     % 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 +134,26 @@ 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]);    
+    U_corr_history = reshape(U_corr, [2,1,pred_hor]);    
 
     u_corr=U_corr_history(:,:, 1);
     
 end
 
-function u_track = utrack(t, q)    
+function u_track = utrack(t, q, sim_data)    
-    global ref dref K
+    ref_s = double(subs(sim_data.ref, t));
-    ref_s = double(subs(ref, t));
+    dref_s = double(subs(sim_data.dref, t));
-    dref_s = double(subs(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)
+function q_track = feedback(q, b)
-    global b
     q_track = [q(1) + b*cos(q(3)); q(2) + b*sin(q(3))  ];
 end
 
-function T_inv = decouple_matrix(q)
+function T_inv = decouple_matrix(q, b)
-    global b
-
     theta = q(3);
     st = sin(theta);
     ct = cos(theta);

sistema.m

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

sistema_discr.m

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

tesiema.m

@@ -3,91 +3,58 @@ clear all
 close all
 
 %% global variables
-global q0 ref dref b tc K SATURATION tc tfin USE_PREDICTION PREDICTION_HORIZON PREDICTION_SATURATION_TOLERANCE;
+global K SATURATION PREDICTION_HORIZON PREDICTION_SATURATION_TOLERANCE;
 
-%% variables
+sim_data = load(['tests/sin/common.mat']);
-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
 
-tc = 0.1
+sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS);
-tfin=30
+[ref dref] = set_trajectory(sim_data.TRAJECTORY);
+sim_data.ref = ref;
+sim_data.dref = dref;
 
-% saturation
+spmd (3)
-% HYP: a diff. drive robot with motors spinning at 100rpm -> 15.7 rad/s.
+    worker_index = spmdIndex;
-% Radius of wheels 10cm. Wheels distanced 15cm from each other
+    data = load(['tests/sin/sin' num2str(spmdIndex) '.mat']);
-% applying transformation, v
+    
-% saturation = [1.57, 20];
+    sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON;
-SATURATION = [1; 1];
+    sim_data
-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)
-
-global tu uu
-
-figure(1)
-PREDICTION_HORIZON = 0;
-[t, q, ref_t, U] = simulate_discr(tfin);
-plot_results(t, q, ref_t, U);
-
-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);
 
+    [t, q, ref_t, U] = simulate_discr(sim_data);
+end
 
+s_ = size(worker_index);
+for n = 1:s_(2)
+    figure(n)
+    plot_results(t{n}, q{n}, ref_t{n}, U{n});
+end
 
 %% FUNCTION DECLARATIONS
 
 % Discrete-time simulation
-function [t, q, ref_t, U] = simulate_discr(tfin)
+function [t, q, ref_t, U] = simulate_discr(sim_data)
-    global ref q0 u_discr tc
+    tc = sim_data.tc;
-
+    steps = sim_data.tfin/tc
-    steps = tfin/tc
     
-    q = q0';
+    q = sim_data.q0';
     t = 0;
-    u_discr = control_act(t, q0);
+    u_discr = control_act(t, q, sim_data);
     U = u_discr';
     
     for n = 1:steps
         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 = control_act(t(end), q(end, :), sim_data);
         U = [U; ones(length(v), 1)*u_discr'];
     end
 
-    ref_t = double(subs(ref, t'))';
+    ref_t = double(subs(sim_data.ref, t'))';
 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);
+    theta = q(3);
-    G_x = [cos(theta), 0; sin(theta), 0; 0, 1];
+    G_q = [cos(theta), 0; sin(theta), 0; 0, 1];
-    dx = G_x*u;
+    dq = G_q*u;
 end