restructure plotting with animated line

animatedline.m

@@ -0,0 +1,141 @@
+## Copyright (C) 2019 Antonius R. Burgers
+##
+## This file is part of Octave.
+##
+## Octave is free software: you can redistribute it and/or modify it
+## under the terms of the GNU General Public License as published by
+## the Free Software Foundation, either version 3 of the License, or
+## (at your option) any later version.
+##
+## Octave is distributed in the hope that it will be useful, but
+## WITHOUT ANY WARRANTY; without even the implied warranty of
+## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+## GNU General Public License for more details.
+##
+## You should have received a copy of the GNU General Public License
+## along with Octave; see the file COPYING.  If not, see
+## <https://www.gnu.org/licenses/>.
+
+classdef animatedline < handle
+  properties (Private)
+    h
+  end
+  properties
+    MaximumNumPoints = 0;
+  end
+
+  methods (Static)
+    function [name, deflt] = lineprops
+      tmp = { ...
+        'Color', 'black', ...
+        'DisplayName', 'none', ...
+        'Marker', 'none', ...
+        'MarkerSize', 7, ...
+        'MarkerFaceColor', 'black', ...
+        'LineWidth', 1};
+      tmp = reshape(tmp, 2, []);
+      name  = tmp(1, :);
+      deflt = tmp(2, :);
+    end
+  end
+
+  methods (Private)
+    function truncatepoints(obj)
+      [x, y, z] = obj.getpoints;
+      if obj.MaximumNumPoints > 0 && numel(x) > obj.MaximumNumPoints
+        is = numel(x) - obj.MaximumNumPoints + 1;
+        set(obj.h, 'XData', x(is:end));
+        set(obj.h, 'YData', y(is:end));
+        if ~isempty(z)
+          set(obj.h, 'ZData', z(is:end));
+        end
+      end
+    end
+
+    function obj = subsasgn(obj, s, val)
+      if strcmp(s.type, '.') && any(strcmpi(s.subs, obj.lineprops))
+        set(obj.h, s.subs, val);
+      else
+        obj = builtin('subsasgn', obj, s, val);
+      end
+    end
+
+  end
+
+  methods
+    function obj = animatedline(varargin)
+      [lineprop_names, lineprop_dflts] = obj.lineprops;
+
+      p = inputParser;
+      p.CaseSensitive = false;
+      for i = 1 : numel(lineprop_names)
+        p.addParameter(lineprop_names{i}, lineprop_dflts{i});
+      end
+      p.addParameter('MaximumNumPoints', 0);
+
+      have_y = 0;
+      have_z = 0;
+      is = 1;
+      if nargin > 0 && isnumeric(varargin{1})
+        x = varargin{1};
+        is = 2;
+        if nargin > 1 && isnumeric(varargin{2})
+          y = varargin{2};
+          is = 3;
+          have_y = 1;
+          if nargin > 2 && isnumeric(varargin{3})
+            z = varargin{3};
+            is = 4;
+            have_z = 1;
+          end
+        end
+      end
+      p.parse(varargin{is:end});
+      % hold on required, because animatedline used
+      % the current axis, and you want to preserve
+      % e.g. xlim, ylim settings for those axes
+      hold('on');
+      if have_z
+        obj.h = plot3(gca, x, y, z);
+        obj.truncatepoints;
+      elseif have_y
+        obj.h = plot(gca, x, y);
+        obj.truncatepoints;
+      else
+        obj.h = plot(gca, 0, 0);
+        obj.clearpoints;
+      end
+      hold('off');
+      for f = reshape(fieldnames(p.Results), 1, [])
+        if any(strcmp(f{1}, lineprop_names))
+          set(obj.h, f{1}, p.Results.(f{1}));
+        else
+          % for MaximumNumPoints
+          obj.(f{1}) = p.Results.(f{1});
+        end
+      end
+    end
+
+    function [x, y, z] = getpoints(obj)
+      x = get(obj.h, 'XData');
+      y = get(obj.h, 'YData');
+      z = get(obj.h, 'ZData');
+    end
+
+    function clearpoints(obj)
+      set(obj.h, 'XData', []);
+      set(obj.h, 'YData', []);
+      set(obj.h, 'ZData', []);
+    end
+
+    function addpoints(obj, x, y, z)
+      [xo, yo, zo] = obj.getpoints;
+      if exist('z', 'var')
+        set(obj.h, 'ZData', [zo, z(:)']);
+      end
+      set(obj.h, 'XData', [xo, x(:)']);
+      set(obj.h, 'YData', [yo, y(:)']);
+      obj.truncatepoints;
+    end
+  end
+end

mppi.m

@@ -21,14 +21,13 @@
 
   % state history
   state_dim = size(init_state, 1);
-  x_hist = zeros(state_dim, 1);
   x_hist = init_state;
   xo = init_state;
 
   % control history
   control_dim = size(init_ctrl_seq, 1);
   sample_u_hist = [];
-  du = intmax * ones(control_dim, num_timesteps);
+  du = realmax('double') * ones(control_dim, num_timesteps);
   u_hist = [];
 
   % trajectory cost history
@@ -37,7 +36,6 @@
   % sample state history
   sample_init_state = func_state_transform(init_state);
   sample_state_dim = size(sample_init_state,1);
-  sample_x_hist = zeros(sample_state_dim, 1);
   sample_x_hist = sample_init_state;
   sample_xo = sample_init_state;
 
@@ -52,18 +50,48 @@
 
   % plot trajectory in real time
   if(plot_traj)
+    state_colors = [[0 0.4470 0.7410]; [0.8500 0.3250 0.0980]; [0.4940 0.1840 0.5560]; [0.4660 0.6740 0.1880]];
+    ctrl_colors = [[0.9290 0.6940 0.1250]; [0.3010 0.7450 0.9330]; [0.6350 0.0780 0.1840]];
+    rep_traj_cost_color = 'k';
+
     state_plot = figure(1);
     title('State Value(s)')
     xlabel('Time');
     ylabel('Value');
+    for sd = 1:state_dim
+      state_animated_lines(sd).animatedline = @animatedline(...
+          'Color', state_colors(mod(sd - 1,size(state_colors,2)) + 1,:));
+          %'DisplayName', ['State ' num2str(sd)]);
+    end
+    legend
+
+    % Go ahead and plot the first state
+    figure(state_plot)
+    hold on
+    for sd = 1:state_dim
+      addpoints(state_animated_lines(sd).animatedline, time_hist(1), x_hist(sd,1));
+    end
+    legend
+    drawnow
+
     control_plot = figure(2);
     title('Control Value(s)');
     xlabel('Time');
     ylabel('Value');
+    for cd = 1:control_dim
+      control_animated_lines(cd).animatedline = @animatedline(...
+          'Color', ctrl_colors(mod(cd - 1,size(ctrl_colors,2)) + 1,:));
+          %'DisplayName', ['Control ' num2str(cd)]);
+    end
+    legend
+
     traj_cost_plot = figure(3);
     title('Trajectory Cost');
     xlabel('Time');
     ylabel('Value');
+    traj_cost_line = animatedline('Color', rep_traj_cost_color);
+        %'DisplayName', 'Trajectory Cost');
+    legend
   end
 
   total_timestep_num = 1;
@@ -170,40 +198,25 @@
 
     % Real time plotting
     if(plot_traj)
-      xlim([0,time_hist(end)])
-      state_colors = ['b', 'r', 'm', 'c'];
-      ctrl_colors = ['g', 'y', 'w'];
-      rep_traj_cost_color = 'k';
 
       figure(state_plot)
       hold on
-      for sd = (1:state_dim)
-        plot(time_hist(total_timestep_num:total_timestep_num+1), ...
-             x_hist(sd,total_timestep_num:total_timestep_num+1)', ...
-             state_colors(mod(sd - 1,size(state_colors,2)) + 1), ...
-             'DisplayName', ['State ' num2str(sd)])
+      for sd = 1:state_dim
+        addpoints(state_animated_lines(sd).animatedline, time_hist(total_timestep_num+1), x_hist(sd, total_timestep_num+1));
       end
       legend
 
-      if (total_timestep_num > 1)
-        figure(control_plot)
-        hold on
-        for cd = 1:control_dim
-          plot(time_hist(total_timestep_num-1:total_timestep_num), ...
-               u_hist(cd,total_timestep_num-1:total_timestep_num)', ...
-               ctrl_colors(mod(cd - 1,size(ctrl_colors,2)) + 1), ...
-               'DisplayName', ['Control ' num2str(cd)])
-        end
-        legend
-
-        figure(traj_cost_plot)
-        hold on
-        plot(time_hist(total_timestep_num-1:total_timestep_num), ...
-               rep_traj_cost_hist(total_timestep_num-1:total_timestep_num), ...
-               rep_traj_cost_color, 'DisplayName', 'Trajectory Cost')
-        legend
+      figure(control_plot)
+      hold on
+      for cd = 1:control_dim
+        addpoints(control_animated_lines(cd).animatedline, time_hist(total_timestep_num), u_hist(cd, total_timestep_num))'
       end
+      legend
 
+      figure(traj_cost_plot)
+      addpoints(traj_cost_line, time_hist(total_timestep_num), rep_traj_cost_hist(total_timestep_num));
+      legend
+    drawnow
     end
 
     total_timestep_num = total_timestep_num + 1;