nav_plot.m

来自「approximate reinforcement learning」· M 代码 · 共 368 行

function figh = nav_plot(cfg)
% Plot various information regarding the navigation problem
%   FIGH = NAV_PLOT(CFG)
% Parameters:
%   CFG         - history for a simple trajectory plot;
%               or full configuration of plot, see commented defaults for explanations
%
% Returns:
%   FIGH        - an (array of) handles to the created figure(s)

% default arguments
if nargin < 1, cfg = ''; end;
% support for default calling mode -- i.e. only history
if isstruct(cfg) && isfield(cfg, 't') && isfield(cfg, 'x'),
    hist = cfg;
    cfg = struct();
    cfg.trajectory = 1;
    cfg.datasource = 'caller';
    cfg.hist = hist;
end;

% where from to load the data, one of:
%       'problem'   - if nav_problem() should be used for init; cannot be used
%           when plotting elements dependent on a learnt parameter vector (such as policy)
%       'caller'    - if the variables should be taken from the calling function
%       filename    - if data should be loaded from a file with name <filename>
CFG.datasource = 'problem';
CFG.datafile   = '';    % data source can be given as a data file here as well
% What to plot
CFG.schematic = 0;      % plot a schematic of the problem
CFG.trajectory = 0;     % if a trajectory should be plotted; extradata should contain the history
CFG.addtraj = 0;        % do not create new figure for trajectory plot, add to fig with handle cfg.addtraj
CFG.policy = 0;         % if a constant-speed slice thru the policy should be plotted
CFG.Q = 0;              % if a constant-speed slice thru the Q-function should be plotted
CFG.dampingmap = 0;     % if a colormap of the damping should be plotted
CFG.dampingsurface = 0; % if the damping surface should be plotted
CFG.both = 0;           % if both a damping colormap and a damping surface should be plotted
CFG.centers = 0;        % if centers should be highlighted on the damping surface/colormap
% plot configuration
CFG.arrowscale = .2;    % how long should be the command be scaled to obtain arrow length in policy plot
CFG.posstep = 1/3;      % the grid step for plotting the policy
CFG.markersize = 3;     % the marker size for centers, grid points, trajectory samples etc.
CFG.speedslice = [0; 0];% which speed slice through the policy/Q-function should be plotted, default (0,0)
CFG.trajstyle = '-';    % the line style for trajectory plot
CFG.trajcolor = 'k';    % the color for trajectory plot
CFG.showreturn = 1;     % whether to show the return near the trajectory
% save configuration
CFG.plottarget = 'screen';     % 'latex', 'beamer', 'screen', ''
% Save implemented for: policy
CFG.savedir = 'D:\Work\tex\papers\alamas07\img\';
                        % path for saving figure
CFG.savefig = 'nav';    % filename for saving figure

% process config
if ischar(cfg), cfg = str2cfg(cfg, fieldnames(CFG)); end;
cfg = checkparams(cfg, CFG);
cfg.grayscale = grayscalefromconfig(cfg);
% ensure compatibility with datafile field
if ~isempty(cfg.datafile), cfg.datasource = cfg.datafile; end;
% whether the damping map should be plotted as support
cfg.dampingsupport = (cfg.trajectory && ~cfg.addtraj) || cfg.policy || cfg.schematic;
% cfg           % feedback on config

% needed vars
vars = {'model', 'X', 'U', 'DIMS', 'XMFS', 'theta'};

% load model, grids, etc. from the data source
switch cfg.datasource,
    case 'caller',              % load needed vars from caller space
        for i=1:length(vars),
            cv.(vars{i}) = evalin('caller', vars{i});
        end;
        structtovars(cv);
    case 'problem',             % use problem fun to init (limited) vars
        model = nav_problem('model');
        grids = nav_problem('fuzzy');
        X = grids.xgrids;
        U = grids.ugrids;
    otherwise                   % load from file
        load(cfg.datasource, vars{:});
end;

% Compute the damping on a fine grid -- needed for all the options
xp = -model.phys.maxx(1):.1:model.phys.maxx(1);
yp = -model.phys.maxx(2):.1:model.phys.maxx(2);
B = zeros(length(xp), length(yp));
for i = 1:length(xp),
    for j = 1:length(yp),
        B(i, j) = feval(model.phys.damping.fun, model.phys.damping, [xp(i); yp(j)]);
    end;
end;


% grayscale styles
gs.schemec = 'k';
gs.innerc = .25 * [1 1 1];
gs.centerc = 0 * [1 1 1];
gs.cm = gray(96);
% reverse, and only use from white to a dark gray, not to black
gs.cm = gs.cm(end:-1:end-64, :);
% color styles
cs.schemec = 'k';
cs.innerc = 'b';
cs.centerc = 'r';
cs.cm = jet(64);
% set style
if cfg.grayscale, sty = gs; 
else sty = cs; end;

% readable labels
commonprop = {'Interpreter','LaTeX','FontSize',13};
rl.x = {'$c_x$',commonprop{:}}; rl.y = {'$c_y$',commonprop{:}}; 
rl.ux = {'$u_x$',commonprop{:}}; rl.uy = {'$u_y$',commonprop{:}}; 
rl.b = {'$b(c_y,c_y)$',commonprop{:}}; rl.q = {'Q*(X,Y)'};
% PSFRAG labels (when plotting for latex)
psl.x = {'CX'}; psl.y = {'CY'}; psl.b = {'DAMP'}; psl.q = {'QSTAR'};
psl.ux = {'UX'}; psl.uy = {'UY'};
% set labels
switch cfg.plottarget,
    case 'latex',   
        labels = psl;
    case {'beamer', 'screen', ''},  
        labels = rl; 
end;

% fig size -- only takes effect for 2D figures 
switch cfg.plottarget
    case 'latex',   
        cfg.figsize = [400 400];
    case 'beamer',  
        cfg.figsize = [400 400];
    otherwise       % default
        cfg.figsize = [];
end; 

% Process plot paths
figh = [];

% ------ Surface plot
if cfg.dampingsurface || cfg.both,
    figh(end+1) = figure; colormap(sty.cm);
    meshc(xp, yp, B'); hold on;
    xlabel(labels.x{:}); ylabel(labels.y{:}); 
    zlabel(labels.b{:});
    if cfg.centers,
        [xx,yy] = ndgrid(X{1}, X{2});
        Bc = zeros(length(X{1}), length(X{2}));
        for i1 = 1:length(X{1}),
            for i2 = 1:length(X{2}),
                Bc(i1, i2) = feval(model.phys.damping.fun, model.phys.damping, [X{1}(i1); X{2}(i2)]);
            end;
        end;
        if cfg.centers,
            surface(X{1}, X{2}, Bc', 'EdgeColor', 'none', 'FaceColor', 'none', ...
                'Marker' ,'o', 'MarkerFaceColor', sty.centerc, 'MarkerSize', cfg.markersize); 
        end;
    end;
end;

% ------ Damping map
if cfg.dampingmap || cfg.both || cfg.dampingsupport,
    figh(end+1) = figure; 
    if ~isempty(cfg.figsize),
        set(gcf, 'Position', [0 0 cfg.figsize]);
        movegui(gcf, 'center');
    end;
    if cfg.dampingsupport,
        % damping map is support: always grayscale, otherwise foreground unreadable
        colormap(gs.cm);
    else
        % damping map is colored as selected 
        colormap(sty.cm);
    end;
    if std(B(:)),
        policy = pcolor(xp, yp, B'); hold on;
        set(policy, 'LineStyle', 'none');
    else % plot in white
        policy = pcolor(xp, yp, B'); hold on;
        set(policy, 'LineStyle', 'none', 'FaceColor', 'w');
        hold on;
    end;
    xlabel(labels.x{:}); ylabel(labels.y{:}); 
    % put a title unless colormap is just background for other plot
    if cfg.dampingmap || cfg.both, title(labels.b{:}); end;
    if cfg.centers && ~cfg.schematic,
        [xx,yy] = ndgrid(X{1}, X{2});
        plot(xx(:), yy(:), 'LineStyle', 'none', 'Color', sty.centerc, ...
            'Marker', 'o', 'MarkerSize', cfg.markersize, 'MarkerFaceColor', sty.centerc); 
    end;
    axis equal image
    set(gcf, 'Units', 'normalized');
end;

% ------- Schematic
if cfg.schematic,
    % Displacement for nice placing of texts
    XDISP = .05; YDISP = .3; FLEN = 1;
    
    % goal region
    tolx = model.goal.zeroband(1);
    toly = model.goal.zeroband(2);
    fill([tolx tolx -tolx -tolx], [-toly toly toly -toly], sty.innerc);
    text(tolx+XDISP, -toly-YDISP, 'Goal region', 'Color', sty.innerc);
    
    % point mass and its 
    w = model.phys.maxx(1); h = model.phys.maxx(2);
    cx = w * .4;
    cy = h * .6;
    % point coords
    plot([cx cx], [0 cy], sty.schemec, 'LineStyle', '--');
    plot([0 cx], [cy cy], sty.schemec, 'LineStyle', '--');
    text(cx/2, cy+YDISP, labels.x{:});
    text(cx+XDISP, cy/2, labels.y{:});
    text(cx+XDISP, cy - 2*YDISP, 'Point mass', 'Color', sty.centerc);
    
    % forces
    [fx fy] = axescoord2figurecoord([cx cx+FLEN], [cy cy]);
    annotation('arrow', fx, fy, 'Color', sty.schemec);
    [fx fy] = axescoord2figurecoord([cx cx], [cy cy+FLEN]);
    annotation('arrow', fx, fy, 'Color', sty.schemec);
    text(cx + FLEN, cy, labels.ux{:});
    text(cx, cy + FLEN, labels.uy{:});
    
    % point
    plot(cx, cy, 'Color', sty.centerc, 'Marker', 'o', 'MarkerSize', cfg.markersize + 2, 'MarkerFaceColor', sty.centerc); 

    % 0-axes
    plot([-w w], [0 0], sty.schemec, 'LineStyle', ':');
    plot([0 0], [-h h], sty.schemec, 'LineStyle', ':');

end;

% ------ Trajectory
if cfg.trajectory,          % extradata holds the policy
    if cfg.addtraj,         % select existing figure
        figh(end+1) = figure(cfg.addtraj); hold on;
    end;
    plot(cfg.hist.x(1, :), cfg.hist.x(2, :), 'Color', cfg.trajcolor, 'LineStyle', cfg.trajstyle, ...
        'LineWidth', 1, 'Marker', 'o', 'MarkerSize', cfg.markersize, 'MarkerFaceColor', cfg.trajcolor);
    if cfg.showreturn,      % put the return label somewhere near the start of the trajectory
        % compute an average position of the first part of the trajectory
        p = mean(cfg.hist.x(1:2, 1:10), 2);
        % move the text away a little bit, remaining within bounds
        lx = model.phys.maxx(1)/8;
        if p(1)+5*lx < model.phys.maxx(1), p(1) = p(1)+lx;
        else p(1) = p(1)-4*lx;
        end;
        ly = model.phys.maxx(2)/16; % height of text smaller than width
        if p(2)+2*ly < model.phys.maxx(2), p(2) = p(2)+ly;
        else p(2) = p(2)-ly;
        end;
        text(p(1), p(2), ['R=' num2str(fix(cfg.hist.R(1)))], 'Color', cfg.trajcolor);
    end;
      
end;

speedslice = cfg.speedslice;
% ------ Fuzzy policy
if cfg.policy,
    
    [Qstar ui] = max(theta, [], 2); clear Qstar;
    ui = lin2ndi(ui, DIMS.dimu);
    % compute optimal policy
    hstar = zeros(DIMS.N, DIMS.q);
    for q = 1:DIMS.q,
        hstar(:, q) = U{q}(ui(:, q));
    end;
    
    % plot intermediate values on a uniform grid    
    x = X{1}(1):cfg.posstep:X{1}(end);
    y = X{2}(1):cfg.posstep:X{2}(end);
    [xx, yy] = ndgrid(x, y);
    plot(xx(:), yy(:), 'LineStyle', 'none', 'Color', sty.innerc, ...
            'Marker', 'o', 'MarkerSize', cfg.markersize-1, 'MarkerFaceColor', sty.innerc);
    for i1 = 1:length(x),
        for i2 = 1:length(y),
            if any(x(i1) == X{1}) && any(y(i2) == X{2}), continue; end;
            mu = mdegs(x(i1), XMFS{1});
            mu = mu(:) * mdegs(y(i2), XMFS{2})';
            for p = 3:DIMS.p, mu = mu(:) * mdegs(speedslice(p-2), XMFS{p})'; end; % zero speed
            phi = find(mu); mu = mu(phi);
            % compute optimal action
            u = (mu' * hstar(phi, :))';
            p0 = [x(i1)  y(i2)];
            pend = p0 + u' * cfg.arrowscale;
%             [fx fy] = axescoord2figurecoord([p0(1) pend(1)], [p0(2) pend(2)]);
%             annotation('arrow', fx, fy, 'Color', sty.innerc, 'HeadStyle', 'plain');
            line([p0(1) pend(1)], [p0(2) pend(2)], 'Color', sty.innerc);
        end;
    end;

    
    % plot the optimal actions in the position grid points
    % just pick up the indicated speed slice
    % plot again the grid points, some will have been overwritten by the grid above
    [xx,yy] = ndgrid(X{1}, X{2});
    plot(xx(:), yy(:), 'LineStyle', 'none', 'Color', sty.centerc, ...
        'Marker', 'o', 'MarkerSize', cfg.markersize, 'MarkerFaceColor', sty.centerc); 
    sliceix = [find(X{3} == speedslice(1)) find(X{4} == speedslice(2))];
    for i1 = 1:DIMS.dimx(1),
        for i2 = 1:DIMS.dimx(2),
            i = ndi2lin([i1 i2 sliceix], DIMS.dimx);
            p0 = [X{1}(i1)  X{2}(i2)];
            pend = p0 + hstar(i, :) * cfg.arrowscale;
            line([p0(1) pend(1)], [p0(2) pend(2)], 'Color', sty.centerc, 'LineWidth', 2);
        end;
    end;

    % title
    switch cfg.plottarget,
        case {'latex', 'beamer'},
        case {'screen', ''},  
            title(['$h^*(C_X, C_Y)$ for $[\dot{C}_X, \dot{C}_Y] = [' ...
                num2str(speedslice(1)) ',' num2str(speedslice(2)) ']$'], 'Interpreter', 'LaTeX');
    end;

end;

% ------ Fuzzy value function
if cfg.Q,
    figh(end+1) = figure; clf;
    
    % plot in red points the optimal values in the position grid points
    % just pick up the indicated speed slice
    sliceix = [find(X{3} == speedslice(1)) find(X{4} == speedslice(2))];
    theta = reshape(max(theta, [], 2), DIMS.dimx);
    % retrieve speed slice
    theta = squeeze(theta(:, :, sliceix(1), sliceix(2)));
    mesh(X{1}, X{2}, theta');
    surface(X{1}, X{2}, theta', 'EdgeColor', 'none', 'FaceColor', 'none', ...
        'Marker' ,'o', 'MarkerFaceColor', 'r', 'MarkerSize', cfg.markersize);
    xlabel(labels.x{:}); ylabel(labels.y{:}); zlabel(labels.q{:});
    
    % title
    switch cfg.plottarget,
        case {'latex', 'beamer'},
        case {'screen', ''},  
            title(['$Q^*(C_X, C_Y)$ for $[\dot{C}_X, \dot{C}_Y] = [' ...
                num2str(speedslice(1)) ',' num2str(speedslice(2)) ']$'], 'Interpreter', 'LaTeX');
    end;
end;

% save last figure if indicated
saveplot(figh(end), [cfg.savedir cfg.savefig], cfg.plottarget);


% END nav_plot RETURNING array of figure handles =================================================


% Old code:

% Plotting intermediate Q-values on a fine grid -- unnecessary since they vary linearly
%     x = X{1}(1):cfg.posstep:X{1}(end);
%     y = X{2}(1):cfg.posstep:X{2}(end);
%     Qstar = zeros(length(x), length(y));
%     for i1 = 1:length(x),
%         for i2 = 1:length(y),
%             mu = mdegs(x(i1), XMFS{1});
%             mu = mu(:) * mdegs(y(i2), XMFS{2})';
%             % the rest are given by the speed slice
%             for p = 3:DIMS.p, mu = mu(:) * mdegs(speedslice(p-2), XMFS{p})'; end;
%             phi = find(mu); 
%             % compute optimal value
%             Qstar(i1, i2) = max(mu(phi)' * theta(phi, :));
%         end;
%     end;
%     mesh(x, y, Qstar'); hold on;