From 1680217a59fb4c2195e1adf45e4d040e434c72cd Mon Sep 17 00:00:00 2001 From: Capprin Bass Date: Thu, 12 Aug 2021 11:46:57 -0600 Subject: [PATCH] Added square gaits; switched back to center A_est --- ProgramFiles/Utilities/add_tlb_plot.m | 2 +- ProgramFiles/Utilities/create_band_plot.m | 23 ++++++------- ProgramFiles/Utilities/create_sweep_plot.m | 13 ++++---- .../sys_calcpath_fcns/bound_third_order.m | 16 ++++++++-- .../sys_calcpath_fcns/calc_tlb_thirdorder.m | 32 +++++++++---------- 5 files changed, 49 insertions(+), 37 deletions(-) diff --git a/ProgramFiles/Utilities/add_tlb_plot.m b/ProgramFiles/Utilities/add_tlb_plot.m index ce7cef9..0d2c6eb 100644 --- a/ProgramFiles/Utilities/add_tlb_plot.m +++ b/ProgramFiles/Utilities/add_tlb_plot.m @@ -51,7 +51,7 @@ function add_point_and_bounds(cBVI_to, cBVI_to_polypoints, shch, color, do_bound name = [name num2str(shch)]; end % TODO: remove the negative below if you have problems - plot(cBVI_to{shch}(1), -cBVI_to{shch}(2), '+',... + plot(cBVI_to{shch}(1), cBVI_to{shch}(2), '+',... 'MarkerSize', 15,... 'LineWidth', 3,... 'Color', color,... diff --git a/ProgramFiles/Utilities/create_band_plot.m b/ProgramFiles/Utilities/create_band_plot.m index 48fcda9..ae7d970 100644 --- a/ProgramFiles/Utilities/create_band_plot.m +++ b/ProgramFiles/Utilities/create_band_plot.m @@ -1,9 +1,10 @@ % plots cBVI w.r.t. amplitude, with a third-order bounding band %% data, styling (change me) -A=0.25:0.25:2; -phi=pi/4:pi/4:2*pi; +A=pi/24:pi/24:pi/6; +phi=1:4; coords = '_opt'; +is_square = true; cbvi.Color = [0 0 0]; cbvi.LineWidth = 2; @@ -29,7 +30,7 @@ cbvi_plot_data = cbvi_data(1,:); % compute third order effects -[p.to, p.to_opt] = calc_tlb_thirdorder(s,p,false); +[p.to, p.to_opt] = calc_tlb_thirdorder(s,p,is_square); to_data = cellfun(@(to_struct) norm(to_struct{1}), p.(['to' coords])); to_data = reshape(to_data, [length(phi), length(A)]); to_true_data = max(to_data, [], 1); @@ -38,11 +39,11 @@ to_plot_data = polyshape(poly_x, poly_y); % generate bound -[~, cBVI_fun, to_fun] = bound_third_order(s, [0 0], @A_est_taylor, @cBVI_est_taylor, 0.1); +[~, cBVI_fun, to_fun] = bound_third_order(s, [0 0], @A_est_center, @cBVI_est_taylor, 0.1, is_square); to_est_data = vecnorm(cell2mat(arrayfun(to_fun, 2*A, 'UniformOutput', false))); -cbvi_est_data = vecnorm(cBVI_fun(2*A)); +cbvi_est_data = vecnorm(cell2mat(arrayfun(cBVI_fun, 2*A, 'UniformOutput', false))); to_est_corr = to_est_data./cbvi_est_data.*cbvi_plot_data; -poly_y = [cbvi_plot_data + to_est_data, fliplr(cbvi_plot_data - to_est_data)]; +poly_y = [cbvi_est_data + to_est_data, fliplr(cbvi_est_data - to_est_data)]; bound_data = polyshape(poly_x, poly_y); % plotting @@ -50,17 +51,17 @@ clf; hold on; hc = plot(A, cbvi_plot_data, 'Color', cbvi.Color, 'LineWidth', cbvi.LineWidth, 'LineStyle', cbvi.LineStyle); -%he = plot(A, cbvi_est_data, 'Color', bound.FaceColor, 'LineWidth', cbvi.LineWidth, 'LineStyle', cbvi.LineStyle); +he = plot(A, cbvi_est_data, 'Color', bound.FaceColor, 'LineWidth', cbvi.LineWidth, 'LineStyle', cbvi.LineStyle); ht = plot(to_plot_data, 'FaceColor', to.FaceColor, 'LineStyle', to.LineStyle, 'FaceAlpha', to.FaceAlpha); hb = plot(bound_data, 'FaceColor', bound.FaceColor, 'LineStyle', bound.LineStyle, 'FaceAlpha', bound.FaceAlpha); set(gca, 'Children', flipud(get(gca, 'Children'))); %flip drawing order hold off; xlabel('Gait Amplitude'); ylabel('Resulting Displacement Norm'); -%legend([hc ht he hb], 'cBVI Estimate', 'Maximum Third Order Contribution', 'cBVI Taylor Series Approximation', 'Estimated Third Order Bound'); -legend([hc ht hb], 'cBVI Estimate', 'Maximum Third Order Contribution', 'Estimated Third Order Bound'); -[num, dem] = rat(A); -labels = arrayfun(@(n,d) [num2str(n) '/' num2str(d)], num, dem, 'UniformOutput', false); +legend([hc ht he hb], 'cBVI Estimate', 'Maximum Third Order Contribution', 'cBVI Taylor Series Approximation', 'Estimated Third Order Bound'); +%legend([hc ht hb], 'cBVI Estimate', 'Maximum Third Order Contribution', 'Estimated Third Order Bound'); +[num, dem] = rat(A/pi); +labels = arrayfun(@(n,d) ['\pi/' num2str(d)], num, dem, 'UniformOutput', false); xticks(A); xticklabels(labels); xlim([min(A) max(A)]); diff --git a/ProgramFiles/Utilities/create_sweep_plot.m b/ProgramFiles/Utilities/create_sweep_plot.m index f61718a..c5b65b4 100644 --- a/ProgramFiles/Utilities/create_sweep_plot.m +++ b/ProgramFiles/Utilities/create_sweep_plot.m @@ -1,5 +1,6 @@ %% styling (change me!) -is_opt = true; +is_opt = false; +is_square = true; net.LineStyle = 'none'; net.LineWidth = 1; @@ -29,7 +30,7 @@ axes = gca; % add third-order effects -add_tlb_plot(s,p,is_opt,false,true); +add_tlb_plot(s,p,is_opt,is_square,true); % store net, third order to_data = []; @@ -63,15 +64,15 @@ % add bound [len, cBVI_fun, to_fun] = bound_third_order(s, [0 0],... - @(a,b) A_est_taylor(a,b,is_opt),... + @(a,b) A_est_center(a,b,is_opt),... @(a,b) cBVI_est_taylor(a,b,is_opt),... - 0.5); + 0.5, is_square); amps = linspace(0, len, 20); bound_data = -cell2mat(arrayfun(@(a) cBVI_fun(a), amps, 'UniformOutput', false)) +... cell2mat(arrayfun(@(a) to_fun(a), amps, 'UniformOutput', false)); bound_data_neg = bound_data.*[1 -1 1]'; -line(axes, bound_data(1,:), bound_data(2,:), 'Color', [234 14 30]/255, 'LineStyle', '--'); -line(axes, bound_data_neg(1,:), bound_data_neg(2,:), 'Color', [234 14 30]/255, 'LineStyle', '--'); +%line(axes, bound_data(1,:), bound_data(2,:), 'Color', [234 14 30]/255, 'LineStyle', '--'); +%line(axes, bound_data_neg(1,:), bound_data_neg(2,:), 'Color', [234 14 30]/255, 'LineStyle', '--'); function edit_properties(child, type) fields = fieldnames(type); diff --git a/ProgramFiles/sys_calcpath_fcns/bound_third_order.m b/ProgramFiles/sys_calcpath_fcns/bound_third_order.m index c58ec5a..71a99fd 100644 --- a/ProgramFiles/sys_calcpath_fcns/bound_third_order.m +++ b/ProgramFiles/sys_calcpath_fcns/bound_third_order.m @@ -22,11 +22,20 @@ % third_order_fun: % function, returning worst-case third-order estimate with respect % to length -function [l, cBVI_fun, third_order_fun] = bound_third_order(s, shape, A_est, cBVI_est, P, phi) +function [l, cBVI_fun, third_order_fun] = bound_third_order(s, shape, A_est, cBVI_est, P, is_square, phi) % sanitize if ~exist('phi', 'var') phi = 0; end + if ~exist('is_square', 'var') + is_square = false; + end + if is_square + theta = [pi/4 3*pi/4]; + else + theta = [pi/8 5*pi/8]; + end + % anonymous helper functions R = @(theta) [cos(theta) -sin(theta); sin(theta) cos(theta)]; lb_wc = @(x,y) [abs(x(2)*y(3)) + abs(y(2)*x(3));... @@ -37,8 +46,9 @@ % construct worst-case alpha+beta l_sym = sym('l', {'real' 'positive'}); A_fun = A_est(s, shape); - a_b_wc = pi/4 * l_sym * (abs(A_fun(l_sym) * R(pi/8) * init_tan) +... - abs(A_fun(l_sym) * R(5*pi/8) * init_tan)); + factor = double(~is_square) * pi/4 + double(is_square); + a_b_wc = factor * l_sym * (abs(A_fun(l_sym) * R(theta(1)) * init_tan) +... + abs(A_fun(l_sym) * R(theta(2)) * init_tan)); % estimate integral of DA inside circle of diameter l cBVI_fun = cBVI_est(s, shape); % do worst-case lie bracket diff --git a/ProgramFiles/sys_calcpath_fcns/calc_tlb_thirdorder.m b/ProgramFiles/sys_calcpath_fcns/calc_tlb_thirdorder.m index 5e793c4..512b6df 100644 --- a/ProgramFiles/sys_calcpath_fcns/calc_tlb_thirdorder.m +++ b/ProgramFiles/sys_calcpath_fcns/calc_tlb_thirdorder.m @@ -35,25 +35,25 @@ s.vecfield.eval.content.Avec_optimized}; % evaluate LC at center (mean) of gait, in orig. and opt. coords % interp assumes two shape vars - %for shvar = 1:length(init_vel) - % for dim = 1:3 - % for i = 1:2 %orig, opt. coords - % LC_interp = interp2(s.grid.eval{2}, s.grid.eval{1},... - % A{i}{dim, shvar},... - % center(1), center(2)); - % LC_center{i}(dim, shvar) = LC_center{i}(dim, shvar) +... - % LC_interp; - % end - % end - %end + for shvar = 1:length(init_vel) + for dim = 1:3 + for i = 1:2 %orig, opt. coords + LC_interp = interp2(s.grid.eval{2}, s.grid.eval{1},... + A{i}{dim, shvar},... + center(1), center(2)); + LC_center{i}(dim, shvar) = LC_center{i}(dim, shvar) +... + LC_interp; + end + end + end % use mean of entire LC, communicating magnitude and direction % jury's out on whether using the entire LC is a good idea - for i = 1:numel(A{1}) - for c = 1:2 %orig, opt. coords - LC_center{c}(i) = mean(A{c}{i}, 'all'); - end - end + %for i = 1:numel(A{1}) + % for c = 1:2 %orig, opt. coords + % LC_center{c}(i) = mean(A{c}{i}, 'all'); + % end + %end % tools for approx. R = @(theta) [cos(theta) -sin(theta); sin(theta) cos(theta)];