Merge pull request #90 from OSU-LRAM/Ross

Ross

ProgramFiles/Geometry/NLinkChain/N_link_chain.m

@@ -1,4 +1,4 @@
-function [h, J, J_full,frame_zero,J_zero] = N_link_chain(geometry,jointangles)
+function [h, J, J_full,frame_zero,J_zero] = N_link_chain(geometry,shapeparams)
 % Build a backbone for a chain of links, specified as a vector of link
 % lengths and the joint angles between them.
 %
@@ -24,6 +24,13 @@
 %                                       sysf_ file. Argument should be the name of
 %                                       a system in the current UserFiles folder
 %
+%       modes (optional): Option to map input "jointangles" across
+%           multiple links in a chain (which can have more joints than the
+%           provided number of "jointangles". Should be a matrix in which
+%           each column is the set of joint angles on the full chain associated
+%           with a unit value of the corresponding "jointangle" input.
+%
+%
 %   length (optional): Total length of the chain. If specified, the elements of
 %       will be scaled such that their sum is equal to L. If this field
 %       is not provided or is entered as an empty matrix, then the links
@@ -83,10 +90,22 @@
     baseframe = geometry.baseframe;
 end
 
-% Force linklength and jointangle vectors to be columns, and normalize link
+% If no modes are specified, use an identity mapping for the modes
+if ~isfield(geometry,'modes') || isempty(geometry.modes)
+    modes = eye(numel(shapeparams));
+else
+    modes = geometry.modes;
+end
+
+% Force linklength and shapeparam vectors to be columns, and normalize link
 % lengths for a total length of 1.
 linklengths = geometry.linklengths(:)/sum(geometry.linklengths)*L;
-jointangles = jointangles(:);
+shapeparams = shapeparams(:);
+
+
+% Expand jointangles from specified shape variables to actual joint angles
+% by multiplying in the modal function
+jointangles = modes*shapeparams;
 
 %%%%%%%%%%%%
 
@@ -283,6 +302,9 @@
 
     end
 
+    % Convert joint-angle Jacobian into shape-mode coordinates
+    J_temp{idx} = J_temp{idx} * modes;
+
 end
 
 
@@ -310,6 +332,8 @@
                                                 joints_v,...
                                                 jointangles,...
                                                 linklengths,...
+                                                shapeparams,...
+                                                modes,...
                                                 J_temp,...
                                                 baseframe);        
 
@@ -318,6 +342,17 @@
 % Jacobian so that they are refefenced off of the new base frame
 [h_m,J,J_full] = N_link_conversion(chain_m,J_temp,frame_zero,J_zero); 
 
+
+
+
+% %%%%%%%
+% % Multiply the Jacobians by the modal matrices to produce Jacobians that
+% % act from the modal coefficients rather than the full joint space
+% J = cellfun(@(j) j*modes,J,'UniformOutput',false);
+% full_mode_conversion = [eye(size(J{1},1)), zeros(size(J{1},1),size(modes,2));
+%                         zeros(size(modes,1),size(J{1},1)),modes];
+% J_full = cellfun(@(j) j*full_mode_conversion,J_full,'UniformOutput',false);
+
 % For output, convert h into row form. Save this into a structure, with
 % link lengths included
 h.pos = mat_to_vec_SE2(h_m);

ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m

@@ -6,6 +6,8 @@
                                                          joints_v,...
                                                          jointangles,...
                                                          linklengths,...
+                                                         shapeparams,...
+                                                         modes,...
                                                          J_temp,...
                                                          baseframe)
 %%%%%%%
@@ -126,7 +128,10 @@
             halfrotation = Adjinv(joints_v(joint_zero,:)/2);
             J_zero = halfrotation * halfstep * J_temp{joint_zero};
 
-            J_zero(:,joint_zero) = [0; 0; .5]; % Account for centered frame being half-sensitive to middle joint
+            % Account for centered frame being half-sensitive to middle joint
+            J_zero = J_zero + .5*[zeros(2,size(modes,2));modes(joint_zero,:)];
+
+            %J_zero(:,joint_zero) = [0; 0; .5]; 
 
 
         end
@@ -316,9 +321,9 @@
                     % of this transform
 
                     frame_mp = zeros(3,1);              % x y theta values of transformation
-                    J_mp = zeros(3,numel(jointangles)); % x y theta rows and joint angle columns of Jacobian
+                    J_mp = zeros(3,numel(shapeparams)); % x y theta rows and joint angle columns of Jacobian
 
-                    jointangles_cell = num2cell(jointangles); % put joint angles into a cell array
+                    shapeparams_cell = num2cell(shapeparams); % put joint angles into a cell array
 
                     % Iterate over x y theta components
                     for idx = 1:numel(frame_mp)
@@ -328,7 +333,7 @@
                         % transformation to m-p coordinates
                         frame_mp(idx) = interpn(s.grid.eval{:},...                  % system evaluation grid
                                                 s.B_optimized.eval.Beta{idx},...    % Components of the transfomation to m-p coordinates
-                                                jointangles_cell{:});               % Current shape
+                                                shapeparams_cell{:});               % Current shape
 
                         % Scale the x and y components of the frame
                         % location 
@@ -337,15 +342,15 @@
                         end
 
                         % Iterate over shape components for Jacobian
-                        for idx2 = 1:numel(jointangles)
+                        for idx2 = 1:numel(shapeparams)
 
 
                             % Interpolate the shape angles into the grid
                             % and extract the corresponding component of
                             % the transformation to m-p coordinates
                             J_mp(idx,idx2) = interpn(s.grid.eval{:},...                  % system evaluation grid
                                                     s.B_optimized.eval.gradBeta{idx,idx2},...    % Components of the transfomation to m-p coordinates
-                                                    jointangles_cell{:});               % Current shape
+                                                    shapeparams_cell{:});               % Current shape
 
                             % Scale the x and y components of the frame
                             % Jacobian
@@ -391,6 +396,8 @@
                                                  joints_v,...
                                                  jointangles,...
                                                  linklengths,...
+                                                 shapeparams,...
+                                                 modes,...
                                                  J_temp,...
                                                  baseframe_original);
 

ProgramFiles/HodgeHelmholtz/fasthelmholtz.m

@@ -0,0 +1,142 @@
+function [gradE, E, resid] = fasthelmholtz(grid,V)
+
+% Recreate the original spacing vectors defining the grid
+gridSpacing = extractGridSpacing(grid);
+
+% % Convert from ndgrid to meshgrid
+% dimcount = numel(grid);     % Count how many dimensions field is defined over
+% dimlist = [2 1 3:dimcount]; % Permutation mapping from ndgrid to meshgrid
+% grid = permute(grid,dimlist); % Perform permutation on grid component ordering
+% V = permute(V,dimlist);       % Perform permutation on vector component ordering 
+% gridSpacing = permute(gridSpacing,dimlist); % Perform permutation on grid spacing component ordering
+% %grid = cellfun(@(g) permute(g,dimlist),grid,'UniformOutput',false); % Perform permutation on grid value ordering
+% %V = cellfun(@(g) permute(g,dimlist),V,'UniformOutput',false);       % Perform permutation on vector value ordering
+
+
+grid = {grid{2}; grid{1}; grid{3:end}};
+V = {V{2}; V{1}; V{3:end}};
+
+
+%%%%
+% Take the gradient of each vector component
+
+% Make a cell matrix the same size as the vector cell matrix
+gradV = cell(size(V));
+
+% Loop over each element of this cell (one cell per component of the
+% vector)
+for idx = 1:numel(grid)
+
+    % Each vector component has a derivative along each vector component
+    gradV{idx} = cell(size(grid));
+
+    % Calculate the gradient
+    [gradV{idx}{:}] = gradient(V{idx},gridSpacing{:});
+end
+
+
+%%%%%
+% Convert the gradients into gradients calculated at each location
+
+% One matrix at each grid point, with as many rows/columns as there are
+% dimensions
+gradV_local = repmat({zeros(numel(grid),numel(grid))},size(grid{1}));
+
+for idx1 = 1:numel(grid)              %Loop over all vector components
+    for idx2 = 1:numel(grid)          %Loop over all derivative directions
+        for idx3 = 1:numel(grid{1})   %Loop over all grid points
+
+            gradV_local{idx3}(idx1,idx2) = gradV{idx1}{idx2}(idx3);
+
+        end        
+    end
+end
+
+
+%%%%%%%%%%
+% Remove the skew-symmetry in the gradient, remainder is derivative of
+% conservative function
+
+gradV_conservative = gradV;
+
+for idx1 = 1:numel(grid)              %Loop over all vector components
+    for idx2 = 1:numel(grid)          %Loop over all derivative directions
+        for idx3 = 1:numel(grid{1})   %Loop over all grid points
+
+            gradV_conservative{idx1}{idx2}(idx3) = ...
+                (gradV{idx1}{idx2}(idx3) + ...
+                    gradV{idx2}{idx1}(idx3))/2;
+
+        end
+    end
+end
+
+
+%%%%%%%
+% Integrate conservative component
+V_conservative = V;
+for idx = 1:numel(grid)   % Loop over all vector components
+    V_conservative{idx} = intgrad2(gradV_conservative{idx}{:},gridSpacing{:});
+end
+
+%%%%%%%
+% Subtract conservative component from original vector field to get the
+% residual non-conservative component
+V_nonconservative = V;
+for idx = 1:numel(grid)   % Loop over all vector components
+    V_nonconservative{idx} = V{idx} - V_conservative{idx};
+end
+
+%%%%%%%
+% Calculate the mean of the nonconservative field, so that we can extract
+% this harmonic component
+% residual non-conservative component
+V_nonconservative_mean = V;
+for idx = 1:numel(grid)   % Loop over all vector components
+    V_nonconservative_mean{idx} = mean(V_nonconservative{idx}(:));
+end
+
+%%%%%%%%
+% Add the mean of the non-conservative field back into the conservative
+% field
+V_conservative_fixed = V;
+V_nonconservative_fixed = V;
+for idx = 1:numel(grid)   % Loop over all vector components
+    V_conservative_fixed{idx} = V_conservative{idx}+V_nonconservative_mean{idx};
+    V_nonconservative_fixed{idx} = V_nonconservative{idx}-V_nonconservative_mean{idx};
+end
+
+
+% 
+% %%%%
+% % Split out the conservative component of the vector gradient
+% 
+% % Predefine a cell array to hold conservative components
+% gradV_local_conservative = gradV_local;
+% 
+% for idx = 1:numel(gradV_local)  % Loop over all grid points
+%     
+%    % Conservative component is average of gradient and its transpose
+%    gradV_local_conservative{idx} = (gradV_local{idx} + transpose(gradV_local{idx}))/2;
+%     
+% end
+
+
+%%%%%
+% Make the 
+
+
+% % Convert from meshgrid to ndgrid
+V_conservative_fixed = {V_conservative_fixed{2}; V_conservative_fixed{1}; V_conservative_fixed{3:end}};
+% V_conservative_fixed = cellfun(@(g) permute(g,dimlist),V_conservative_fixed,'UniformOutput',false);       % Perform permutation on vector value ordering
+
+V_nonconservative_fixed = {V_nonconservative_fixed{2}; V_nonconservative_fixed{1}; V_nonconservative_fixed{3:end}};
+% V_nonconservative_fixed = cellfun(@(g) permute(g,dimlist),V_nonconservative_fixed,'UniformOutput',false);       % Perform permutation on vector value ordering
+
+E = [];
+
+gradE = V_conservative_fixed;
+resid = V_nonconservative_fixed;
+
+
+end