Homec4science
Diffusion QPFit (master) Browse

QPFit/gen_doubleintegrator.m
3bfb25b5e70amaster

Last Change
Raw File

gen_doubleintegrator.m
View Options

clear
N = 5;
nSamples = 1000;
ROI = 5;
sys.A = [1 1; 0 1];
sys.B = [1; 0.5];
sys.C = [1 0];
sys.D = 0;
x = sdpvar(2, N);
u = sdpvar(1, N-1);
con = sys.A * x(:, 1:end-1) + sys.B * u == x(:, 2:end);
% for i = 1:N-1
% x(:,i+1) = sys.A * x(:,i) + sys.B * u(:,i);
% end
con = [con, -ROI <= x(:,2:end) <= ROI];
con = [con, -1 <= u <= 1];
obj = x(:)'*x(:) + u(:)'*u(:);
gnd.opt = optimizer(con, obj, sdpsettings('solver','gurobi'), x(:,1), u(:,1));
gnd.prob = Opt(con,obj,x(:,1),u(:,1))
%%
n = 2;
z = x(:,2:end);
q = [x(:,1);z(:);u(:)];
P = yal_to_poly([con, -10 <= x(:,1) <= 10], q);
F = P.projection(1:2,'ifourier');
%%
XX = [];
UU = [];
while size(XX,2) < nSamples
fprintf('%i of %i\n', size(XX,2), nSamples);
X = 3*(2 * rand(2,1000) - 1);
[U,infeas] = gnd.opt(X);
X = X(:,~infeas);
U = U(:,~infeas);
XX = [XX X];
UU = [UU U];
end
X = XX; U = UU;
save('data/double_integrator.mat','X','U')
% !scp double_integrator.mat gpu:qpth/Notebooks/
% return
%%
clf
plot3(X(1,:),X(2,:),U,'o')
grid on
%%
% gnd.sol = gnd.prob.solve
% gnd.sol.xopt.plot
% gnd.sol.xopt.fplot('primal','show_set',true,'LineWidth',3)
%% Learned model
load double_integrator_fit.mat
fit.F = double(F);
fit.f = double(f(:));
fit.L = double(L);
% min .5*z'*Q*z + (F*x + f')'*z
% s.t. z >= 0
nVar = size(fit.L,2);
nParam = size(fit.F,2);
fit.Q = fit.L'*fit.L + 1e-3*eye(nVar);
z = sdpvar(nVar,1);
x = sdpvar(nParam,1);
obj = 0.5*z'*fit.Q*z + (fit.F*x+fit.f)'*z;
g = sdpsettings('solver','gurobi+');
g.gurobi.IterationLimit = double(maxIter);
fit.opt= optimizer(z >= 0, obj, g, x, z);
fit.prob = Opt([z >= 0; -ROI <= x <= ROI],obj,x,z);
% fit.prob.bth = ROI * ones(4,1);
fit.sol = fit.prob.solve();
%%
% [Z,infeas] = fit.opt(X);
% ul = min(1, max(-1, O*Z+o));
fit.ctrl = @(x) min(1, max(-1, O*fit.opt(x)+o));
ul = fit.ctrl(X);
%%
figure(1)
clf
plot3(X(1,:),X(2,:),U,'ob')
hold on
plot3(X(1,:),X(2,:),ul,'or')
figure(3)
clf
plot3(X(1,:),X(2,:),U-ul,'.')
%%
% Simulate closed-loop system
figure(2);
clf
N = 20
for i = 1:N
fprintf('%i of %i\n', i, N);
while 1
x0 = 5 * (2*rand(2,1) - 1);
[u,infeas] = gnd.opt(x0);
if infeas == 0, break; end
end
[Y,X,U] = simsys(sys, x0, gnd.opt, 50);
plot(X(1,:), X(2,:), 'bo-');
hold on
Yss(i) = Y(end);
[Y,X,U] = simsys(sys, x0, fit.ctrl, 50);
plot(X(1,:), X(2,:), 'ro-')
fYss(i) = Y(end);
drawnow()
end
% sim.gnd.X = 5*(2*(rand(2,1) - 0.5));
% sim.fit.X = sim.gnd.X;
% while 1
% sim.gnd.U(:,end+1) = sim.gnd.opt(sim.gnd.X(:,end));
% sim.gnd.X(:,end+1) = gnd.A * sim.gnd.X(:,end) + gnd.B * sim.gnd.U(:,end);
% end
c4science · Help