Skip to content

Commit 653a44f

Browse files
authored
Merge pull request #90 from OSU-LRAM/Ross
Ross
2 parents 8d64c1f + be131a6 commit 653a44f

32 files changed

+2204
-101
lines changed

ProgramFiles/Geometry/NLinkChain/N_link_chain.m

Lines changed: 38 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
function [h, J, J_full,frame_zero,J_zero] = N_link_chain(geometry,jointangles)
1+
function [h, J, J_full,frame_zero,J_zero] = N_link_chain(geometry,shapeparams)
22
% Build a backbone for a chain of links, specified as a vector of link
33
% lengths and the joint angles between them.
44
%
@@ -24,6 +24,13 @@
2424
% sysf_ file. Argument should be the name of
2525
% a system in the current UserFiles folder
2626
%
27+
% modes (optional): Option to map input "jointangles" across
28+
% multiple links in a chain (which can have more joints than the
29+
% provided number of "jointangles". Should be a matrix in which
30+
% each column is the set of joint angles on the full chain associated
31+
% with a unit value of the corresponding "jointangle" input.
32+
%
33+
%
2734
% length (optional): Total length of the chain. If specified, the elements of
2835
% will be scaled such that their sum is equal to L. If this field
2936
% is not provided or is entered as an empty matrix, then the links
@@ -83,10 +90,22 @@
8390
baseframe = geometry.baseframe;
8491
end
8592

86-
% Force linklength and jointangle vectors to be columns, and normalize link
93+
% If no modes are specified, use an identity mapping for the modes
94+
if ~isfield(geometry,'modes') || isempty(geometry.modes)
95+
modes = eye(numel(shapeparams));
96+
else
97+
modes = geometry.modes;
98+
end
99+
100+
% Force linklength and shapeparam vectors to be columns, and normalize link
87101
% lengths for a total length of 1.
88102
linklengths = geometry.linklengths(:)/sum(geometry.linklengths)*L;
89-
jointangles = jointangles(:);
103+
shapeparams = shapeparams(:);
104+
105+
106+
% Expand jointangles from specified shape variables to actual joint angles
107+
% by multiplying in the modal function
108+
jointangles = modes*shapeparams;
90109

91110
%%%%%%%%%%%%
92111

@@ -283,6 +302,9 @@
283302

284303
end
285304

305+
% Convert joint-angle Jacobian into shape-mode coordinates
306+
J_temp{idx} = J_temp{idx} * modes;
307+
286308
end
287309

288310

@@ -310,6 +332,8 @@
310332
joints_v,...
311333
jointangles,...
312334
linklengths,...
335+
shapeparams,...
336+
modes,...
313337
J_temp,...
314338
baseframe);
315339

@@ -318,6 +342,17 @@
318342
% Jacobian so that they are refefenced off of the new base frame
319343
[h_m,J,J_full] = N_link_conversion(chain_m,J_temp,frame_zero,J_zero);
320344

345+
346+
347+
348+
% %%%%%%%
349+
% % Multiply the Jacobians by the modal matrices to produce Jacobians that
350+
% % act from the modal coefficients rather than the full joint space
351+
% J = cellfun(@(j) j*modes,J,'UniformOutput',false);
352+
% full_mode_conversion = [eye(size(J{1},1)), zeros(size(J{1},1),size(modes,2));
353+
% zeros(size(modes,1),size(J{1},1)),modes];
354+
% J_full = cellfun(@(j) j*full_mode_conversion,J_full,'UniformOutput',false);
355+
321356
% For output, convert h into row form. Save this into a structure, with
322357
% link lengths included
323358
h.pos = mat_to_vec_SE2(h_m);

ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m

Lines changed: 13 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,8 @@
66
joints_v,...
77
jointangles,...
88
linklengths,...
9+
shapeparams,...
10+
modes,...
911
J_temp,...
1012
baseframe)
1113
%%%%%%%
@@ -126,7 +128,10 @@
126128
halfrotation = Adjinv(joints_v(joint_zero,:)/2);
127129
J_zero = halfrotation * halfstep * J_temp{joint_zero};
128130

129-
J_zero(:,joint_zero) = [0; 0; .5]; % Account for centered frame being half-sensitive to middle joint
131+
% Account for centered frame being half-sensitive to middle joint
132+
J_zero = J_zero + .5*[zeros(2,size(modes,2));modes(joint_zero,:)];
133+
134+
%J_zero(:,joint_zero) = [0; 0; .5];
130135

131136

132137
end
@@ -316,9 +321,9 @@
316321
% of this transform
317322

318323
frame_mp = zeros(3,1); % x y theta values of transformation
319-
J_mp = zeros(3,numel(jointangles)); % x y theta rows and joint angle columns of Jacobian
324+
J_mp = zeros(3,numel(shapeparams)); % x y theta rows and joint angle columns of Jacobian
320325

321-
jointangles_cell = num2cell(jointangles); % put joint angles into a cell array
326+
shapeparams_cell = num2cell(shapeparams); % put joint angles into a cell array
322327

323328
% Iterate over x y theta components
324329
for idx = 1:numel(frame_mp)
@@ -328,7 +333,7 @@
328333
% transformation to m-p coordinates
329334
frame_mp(idx) = interpn(s.grid.eval{:},... % system evaluation grid
330335
s.B_optimized.eval.Beta{idx},... % Components of the transfomation to m-p coordinates
331-
jointangles_cell{:}); % Current shape
336+
shapeparams_cell{:}); % Current shape
332337

333338
% Scale the x and y components of the frame
334339
% location
@@ -337,15 +342,15 @@
337342
end
338343

339344
% Iterate over shape components for Jacobian
340-
for idx2 = 1:numel(jointangles)
345+
for idx2 = 1:numel(shapeparams)
341346

342347

343348
% Interpolate the shape angles into the grid
344349
% and extract the corresponding component of
345350
% the transformation to m-p coordinates
346351
J_mp(idx,idx2) = interpn(s.grid.eval{:},... % system evaluation grid
347352
s.B_optimized.eval.gradBeta{idx,idx2},... % Components of the transfomation to m-p coordinates
348-
jointangles_cell{:}); % Current shape
353+
shapeparams_cell{:}); % Current shape
349354

350355
% Scale the x and y components of the frame
351356
% Jacobian
@@ -391,6 +396,8 @@
391396
joints_v,...
392397
jointangles,...
393398
linklengths,...
399+
shapeparams,...
400+
modes,...
394401
J_temp,...
395402
baseframe_original);
396403

Lines changed: 142 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,142 @@
1+
function [gradE, E, resid] = fasthelmholtz(grid,V)
2+
3+
% Recreate the original spacing vectors defining the grid
4+
gridSpacing = extractGridSpacing(grid);
5+
6+
% % Convert from ndgrid to meshgrid
7+
% dimcount = numel(grid); % Count how many dimensions field is defined over
8+
% dimlist = [2 1 3:dimcount]; % Permutation mapping from ndgrid to meshgrid
9+
% grid = permute(grid,dimlist); % Perform permutation on grid component ordering
10+
% V = permute(V,dimlist); % Perform permutation on vector component ordering
11+
% gridSpacing = permute(gridSpacing,dimlist); % Perform permutation on grid spacing component ordering
12+
% %grid = cellfun(@(g) permute(g,dimlist),grid,'UniformOutput',false); % Perform permutation on grid value ordering
13+
% %V = cellfun(@(g) permute(g,dimlist),V,'UniformOutput',false); % Perform permutation on vector value ordering
14+
15+
16+
grid = {grid{2}; grid{1}; grid{3:end}};
17+
V = {V{2}; V{1}; V{3:end}};
18+
19+
20+
%%%%
21+
% Take the gradient of each vector component
22+
23+
% Make a cell matrix the same size as the vector cell matrix
24+
gradV = cell(size(V));
25+
26+
% Loop over each element of this cell (one cell per component of the
27+
% vector)
28+
for idx = 1:numel(grid)
29+
30+
% Each vector component has a derivative along each vector component
31+
gradV{idx} = cell(size(grid));
32+
33+
% Calculate the gradient
34+
[gradV{idx}{:}] = gradient(V{idx},gridSpacing{:});
35+
end
36+
37+
38+
%%%%%
39+
% Convert the gradients into gradients calculated at each location
40+
41+
% One matrix at each grid point, with as many rows/columns as there are
42+
% dimensions
43+
gradV_local = repmat({zeros(numel(grid),numel(grid))},size(grid{1}));
44+
45+
for idx1 = 1:numel(grid) %Loop over all vector components
46+
for idx2 = 1:numel(grid) %Loop over all derivative directions
47+
for idx3 = 1:numel(grid{1}) %Loop over all grid points
48+
49+
gradV_local{idx3}(idx1,idx2) = gradV{idx1}{idx2}(idx3);
50+
51+
end
52+
end
53+
end
54+
55+
56+
%%%%%%%%%%
57+
% Remove the skew-symmetry in the gradient, remainder is derivative of
58+
% conservative function
59+
60+
gradV_conservative = gradV;
61+
62+
for idx1 = 1:numel(grid) %Loop over all vector components
63+
for idx2 = 1:numel(grid) %Loop over all derivative directions
64+
for idx3 = 1:numel(grid{1}) %Loop over all grid points
65+
66+
gradV_conservative{idx1}{idx2}(idx3) = ...
67+
(gradV{idx1}{idx2}(idx3) + ...
68+
gradV{idx2}{idx1}(idx3))/2;
69+
70+
end
71+
end
72+
end
73+
74+
75+
%%%%%%%
76+
% Integrate conservative component
77+
V_conservative = V;
78+
for idx = 1:numel(grid) % Loop over all vector components
79+
V_conservative{idx} = intgrad2(gradV_conservative{idx}{:},gridSpacing{:});
80+
end
81+
82+
%%%%%%%
83+
% Subtract conservative component from original vector field to get the
84+
% residual non-conservative component
85+
V_nonconservative = V;
86+
for idx = 1:numel(grid) % Loop over all vector components
87+
V_nonconservative{idx} = V{idx} - V_conservative{idx};
88+
end
89+
90+
%%%%%%%
91+
% Calculate the mean of the nonconservative field, so that we can extract
92+
% this harmonic component
93+
% residual non-conservative component
94+
V_nonconservative_mean = V;
95+
for idx = 1:numel(grid) % Loop over all vector components
96+
V_nonconservative_mean{idx} = mean(V_nonconservative{idx}(:));
97+
end
98+
99+
%%%%%%%%
100+
% Add the mean of the non-conservative field back into the conservative
101+
% field
102+
V_conservative_fixed = V;
103+
V_nonconservative_fixed = V;
104+
for idx = 1:numel(grid) % Loop over all vector components
105+
V_conservative_fixed{idx} = V_conservative{idx}+V_nonconservative_mean{idx};
106+
V_nonconservative_fixed{idx} = V_nonconservative{idx}-V_nonconservative_mean{idx};
107+
end
108+
109+
110+
%
111+
% %%%%
112+
% % Split out the conservative component of the vector gradient
113+
%
114+
% % Predefine a cell array to hold conservative components
115+
% gradV_local_conservative = gradV_local;
116+
%
117+
% for idx = 1:numel(gradV_local) % Loop over all grid points
118+
%
119+
% % Conservative component is average of gradient and its transpose
120+
% gradV_local_conservative{idx} = (gradV_local{idx} + transpose(gradV_local{idx}))/2;
121+
%
122+
% end
123+
124+
125+
%%%%%
126+
% Make the
127+
128+
129+
% % Convert from meshgrid to ndgrid
130+
V_conservative_fixed = {V_conservative_fixed{2}; V_conservative_fixed{1}; V_conservative_fixed{3:end}};
131+
% V_conservative_fixed = cellfun(@(g) permute(g,dimlist),V_conservative_fixed,'UniformOutput',false); % Perform permutation on vector value ordering
132+
133+
V_nonconservative_fixed = {V_nonconservative_fixed{2}; V_nonconservative_fixed{1}; V_nonconservative_fixed{3:end}};
134+
% V_nonconservative_fixed = cellfun(@(g) permute(g,dimlist),V_nonconservative_fixed,'UniformOutput',false); % Perform permutation on vector value ordering
135+
136+
E = [];
137+
138+
gradE = V_conservative_fixed;
139+
resid = V_nonconservative_fixed;
140+
141+
142+
end

0 commit comments

Comments
 (0)