Added square gaits; switched back to center A_est

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,...

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,29 +39,29 @@
 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
 figure(1);
 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)]);

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);

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

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)];