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

correction is on untrasformed inputs, but cost and constraints are on robot inputs

master-costfun2
EmaMaker 2024-09-16 18:04:18 +02:00
parent ceb7659bcc
commit 41f0d66851
2 changed files with 24 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
control_act.m
View File

@ -2,9 +2,10 @@ function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data)
dc = decouple_matrix(q, sim_data); dc = decouple_matrix(q, sim_data);
ut = utrack(t, q, sim_data); ut = utrack(t, q, sim_data);
ut = dc*ut; ut = dc*ut;
[uc, U_corr_history, q_pred] = ucorr(t, q, sim_data); [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
uc = dc*uc;
u = ut+uc; u = ut+uc;
% saturation % saturation
u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u)); u = min(sim_data.SATURATION, max(-sim_data.SATURATION, u));
@ -28,20 +29,21 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
if eq(pred_hor, 0) if eq(pred_hor, 0)
return return
elseif eq(pred_hor, 1) elseif eq(pred_hor, 1)
H = [1, 0; 0, -1];
%H = eye(2);
f = zeros(2,1);
T_inv = decouple_matrix(q_act, sim_data); T_inv = decouple_matrix(q_act, sim_data);
ut = utrack(t, q_act, sim_data); ut = utrack(t, q_act, sim_data);
%A = [T_inv; -T_inv];
A = [eye(2); -eye(2)]; H = 2 * (T_inv') * T_inv;
%H = eye(2);
f = zeros(2,1);
A = [T_inv; -T_inv];
%A = [eye(2); -eye(2)];
d = T_inv*ut; d = T_inv*ut;
b = [s_-d;s_+d]; b = [s_-d;s_+d];
% solve qp problem % solve qp problem
options = optimoptions('quadprog'); options = optimoptions('quadprog', 'Display', 'off');
u_corr = quadprog(H, f, A, b, [],[],[],[],[],options) u_corr = quadprog(H, f, A, b, [],[],[],[],[],options);
q_pred = q_act; q_pred = q_act;
U_corr_history(:,:,1) = u_corr; U_corr_history(:,:,1) = u_corr;
@ -69,7 +71,7 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
T_inv = decouple_matrix(q_act, sim_data); T_inv = decouple_matrix(q_act, sim_data);
% compute inputs (v, w)/(wr, wl) % compute inputs (v, w)/(wr, wl)
u_ = T_inv * u_track_ + u_corr_; u_ = T_inv * (u_track_ + u_corr_);
% if needed, map (wr, wl) to (v, w) for unicicle % if needed, map (wr, wl) to (v, w) for unicicle
@ -117,15 +119,23 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
% A will be at most PREDICTION_HORIZON * 2 * 2 (2: size of T_inv; 2: % 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 (number of % accounting for T_inv and -T_inv) by PREDICTION_HORIZON (number of
% vectors in u_corr times the number of elements [2] in each vector) % vectors in u_corr times the number of elements [2] in each vector)
A_deq = [];
b_deq = []; b_deq = [];
H1 = [];
for k=1:pred_hor for k=1:pred_hor
T_inv = T_inv_pred(:,:,k); T_inv = T_inv_pred(:,:,k);
u_track = u_track_pred(:,:,k); u_track = u_track_pred(:,:,k);
d = T_inv*u_track; d = T_inv*u_track;
H1 = blkdiag(H1, T_inv);
H2 = blkdiag(H2, T_inv');
A_deq = blkdiag(A_deq, [T_inv; -T_inv]);
b_deq = [b_deq; s_ - d; s_ + d]; b_deq = [b_deq; s_ - d; s_ + d];
end end
H = H1'*H1;
A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]); %A_deq = kron(eye(pred_hor), [eye(2); -eye(2)]);
%A_deq %A_deq
%b_deq %b_deq
% unknowns % unknowns
@ -133,7 +143,7 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
% squared norm of u_corr. H must be identity, % squared norm of u_corr. H must be identity,
% PREDICTION_HORIZON*size(u_corr) % PREDICTION_HORIZON*size(u_corr)
%H = eye(pred_hor*2)*2; %H = eye(pred_hor*2)*2;
H = kron(eye(pred_hor), [1,0;0,0]); %H = kron(eye(pred_hor), [1,0;0,0]);
% no linear terms % no linear terms
f = zeros(pred_hor*2, 1); f = zeros(pred_hor*2, 1);

3
tesi.m
View File

@ -24,6 +24,7 @@ for i = 1:length(TESTS)
sim_data.(fn{1}) = test_data.(fn{1}); sim_data.(fn{1}) = test_data.(fn{1});
end end
sim_data.r = 0.06
% set trajectory and starting conditions % set trajectory and starting conditions
sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS); sim_data.q0 = set_initial_conditions(sim_data.INITIAL_CONDITIONS);
[ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data); [ref dref] = set_trajectory(sim_data.TRAJECTORY, sim_data);
@ -35,7 +36,7 @@ for i = 1:length(TESTS)
% 1: track only % 1: track only
% 2: track + 1step % 2: track + 1step
% 3: track + multistep % 3: track + multistep
spmd (2) spmd (3)
worker_index = spmdIndex; worker_index = spmdIndex;
% load controller-specific options % load controller-specific options
data = load(['tests/' num2str(worker_index) '.mat']); data = load(['tests/' num2str(worker_index) '.mat']);