Skip to content
Permalink
Browse files

adding feasible control constraint to Quad4DCAvoid

  • Loading branch information...
mochen72 committed May 3, 2018
1 parent 64ae5e2 commit c541073f3038f999d22d67d75fc10d5f4a702102
@@ -0,0 +1,36 @@
function [m, b, mNorm] = feas_ctrl_constr(obj, ~, x, deriv, dMode)
% [m, b] = feas_ctrl_constr(obj, ~, x, deriv, uMode, dMode)
%
% Feasible control constraints in the form of m*u + b >= 0 or <= 0

if nargin < 5
dMode = 'min';
end

dOpt = obj.optDstb([], [], deriv, dMode);

% Compute b using Hamiltonian with zero control
b = 0;
uZero = {zeros(size(x{1})); zeros(size(x{1}))};
dx = obj.dynamics([], x, uZero, dOpt);
for i = 1:obj.nx
b = b + deriv{i}.*dx{i};
end

% Compute m by hard-coding terms in Hamiltonian involving u
m = cell(obj.nu, 1);
if any(obj.dims==2)
m{1} = -deriv{obj.dims==2};
end

if any(obj.dims==4)
m{2} = -deriv{obj.dims==4};
end

mNorm = sqrt(m{1}.^2 + m{2}.^2);

m{1}(mNorm>0) = m{1}(mNorm>0)./mNorm(mNorm>0);
m{2}(mNorm>0) = m{2}(mNorm>0)./mNorm(mNorm>0);
b(mNorm>0) = b(mNorm>0)./mNorm(mNorm>0);

end
@@ -0,0 +1,50 @@
function feas_ctrl_constr_test(dCar, g, data, deriv)
% [m, b] = feas_ctrl_constr(obj, ~, x, deriv, uMode, dMode)
%
% Feasible control constraints in the form of m*u + b >= 0 or <= 0

dMode = 'min';

% Velocity slice
vx = 4;
vy = 2;

% Visualize reachable set
[g2D, data2D] = proj(g, data(:,:,:,:,end), [0 1 0 1], [vx vy]);

figure;
visSetIm(g2D, data2D, 'r');

% Compute gradient at the points on the v slice
derivN = {deriv{1}(:,:,:,:,end); deriv{2}(:,:,:,:,end);
deriv{3}(:,:,:,:,end); deriv{4}(:,:,:,:,end)};

xsSlice = [g2D.xs{1}(:) vx*ones(size(g2D.xs{1}(:))) g2D.xs{2}(:) ...
vy*ones(size(g2D.xs{1}(:)))];

deriv_xy = cell(4,1);
for i = 1:4
deriv_xy{i} = eval_u(g, derivN{i}, xsSlice);
deriv_xy{i} = reshape(deriv_xy{i}, g2D.shape);
end

% Compute feasible controls
xsSliceCell = {g2D.xs{1}; vx*ones(size(g2D.xs{1})); g2D.xs{2}; ...
vy*ones(size(g2D.xs{1}))};

[m, b, mNorm] = dCar.feas_ctrl_constr([], xsSliceCell, deriv_xy, dMode);

% Plot feasible controls
hold on

quiver(g2D.xs{1}, g2D.xs{2}, m{1}, m{2});

ctrl_all_feas = mNorm==0 & b >= 0;
plot(g2D.xs{1}(ctrl_all_feas), g2D.xs{2}(ctrl_all_feas), 'o')

% Plot locations where no control is feasible
ctrl_all_infeas = mNorm==0 & b < 0;
plot(g2D.xs{1}(ctrl_all_infeas), g2D.xs{2}(ctrl_all_infeas), 'x')

axis equal
end
@@ -0,0 +1,49 @@
function [data, tau2] = Quad4DCAvoid_test()

%% Grid
grid_min = [-8; -8; -5; -3]; % Lower corner of computation domain
grid_max = [8; 8; 5; 3]; % Upper corner of computation domain
N = 21*ones(4,1); % Number of grid points per dimension

g = createGrid(grid_min, grid_max, N);

%% target set
R = 3;
data0 = shapeCylinder(g, [2, 4], [0; 0; 0; 0], R);

%% time vector
t0 = 0;
tMax = 3;
dt = 0.1;
tau = t0:dt:tMax;

%% problem parameters
aMax = [6 2];
bMax = [6 2];

uMode = 'max';
dMode = 'min';
% do dStep2 here


%% Pack problem parameters
dCar = Quad4DCAvoid([0,0,0,0], aMax, bMax);

% Put grid and dynamic systems into schemeData
schemeData.grid = g;
schemeData.dynSys = dCar;
schemeData.accuracy = 'veryHigh'; %set accuracy
schemeData.uMode = uMode;
schemeData.dMode = dMode;

%% Compute value function
HJIextraArgs.visualize = true;
HJIextraArgs.fig_num = 1;
HJIextraArgs.deleteLastPlot = true;
HJIextraArgs.stopConverge = true;

[data, tau2] = HJIPDE_solve(data0, tau, schemeData, 'zero', HJIextraArgs);

deriv = computeGradients(g, data);

end

0 comments on commit c541073

Please sign in to comment.
You can’t perform that action at this time.