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

misc tests

master-costfun2
EmaMaker 2024-09-13 20:26:01 +02:00
parent ee076259ca
commit ceb7659bcc
2 changed files with 10 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
control_act.m
View File

@ -1,11 +1,9 @@
function [u, ut, uc, U_corr_history, q_pred] = control_act(t, q, sim_data) 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);
[uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
ut = dc*ut; ut = dc*ut;
%uc = dc*uc;
%uc = zeros(2,1); [uc, U_corr_history, q_pred] = ucorr(t, q, sim_data);
u = ut+uc; u = ut+uc;
% saturation % saturation
@ -30,7 +28,8 @@ 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 = eye(2); H = [1, 0; 0, -1];
%H = eye(2);
f = zeros(2,1); 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);
@ -41,8 +40,8 @@ function [u_corr, U_corr_history, q_pred] = ucorr(t, q, sim_data)
b = [s_-d;s_+d]; b = [s_-d;s_+d];
% solve qp problem % solve qp problem
options = optimoptions('quadprog', 'Display', 'off'); options = optimoptions('quadprog');
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;
@ -133,7 +132,8 @@ 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]);
% no linear terms % no linear terms
f = zeros(pred_hor*2, 1); f = zeros(pred_hor*2, 1);

6
disaster.m
View File

@ -1,13 +1,11 @@
clc clc
clear all clear all
close all close allQQQ
%load('results-diffdrive/circle/start_center/10-09-2024 13-27-12/workspace_composite.mat') %load('results-diffdrive/circle/start_center/10-09-2024 13-27-12/workspace_composite.mat')
%load('results-diffdrive/circle/start_center/10-09-2024 15-33-08/workspace_composite.mat') load('results-diffdrive/circle/start_center/10-09-2024 15-33-08/workspace_composite.mat')
%load('/home/emamaker/documents/Università/tesi/tesi-sim/results-diffdrive/square/10-09-2024 13-53-35/workspace_composite.mat') %load('/home/emamaker/documents/Università/tesi/tesi-sim/results-diffdrive/square/10-09-2024 13-53-35/workspace_composite.mat')
load('results-diffdrive/figure8/toofast/10-09-2024-22-35-17/workspace_composite.mat')
y = cell(1,3); y = cell(1,3);