diff --git a/control_act.m b/control_act.m index c11e943..7e72174 100644 --- a/control_act.m +++ b/control_act.m @@ -1,42 +1,40 @@ -function u = control_act(t, q) - global SATURATION - - dc = decouple_matrix(q); - ut = utrack(t,q); - uc = ucorr(t,q); +function u = control_act(t, q, sim_data) + dc = decouple_matrix(q, sim_data.b); + ut = utrack(t, q, sim_data); + uc = 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) +function u_corr = 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; + + 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 = []; - %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_pred=zeros(3,1, pred_hor); + u_track_pred=zeros(2,1, pred_hor+1); + T_inv_pred=zeros(2,2, pred_hor+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) - 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); diff --git a/sistema.m b/sistema.m index 7f4641e..aee1e93 100644 --- a/sistema.m +++ b/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 diff --git a/sistema_discr.m b/sistema_discr.m index 4f8a366..ccde484 100644 --- a/sistema_discr.m +++ b/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 diff --git a/tesi-spmd.m b/tesi-spmd.m new file mode 100644 index 0000000..e69de29 diff --git a/tesiema.m b/tesiema.m index 431c420..d4f6b73 100644 --- a/tesiema.m +++ b/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 -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 +sim_data = load(['tests/sin/common.mat']); -tc = 0.1 -tfin=30 +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) - -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); +spmd (3) + worker_index = spmdIndex; + data = load(['tests/sin/sin' num2str(spmdIndex) '.mat']); + + sim_data.PREDICTION_HORIZON = data.PREDICTION_HORIZON; + sim_data + [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) - global ref q0 u_discr tc - - steps = tfin/tc +function [t, q, ref_t, U] = 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 = 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 diff --git a/tesiema.mlx b/tesiema_live.mlx similarity index 100% rename from tesiema.mlx rename to tesiema_live.mlx diff --git a/tesiema_sinusoide_diff_1step_track.mlx b/tesiema_sinusoide_diff_1step_track.mlx index e5f4e12..78cbc96 100644 Binary files a/tesiema_sinusoide_diff_1step_track.mlx and b/tesiema_sinusoide_diff_1step_track.mlx differ diff --git a/tests/sin/common.mat b/tests/sin/common.mat new file mode 100644 index 0000000..3397ae8 Binary files /dev/null and b/tests/sin/common.mat differ diff --git a/tests/sin/sin1.mat b/tests/sin/sin1.mat new file mode 100644 index 0000000..af58e4d Binary files /dev/null and b/tests/sin/sin1.mat differ diff --git a/tests/sin/sin2.mat b/tests/sin/sin2.mat new file mode 100644 index 0000000..a7f7bb5 Binary files /dev/null and b/tests/sin/sin2.mat differ diff --git a/tests/sin/sin3.mat b/tests/sin/sin3.mat new file mode 100644 index 0000000..4b8bfb2 Binary files /dev/null and b/tests/sin/sin3.mat differ diff --git a/unicycle.m b/unicycle.m index e7e7a7e..32475ee 100644 --- a/unicycle.m +++ b/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