From 9e3b4203da92f717a8d360ba4526c7df524a9138 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Thu, 14 Nov 2019 02:37:11 -0500 Subject: [PATCH] restructure plotting with animated line --- animatedline.m | 141 +++++++++++++++++++++++++++++++++++++++++++++++++ mppi.m | 71 +++++++++++++++---------- 2 files changed, 183 insertions(+), 29 deletions(-) create mode 100644 animatedline.m diff --git a/animatedline.m b/animatedline.m new file mode 100644 index 0000000..9d8ace1 --- /dev/null +++ b/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 +## . + +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 diff --git a/mppi.m b/mppi.m index e9d1483..6c2e246 100644 --- a/mppi.m +++ b/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;