Homec4science
Diffusion QPFit (master) Browse

QPFit/gen_bandp.m
3bfb25b5e70amaster

Last Change
Raw File

gen_bandp.m
View Options

clear
analyze = false;
generate_explicit = false;
%%
Ts = 0.1;
N = 10;
s = tf('s');
% Transfer function from reference to angle to angle / force
tau = Ts*1.5;
tableServo = 1/(tau*s+1);
ballDynamics = 1/s^2;
[sA,sB,sC,sD] = ssdata(tableServo);
[bA,bB,bC,bD] = ssdata(ballDynamics);
A = [bA bB*sC; 0 0 sA];
B = [0;0;sB];
C = [bC 0; 1 0 0; 0 0 sC]; % Pos; Vel; Table
D = zeros(3,1);
% step(ss(A,B,C,zeros(3,1),'OutputName',{'Pos','Vel','Table'}), 10)
% y1 == position of the ball
% y2 == velocity of the ball
% y3 == angle of the table
Itable = 3;
Ivel = 2;
Ipos = 1;
[A,B,C,D] = ssdata(c2d(ss(A,B,C,D), Ts));
sys = struct('A',A,'B',B,'C',C);
sys.Itable = Itable;
sys.Ivel = Ivel;
sys.Ipos = Ipos;
sys.Ts = Ts;
% sys = ss(A,B,C,D,Ts,'OutputName',{'BallPos','BallVel','TableAngle'});
% clf
% step(sys, 10)
%%
[n,m] = size(B);
x = sdpvar(n,N);
u = sdpvar(m,N-1);
r = sdpvar();
maxBallPos = 10;
maxBallVel = 1.5;
maxTableAngle = 2;
sys.maxBallPos = maxBallPos;
sys.maxBallVel = maxBallVel;
sys.maxTableAngle = maxTableAngle;
ballPos = C(Ipos,:) * x;
ballVel = C(Ivel,:) * x;
tableAngle = C(Itable,:) * x;
gnd.tableAngle = tableAngle;
gnd.ballVel = ballVel;
gnd.ballPos = ballPos;
gnd.u = u;
gnd.x = x;
gnd.r = r;
gnd.N = N;
con = [];
con = [con, -maxBallPos <= ballPos(:,2:end) <= maxBallPos];
con = [con, -maxBallVel <= ballVel(:,2:end) <= maxBallVel];
con = [con, -maxTableAngle <= tableAngle(:,2:end) <= maxTableAngle];
con = [con, -maxTableAngle <= u <= maxTableAngle];
for i = 1:N-1
con = [con, x(:,i+1) == A * x(:,i) + B * u(:,i)];
end
% Tracking objective
q = inv([A-eye(n) B; C(Ipos,:) 0]) * [zeros(n,1); 1];
xs = q(1:n);
us = q(n+1:end);
obj = 0;
for i = 2:N
err = ballPos(i) - r;
obj = obj + err'*err;
end
for i = 1:N-2
deltaU = u(:,i+1) - u(:,i);
obj = obj + deltaU'*deltaU;
end
% Reference bound
con = [con, -10 <= r <= 10];
gnd.opt = optimizer(con, obj, sdpsettings('solver','gurobi'), ...
{x(:,1), r}, u(:,1));
if generate_explicit && ~analyze
gnd.prob = Opt(con,obj,[x(:,1); r],u(:,1));
gnd.sol = gnd.prob.solve();
save bandp_explicit gnd
% else
% load bandp_explicit
end
%% Plot trajectory
x0 = [0;0;0];
r = 9.8487;
plotStarTraj(x0, r, con, obj, sys, gnd)
%%
% Simulate tracking
% Track a star
TrackSpeed = 0.0078;
% TrackSpeed = 0.011;
StarLength = 9;
TSim = StarLength / TrackSpeed * Ts;
% TSim = ceil(90 * 9/7 / Ts) * Ts;
nSim = ceil(TSim / Ts);
t = linspace(0,TSim,nSim);
% TrackSpeed = 9 / nSim; % One times around the track during the sim
p = 9; % Number of points
q = 3; % Number of points to increment
% DistanceToTravel = speed * TSim;
% MaxTH = DistanceToTravel / numSegs;
radius = 10;
% R = radius * star(7, 3, nSim, TrackSpeed, true);
[R,seg] = star(p, q, nSim, TrackSpeed, true);
R = R * radius;
%%
x0 = [0;0;0;0;0;0];
fprintf('Simulating ground truth...\n')
sim = simStar(@(x,r) gnd.opt({x, r}), x0, R, sys, 1);
% fprintf('Simulating approximate controller...\n')
% sim{2} = simStar(@(x,r) fit.ctrl([x; r]), x0, R, sys, 3);
%
% [sim{1}.MSE sim{2}.MSE]
%%
% Sample points
if ~analyze
datafile = 'bandp';
F = gnd.sol.xopt.convexHull();
nBatch = 200;
nSamples = nBatch * 1;
n = 4;
m = 1;
X = [];
U = [];
Set = gnd.sol.xopt.Set;
i = 1;
Fail = 0;
tic
t0 = datetime;
Set = gnd.sol.xopt.Set;
while i < nSamples
% Generate 10000 feasible samples
fprintf('\tSampling points\n');
x0 = sample_polytope(F, nBatch);
fprintf('\tDetermining regions\n');
Region = -ones(size(x0,2),1);
for j = 1:length(Set)
if toc > 2
fprintf('Tested %i regions of %i\n', j, length(Set));
tic
end
I = all(Set(j).A * x0 <= Set(j).b);
Region(I) = j;
end
Remove = Region == -1;
x0(:, Remove) = [];
Region(Remove) = [];
if sum(Remove) > 0, fprintf('\t\tRemoved %i\n', sum(Remove)); end
fprintf('\t\t%i of %i regions covered\n', length(unique(Region)), ...
length(Set));
fprintf('\tEvaluating functions\n');
u = NaN*zeros(m, size(x0,2));
for k = 1:length(Set)
if toc > 2
fprintf('Tested %i regions of %i\n', k, length(Set));
tic
end
I = Region == k;
if all(~I), continue; end
u(:,I) = Set(k).Func{2}.F * x0(:,I) + Set(k).Func{2}.g;
end
Remove = isnan(u);
x0(:, Remove) = [];
Region(Remove) = [];
u(:, Remove) = [];
if sum(Remove) > 0, fprintf('\t\tRemoved %i\n', sum(Remove)); end
X = [X x0];
U = [U u];
i = size(X,2);
filename = sprintf('data/%s_%i', datafile, i);
save(filename,'X','U')
fprintf('%i of %i samples found\n', i, nSamples);
end
fprintf('DONE!\n');
end
%%
% Load and test learned solution
% load data/bandp_200000_fit_var10.mat
c4science · Help