diff --git a/control_act.m b/control_act.m index 2451471..8c1b5ec 100644 --- a/control_act.m +++ b/control_act.m @@ -32,26 +32,32 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data) T_inv = decouple_matrix(q_act, sim_data); ut = utrack(t, q_act, sim_data); - % minimize v, unicycle and ddr - % actually minimize xdot^2 + ydot^2 = v - H = 2*eye(2); - - % minimize vcorr^2 + wcorr^2, unicycle and ddr - %H = T_inv - + if eq(sim_data.costfun,1) + % minimize v, unicycle and ddr + % actually minimize xdot^2 + ydot^2 = v + H = 2*eye(2); + elseif eq(sim_data.costfun,2) + % minimize vcorr^2 + wcorr^2, unicycle and ddr + H = T_inv; + end if eq(sim_data.robot, 0) - % ex3: unicycle, minimize v - %H = T_inv' * [1 0; 0 0] * T_inv; - % ex4: unicycle, minimize w - %H = T_inv' * [0 0; 0 1] * T_inv; + if eq(sim_data.costfun, 3) + % ex3: unicycle, minimize v + H = T_inv' * [1 0; 0 0] * T_inv; + elseif eq(sim_data.costfun, 4) + % ex4: unicycle, minimize w + H = T_inv' * [0 0; 0 1] * T_inv; + end else - % ex1: ddr, minimize v. det(H) = 0 H not symmetric - %R = [sim_data.r/2 sim_data.r/2] - %H = T_inv' * R' * R * T_inv; - - % ex2: ddr, minimize w. det(H) = 0 - %R = [sim_data.r/sim_data.d -sim_data.r/sim_data.d] - %H = T_inv' * R' * R * T_inv; + if eq(sim_data.costfun, 3) + % ex1: ddr, minimize v. det(H) = 0 H not symmetric + R = [sim_data.r/2 sim_data.r/2] + H = T_inv' * R' * R * T_inv; + elseif eq(sim_data.costfun, 4) + % ex2: ddr, minimize w. det(H) = 0 + R = [sim_data.r/sim_data.d -sim_data.r/sim_data.d] + H = T_inv' * R' * R * T_inv; + end end f = zeros(2,1); @@ -62,8 +68,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data) b = [s_-d;s_+d]; % solve qp problem - options = optimoptions('quadprog', 'Display', 'off'); - u_corr = quadprog(H, f, A, b, [],[],[],[],[],options); + options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set'); + u_corr = quadprog(H, f, A, b, [],[],[],[],zeros(2,1),options); q_pred = q_act; U_corr_history(:,:,1) = u_corr; @@ -141,6 +147,7 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data) % vectors in u_corr times the number of elements [2] in each vector) A_deq = []; b_deq = []; + H = []; for k=1:pred_hor T_inv = T_inv_pred(:,:,k); u_track = u_track_pred(:,:,k); @@ -148,6 +155,36 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data) A_deq = blkdiag(A_deq, [T_inv; -T_inv]); b_deq = [b_deq; s_ - d; s_ + d]; + + + if eq(sim_data.costfun,1) + % minimize v, unicycle and ddr + % actually minimize xdot^2 + ydot^2 = v + H = blkdiag(H, 2*eye(2)); + elseif eq(sim_data.costfun,2) + % minimize vcorr^2 + wcorr^2, unicycle and ddr + H = blkdiag(H, T_inv); + H = kron(eye(pred_hor), T_inv(:, :, :)); + end + if eq(sim_data.robot, 0) + if eq(sim_data.costfun, 3) + % ex3: unicycle, minimize v + H = blkdiag(H, T_inv' * [1 0; 0 0] * T_inv); + elseif eq(sim_data.costfun, 4) + % ex4: unicycle, minimize w + H = blkdiag(H, T_inv' * [0 0; 0 1] * T_inv); + end + else + if eq(sim_data.costfun, 3) + % ex1: ddr, minimize v. det(H) = 0 H not symmetric + R = [sim_data.r/2 sim_data.r/2]; + H = blkdiag(H, T_inv' * (R') * R * T_inv); + elseif eq(sim_data.costfun, 4) + % ex2: ddr, minimize w. det(H) = 0 + R = [sim_data.r/sim_data.d -sim_data.r/sim_data.d]; + H = blkdiag(H, T_inv' * (R') * R * T_inv); + end + end end %A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]); @@ -158,14 +195,15 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data) % squared norm of u_corr. H must be identity, % PREDICTION_HORIZON*size(u_corr) %H = eye(pred_hor*2)*2; - H = kron(eye(pred_hor), 2*eye(2)); %H = kron(eye(pred_hor), 2*ones(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); + options = optimoptions('quadprog', 'Display', 'off', 'Algorithm', 'active-set'); + U_corr = quadprog(H, f, A_deq, b_deq, [],[],[],[],zeros(2*pred_hor, 1),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 diff --git a/plot_all.m b/plot_all.m index 28d4773..250bbd5 100644 --- a/plot_all.m +++ b/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-diffdrive-costfun-ddronly/circle/start_center/ddr-minv-activeset"; + "results-diffdrive-costfun-ddronly/circle/start_center/ddr-minw-activeset"; ] s_ = size(PLOT_TESTS) diff --git a/tesi.m b/tesi.m index af18749..be39596 100644 --- a/tesi.m +++ b/tesi.m @@ -31,12 +31,14 @@ for i = 1:length(TESTS) sim_data.ref = ref; sim_data.dref = dref; %sim_data.tfin = 15; + sim_data.costfun=4 + sim_data.tc=0.05 % spawn a new worker for each controller % 1: track only % 2: track + 1step % 3: track + multistep - spmd (2) + spmd (3) worker_index = spmdIndex; % load controller-specific options data = load(['tests/' num2str(worker_index) '.mat']); diff --git a/tesi_st.m b/tesi_st.m new file mode 100644 index 0000000..66af356 --- /dev/null +++ b/tesi_st.m @@ -0,0 +1,130 @@ +clc +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 = ["figure8/chill"] +CONTROLLER = 3 + +% main +s_ = size(TESTS); +for i = 1:length(TESTS) + clearvars -except i s_ TESTS ROBOT CONTROLLER + close all + + % load simulation parameters common to all robots and all tests + sim_data = load(["tests/robot_common.mat"]); + + TEST = convertStringsToChars(TESTS(i)) + + % load test data (trajectory, etc) + test_data = load(['tests/' TEST '/common.mat']); + for fn = fieldnames(test_data)' + sim_data.(fn{1}) = test_data.(fn{1}); + end + + % set trajectory and starting conditions + sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS); + [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data); + sim_data.ref = ref; + sim_data.dref = dref; + + % spawn a new worker for each contzroller + % 1: track only + % 2: track + 1step + % 3: track + multistep + % load controller-specific options + data = load(['tests/' num2str(CONTROLLER) '.mat']); + for fn = fieldnames(data)' + sim_data.(fn{1}) = data.(fn{1}); + end + + % load robot-specific options + % put here to overwrite any parameter value left over in the tests + % .mat files, just in case + data = load(['tests/' ROBOT '.mat']); + for fn = fieldnames(data)' + sim_data.(fn{1}) = data.(fn{1}); + 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, Prob] = simulate_discr(sim_data); + toc; + + disp('Done') + + % save simulation data + f1 = [ TEST '/' char(datetime, 'dd-MM-yyyy-HH-mm-ss')]; % windows compatible name + f = ['results-' ROBOT '-costfun2-st/' f1]; + mkdir(f) + % save workspace + dsave([f '/workspace_composite.mat']); + % save test file + copyfile(['tests/' TEST], f); + + % save figures + plot results + % plot results + h = figure('Name', [TEST ' ' num2str(CONTROLLER)] ); + plot_results(t, q, ref_t, U, U_track, U_track); + % save figures + savefig(h, [f '/figure.fig']); +end + + +%% FUNCTION DECLARATIONS + +% Discrete-time simulation +function [t, q, y, ref_t, U, U_track, Q_pred, Prob] = 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, steps + 1); + Prob = cell(steps+1, 1); + + [u_discr, u_track, q_pred, prob] = control_act(t(end), q(end, :), sim_data); + U = u_discr'; + U_track = u_track'; + Q_pred(:, :, 1) = q_pred; + Prob{1} = prob; + + if eq(sim_data.robot, 0) + fun = @(t, q, u_discr, sim_data) unicycle(t, q, u_discr, sim_data); + elseif eq(sim_data.robot, 1) + fun = @(t, q, u_discr, sim_data) diffdrive(t, q, u_discr, sim_data); + end + + for n = 1:steps + tspan = [(n-1)*tc n*tc]; + z0 = q(end, :); + + opt = odeset('MaxStep', 0.005); + [v, z] = ode45(@(v, z) fun(v, z, u_discr, sim_data), tspan, z0, opt); + + q = [q; z]; + t = [t; v]; + + [u_discr, u_track, q_pred, prob] = control_act(t(end), q(end, :), sim_data); + Prob{1+n} = prob; + U = [U; ones(length(v), 1)*u_discr']; + U_track = [U_track; ones(length(v), 1)*u_track']; + Q_pred(:, :, 1+n) = q_pred; + end + + y1 = q(:, 1) + sim_data.b * cos(q(:,3)); + y2 = q(:, 2) + sim_data.b * sin(q(:,3)); + y = [y1, y2]; + + ref_t = double(subs(sim_data.ref, t'))'; +end + +%%