Inertia optimizer

ProgramFiles/GaitOptimization/Tools/Granular_metric_calc2.m

@@ -0,0 +1,20 @@
+function Mp = Granular_metric_calc2(x,Mp_raw,alpha1,alpha2)
+% This function take the shape changes and compute the Metric tensor based
+% on new shape change using interpolation.
+    if numel(Mp_raw) ~= 4
+
+        if iscell(Mp_raw)
+            Mp_raw = cell2mat(Mp_raw);
+        end
+        % Rearrange the Metric Tensor
+        Mp_cell = [{Mp_raw(1:2:end,1:2:end)} {Mp_raw(1:2:end,2:2:end)};{Mp_raw(2:2:end,1:2:end)} {Mp_raw(2:2:end,2:2:end)}];
+
+    else
+        Mp_cell = Mp_raw;
+    end
+
+    Mp = cellfun(@(u) interpn(alpha1,alpha2,u,x{:}),Mp_cell, 'UniformOutput', false);
+
+    Mp = cell2mat(Mp);
+
+end

ProgramFiles/GaitOptimization/Tools/cdiff.m

@@ -0,0 +1,22 @@
+function dx = cdiff( x )
+% dx = CDIFF(x) Central differencing (2nd order) of x
+%   ==========
+%   dx = [d/dt]x(t)
+%   ==========
+%    x: row of column vectors
+%   ----------
+
+[n,m] = size(x);    % find the size we must match
+dx = zeros(n,m);  % preallocate
+
+% operating on a column of row vectors
+% dx(:,1)   = x(:,2)-x(:,1);     % first diff from 2 points
+% dx(:,m)   = x(:,m)-x(:,m-1);   % last diff from 2 points
+% dx(:,1) = nan(n,1);
+% dx(:,m) = nan(n,1);
+
+% all interior diffs from 3 points
+idx = 2:m-1;
+dx(:,idx) = 0.5 * (x(:,idx+1)-x(:,idx-1));
+
+end

ProgramFiles/GaitOptimization/Tools/celltensorconvert.m

@@ -0,0 +1,21 @@
+function B = celltensorconvert(A)
+% Takes an NxM cell array of YxZ arrays, and returns an YxZ cell array of
+% NxM arrays
+
+
+% Create a cell containing a matrix whose dimensions are taken from A
+new_cell = {zeros(size(A))};
+
+% Replicate this cell to make a cell array whose dimensions are taken from
+% the contents of (the first value of) A
+B = repmat(new_cell,size(A{1}));
+
+for i = 1:numel(A)
+
+	for j = 1:numel(B)
+
+		B{j}(i) = A{i}(j);
+
+	end
+
+end

ProgramFiles/GaitOptimization/Tools/ddiff.m

@@ -0,0 +1,10 @@
+function ddx = ddiff( x )
+% x: row of column vectors
+
+[n,m] = size(x);    % find the size we must match
+ddx = nan(n,m);  % preallocate
+
+ddx(:,2:end-1) = x(:,3:end) - 2*x(:,2:end-1) + x(:,1:end-2);
+
+end
+

ProgramFiles/GaitOptimization/Tools/evaluate_displacement_and_cost.m

@@ -0,0 +1,231 @@
+function [net_disp_orig, net_disp_opt, cost] = evaluate_displacement_and_cost(s,p,tspan,ConnectionEval,IntegrationMethod,resolution)
+% Evaluate the displacement and cost for the gait specified in the
+% structure GAIT when executed by the system described in the structure
+% S.
+%
+% S should be a sysplotter 's' structure loaded from a file
+% sysf_FILENAME_calc.mat (note the _calc suffix)
+%
+% P should be a structure with fields "phi" and "dphi", returning a
+% vector of shapes and shape velocities respectively. If it is not
+% convenient to analytically describe the shape velocity function,
+% gait.dphi should be defined as 
+%
+% p.dphi =  @(t) jacobianest(@(T) p.phi (T),t)
+%
+% as is done automatically by sysplotter, but note that this will be slower
+% than specifying a function directly
+%
+% ConnectionEval can specify whether the local connection should be generated from
+% its original function definiton, or by interpolation into the evaluated
+% matrix, but is optional. Valid options are 'functional' or
+% 'interpolated'. Defaults to "interpolated", which significantly faster
+% when calculating the local connection or metric from scratch takes
+% appreciable computational time
+%
+% IntegrationMethod can specify whether ODE45 or a fixed point
+% (euler-exponential) integration method should be employed. Defaults to
+% ODE, fixed point code is experimental.
+%
+% RESOLUTION specifies the number of points for fixed-point resolution
+% evaluation. A future option may support autoconvergence, but ODE
+% performance with interpolated evaluation appears to be fast enough that
+% refinement of fixed-point performance is on hold.
+
+
+	% if no ConnectionEval method is specified, default to interpolated
+	if ~exist('ConnectionEval','var')
+		ConnectionEval = 'interpolated';
+	end
+
+    % if no IntegrationMethod is specified, default to ODE
+	if ~exist('IntegrationMethod','var')
+		IntegrationMethod = 'ODE';
+	end
+
+    % if no resolution is specified, default to 100 (this only affects
+    % fixed_step integration)
+	if ~exist('resolution','var')
+		resolution = 100;
+	end
+
+
+
+	switch IntegrationMethod
+
+		case 'fixed_step'
+
+			[net_disp_orig, cost] = fixed_step_integrator(s,p,tspan,ConnectionEval,resolution);
+
+        case 'ODE'
+
+            % Calculate the system motion over the gait
+            sol = ode45(@(t,y) helper_function(t,y,s,p,ConnectionEval),tspan,[0 0 0 0]');
+
+            % Extract the final motion
+            disp_and_cost = deval(sol,tspan(end));
+
+            net_disp_orig = disp_and_cost(1:3);
+            cost = disp_and_cost(end);
+
+            % Convert the final motion into its representation in optimal
+            % coordinates
+            startshape = p.phi(0);
+            startshapelist = num2cell(startshape);
+            beta_theta = interpn(s.grid.eval{:},s.B_optimized.eval.Beta{3},startshapelist{:},'cubic');
+            net_disp_opt = [cos(beta_theta) sin(beta_theta) 0;...
+                -sin(beta_theta) cos(beta_theta) 0;...
+                0 0 1]*net_disp_orig;
+
+        otherwise
+			error('Unknown method for integrating motion');
+	end
+
+
+	% Convert the final motion into its representation in optimal
+	% coordinates
+	startshape = p.phi(0);
+	startshapelist = num2cell(startshape);
+	beta_theta = interpn(s.grid.eval{:},s.B_optimized.eval.Beta{3},startshapelist{:},'cubic');
+	net_disp_opt = [cos(beta_theta) sin(beta_theta) 0;...
+		-sin(beta_theta) cos(beta_theta) 0;...
+		0 0 1]*net_disp_orig;
+
+
+end
+
+% Evaluate the body velocity and cost velocity (according to system metric)
+% at a given time
+function [xi, dcost] = get_velocities(t,s,gait,ConnectionEval)
+
+	% Get the shape and shape derivative at the current time
+	shape = gait.phi(t);
+	shapelist = num2cell(shape);
+	dshape = gait.dphi(t);
+
+
+	% Get the local connection and metric at the current time, in the new coordinates	
+	switch ConnectionEval
+		case 'functional'
+
+			A = s.A_num(shapelist{:})./s.A_den(shapelist{:});
+
+			M = s.metric(shapelist{:});
+
+		case 'interpolated'
+
+			A = -cellfun(@(C) interpn(s.grid.eval{:},C,shapelist{:}),s.vecfield.eval.content.Avec);
+
+			M =  cellfun(@(C) interpn(s.grid.eval{:},C,shapelist{:}),s.metricfield.eval.content.metric);
+
+		otherwise
+			error('Unknown method for evaluating local connection');
+	end
+
+	% Get the body velocity at the current time
+	xi = - A * dshape(:);
+
+	% get the cost velocity
+	dcost = sqrt(dshape(:)' * M * dshape(:));
+
+end
+
+
+% Function to integrate up system velocities using a fixed-step method
+function [net_disp_orig, cost] = fixed_step_integrator(s,gait,tspan,ConnectionEval,resolution)
+
+	% Duplicate 'resolution' to 'res' if it is a number, or place res at a
+	% starting resolution if an automatic convergence method is selected
+	% (automatic convergence not yet enabled)
+	default_res = 100;
+	if isnumeric(resolution)
+		res = resolution;
+	elseif isstr(resolution) && strcmp(resolution,'autoconverge')
+		res = default_res;
+	else
+		error('Unexpected value for resolution');
+	end
+
+	% Generate the fixed points from the time span and resolution
+	tpoints = linspace(tspan(1),tspan(2),res);
+	tsteps = gradient(tpoints);
+
+	% Evaluate the velocity function at each time
+	[xi, dcost] = arrayfun(@(t) get_velocities(t,s,gait,ConnectionEval),tpoints,'UniformOutput',false);
+
+
+	%%%%%%%
+	% Integrate cost and displacement into final values
+
+	%%
+	% Exponential integration for body velocity
+
+	% Exponentiate each velocity over the corresponding time step
+	expXi = cellfun(@(xi,timestep) se2exp(xi*timestep),xi,num2cell(tsteps),'UniformOutput',false);
+
+	% Start off with zero position and displacement
+	net_disp_matrix = eye(size(expXi{1}));
+
+	% Loop over all the time steps from 1 to n-1, multiplying the
+	% transformation into the current displacement
+	for i = 1:(length(expXi)-1)
+
+		net_disp_matrix = net_disp_matrix * expXi{i};
+
+	end
+
+	% De-matrixafy the result
+	g_theta = atan2(net_disp_matrix(2,1),net_disp_matrix(1,1));
+	g_xy = net_disp_matrix(1:2,3);
+
+	net_disp_orig = [g_xy;g_theta];
+
+	%%
+	% Trapezoidal integration for cost
+	dcost = [dcost{:}];
+	cost = trapz(tpoints,dcost);
+
+end
+
+
+% Function to evaluate velocity and differential cost at each time for ODE
+% solver
+function dX = helper_function(t,X,s,gait,ConnectionEval)
+
+	% X is the accrued displacement and cost
+
+	[xi, dcost] = get_velocities(t,s,gait,ConnectionEval);
+
+	% Rotate body velocity into world frame
+	theta = X(3);
+	v = [cos(theta) -sin(theta) 0; sin(theta) cos(theta) 0; 0 0 1]*xi;
+
+	% Combine the output
+	dX = [v;dcost];
+
+
+end
+
+function expXi = se2exp(xi)
+
+	% Make sure xi is a column
+	xi = xi(:);
+
+	% Special case non-rotating motion
+	if xi(3) == 0
+
+		expXi = [eye(2) xi(1:2); 0 0 1];
+
+	else
+
+		z_theta = xi(3);
+
+		z_xy = 1/z_theta * [sin(z_theta), 1-cos(z_theta); cos(z_theta)-1, sin(z_theta)] * xi(1:2);
+
+		expXi = [ [cos(z_theta), -sin(z_theta); sin(z_theta), cos(z_theta)], z_xy;
+			0 0 1];
+
+	end
+
+
+end