EmaMaker
/
thesis
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

activeset

master-costfun
EmaMaker 2024-10-06 16:03:19 +02:00
parent 9fcadcde54
commit a94c59c489
4 changed files with 196 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

84
control_act.m
View File

@ -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

21
plot_all.m
View File

@ -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)

4
tesi.m
View File

@ -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']);

130
tesi_st.m Normal file
View File

@ -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
%%