diff --git a/.gitignore b/.gitignore index 5fd28ef8..f05cfa27 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ + # OS generated files # ###################### .DS_Store @@ -9,7 +10,7 @@ ehthumbs.db Thumbs.db # Sysplotter data and configuration files # -###################### +########################################### sysplotter_config.mat sysplotter_data @@ -22,3 +23,6 @@ UserFiles/GenericUser/Shape_Changes/shchf_params_* UserFiles/GenericUser/Shape_Changes/opt_* UserFiles/GenericUser/Shape_Changes/shchf_opt_* +# Generated Files # +################### +exp_deriv.m \ No newline at end of file diff --git a/InertialPurgatory/Inertial_connection_discrete.m.orig b/InertialPurgatory/Inertial_connection_discrete.m.orig new file mode 100644 index 00000000..d525ec10 --- /dev/null +++ b/InertialPurgatory/Inertial_connection_discrete.m.orig @@ -0,0 +1,139 @@ +function [A, h, J, J_full, omega, M_full, local_inertias] = Inertial_connection_discrete(geometry,physics,jointangles) +% Calculate the local connection for for an inertial system (floating in space or +% ideal high-Re fluid) +% +% Inputs: +% +% geometry: Structure containing information about geometry of chain. +% Fields required for this function are: +% +% linklengths: A vector of link lengths defining a kinematic chain +% +% baseframe (optional): Specification of which link on the body should be +% treated as the base frame. Default behavior is to put the base +% frame at the center of the chain, either at the midpoint of the +% middle link, or at the joint between the two middle links and at +% the mean of their orientation. Options for this field are +% specified in the documentation of N_link_chain. +% +% 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 +% will not be scaled. +% +% link_shape: string naming what kind of object the link is +% +% link_shape_parameters: structure containing parameters of link +% shape +% +% physics: Structure containing information about the system's physics. +% Fields are: +% +% fluid_density: density of fluid surrounding object, relative to +% object +% +% jointangles: Angles of the joints between the links, defining the +% chain's current shape. +% +% Outputs: +% +% A: The "local connection" matrix, or "Locomotion Jacobian". This matrix +% maps joint angular velocities ("shape velocities") to body +% velocities of the system's base frame as +% +% g_b = -A * alphadot +% +% with g_b the body velocity and alphadot the joint angular velocity. +% +% (Note the negative sign in this equation; sysplotter code uses the +% classical formulation of the geometric mechanics equations, which +% includes this negative sign. +% +% [h,J,J_full]: location and Jacobians for the links on the chain. These +% are passed through from N_link_chain; see the documentation on that +% function for more details +% +% omega: The "Pfaffian constraint matrix" from which the local connection +% A is calculated. This matrix is a linear map from system body and +% shape velocities to net external forces acting on the base frame, +% which must be zero for all achievable motions of the system +% +% local_inertias: The inertia tensor of the link as measured in its fixed +% coordinate frame, which includes the added mass from the surrounding +% fluid. + + + %%%% + % First, get the positions of the links in the chain and their + % Jacobians with respect to the system parameters +<<<<<<< HEAD + [h, J, J_full,~,~] = N_link_chain(geometry,jointangles); +======= + if isfield(geometry,'subchains') + [h, J, J_full] = branched_chain(geometry,jointangles); + else + [h, J, J_full] = N_link_chain(geometry,jointangles); + end +>>>>>>> master + + %%%%%%%% + % We are modeling low Reynolds number physics as being resistive + % viscous force, with a quasistatic equilibrium condition that inertial + % forces are negligable, so that the viscous drag on the whole system + % is zero at any given time. (The drag is anisotropic, so the system + % can actually be moving even though there is no net force acting on + % it). + % + % Because viscous drag is linear, we can build a linear map from the + % body velocity of a given link to the force and moment acting on the + % link. + % + % Further, because the velocity kinematics from the system's body and + % shape velocities to the body velocity of each link are linearly + % encoded by the Jacobians of the links, we can use these Jacobians to + % pull the drag operators on the links back onto the space of system + % body and shape velocities, giving us a linear map from the system's + % body and shape velocity to the net forces acting on the system + % + % To calculate this linear mapping, we first pre-allocate storage for + % the metric contributions. Each contribution is 3 x m (rows for + % x,y,theta motion, and one column per joint), and there is one + % contribution per link. This structure is of the same dimensions as + % J_full, so we use it as a template. + link_force_maps = J_full; + + % Now iterate over each link, calculating the map from system body and + % shape velocities to forces acting on the body + for idx = 1:numel(link_force_maps) + + [link_inertias{idx},local_inertias{idx}] = Inertia_link(h.pos(idx,:),... % Position of this link relative to the base frame + J_full{idx},... % Jacobian from body velocity of base link and shape velocity to body velocity of this link + h.lengths(idx),... % Length of this link + geometry.link_shape{idx},... % Shape type of this link + geometry.link_shape_parameters{idx},... % Shape parametes for this link + physics.fluid_density); % Fluid density relative to link + + end + + if isa(link_inertias{1},'sym') + M_full = sym(zeros(size(link_inertias{1}))); + inertia_stack = cat(3,link_inertias{:}); + for i = 1:size(inertia_stack,1) + for j = 1:size(inertia_stack,2) + temp = inertia_stack(i,j,:); + M_full(i,j) = sum(temp(:)); + end + end + else + % Sum the force-maps for each link to find the total map from system + % body and shape velocities to force actign on the body + M_full = sum(cat(3,link_inertias{:}),3); + end + + + % Pfaffian is first three rows of M_full + omega = M_full(1:3,:); + + % Build the local connection + A = omega(:,1:3)\omega(:,4:end); +end \ No newline at end of file diff --git a/InertialPurgatory/Inertial_tensors_discrete.m b/InertialPurgatory/Inertial_tensors_discrete.m new file mode 100644 index 00000000..7353744a --- /dev/null +++ b/InertialPurgatory/Inertial_tensors_discrete.m @@ -0,0 +1,148 @@ +function [A, M_alpha, J_full, Inertia_link_local, M_full] = Inertial_tensors_discrete(geometry,physics,jointangles) +% Calculate the local connection for for an inertial system (floating in space or +% ideal high-Re fluid) +% +% Inputs: +% +% geometry: Structure containing information about geometry of chain. +% Fields required for this function are: +% +% linklengths: A vector of link lengths defining a kinematic chain +% +% baseframe (optional): Specification of which link on the body should be +% treated as the base frame. Default behavior is to put the base +% frame at the center of the chain, either at the midpoint of the +% middle link, or at the joint between the two middle links and at +% the mean of their orientation. Options for this field are +% specified in the documentation of N_link_chain. +% +% 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 +% will not be scaled. +% +% link_shape: string naming what kind of object the link is +% +% link_shape_parameters: structure containing parameters of link +% shape +% +% physics: Structure containing information about the system's physics. +% Fields are: +% +% fluid_density: density of fluid surrounding object, relative to +% object +% +% jointangles: Angles of the joints between the links, defining the +% chain's current shape. +% +% Outputs: +% +% A: The "local connection" matrix, or "Locomotion Jacobian". This matrix +% maps joint angular velocities ("shape velocities") to body +% velocities of the system's base frame as +% +% g_b = -A * alphadot +% +% with g_b the body velocity and alphadot the joint angular velocity. +% +% (Note the negative sign in this equation; sysplotter code uses the +% classical formulation of the geometric mechanics equations, which +% includes this negative sign. +% +% [h,J,J_full]: location and Jacobians for the links on the chain. These +% are passed through from N_link_chain; see the documentation on that +% function for more details +% +% omega: The "Pfaffian constraint matrix" from which the local connection +% A is calculated. This matrix is a linear map from system body and +% shape velocities to net external forces acting on the base frame, +% which must be zero for all achievable motions of the system +% +% local_inertias: The inertia tensor of the link as measured in its fixed +% coordinate frame, which includes the added mass from the surrounding +% fluid. + + %%%% + % First, get the positions of the links in the chain and their + % Jacobians with respect to the system parameters + if isfield(geometry,'subchains') + [h, J, J_full] = branched_chain(geometry,jointangles); + else + [h, J, J_full] = N_link_chain(geometry,jointangles); + end + + %If examining inter-panel interaction + if isfield(physics,'interaction') + s.geometry = geometry; + s.physics = physics; + + %Then, get added mass metric using flat-plate panel method + [M_full,J,J_full,h,Inertia_link_local] = getAddedMass_NLinkChain(jointangles',s); + else + + + %%%%%%%% + % We are modeling low Reynolds number physics as being resistive + % viscous force, with a quasistatic equilibrium condition that inertial + % forces are negligable, so that the viscous drag on the whole system + % is zero at any given time. (The drag is anisotropic, so the system + % can actually be moving even though there is no net force acting on + % it). + % + % Because viscous drag is linear, we can build a linear map from the + % body velocity of a given link to the force and moment acting on the + % link. + % + % Further, because the velocity kinematics from the system's body and + % shape velocities to the body velocity of each link are linearly + % encoded by the Jacobians of the links, we can use these Jacobians to + % pull the drag operators on the links back onto the space of system + % body and shape velocities, giving us a linear map from the system's + % body and shape velocity to the net forces acting on the system + % + % To calculate this linear mapping, we first pre-allocate storage for + % the metric contributions. Each contribution is 3 x m (rows for + % x,y,theta motion, and one column per joint), and there is one + % contribution per link. This structure is of the same dimensions as + % J_full, so we use it as a template. + link_force_maps = J_full; + + % Now iterate over each link, calculating the map from system body and + % shape velocities to forces acting on the body + Inertia_link_system = cell(size(force_maps)); + Inertia_link_local = cell(size(force_maps)); + for idx = 1:numel(link_force_maps) + + [Inertia_link_system{idx},Inertia_link_local{idx}] = Inertia_link(h.pos(idx,:),... % Position of this link relative to the base frame + J_full{idx},... % Jacobian from body velocity of base link and shape velocity to body velocity of this link + h.lengths(idx),... % Length of this link + geometry.link_shape_parameters{idx},... % Shape parametes for this link + physics.fluid_density); % Fluid density relative to link + + end + + if isa(Inertia_link_system{1},'sym') + M_full = sym(zeros(size(Inertia_link_system{1}))); + inertia_stack = cat(3,Inertia_link_system{:}); + for i = 1:size(inertia_stack,1) + for j = 1:size(inertia_stack,2) + temp = inertia_stack(i,j,:); + M_full(i,j) = sum(temp(:)); + end + end + else + % Sum the force-maps for each link to find the total map from system + % body and shape velocities to force actign on the body + M_full = sum(cat(3,Inertia_link_system{:}),3); + end + end + + % Pfaffian is first three rows of M_full + omega = M_full(1:3,:); + + % Build the local connection + A = omega(:,1:3)\omega(:,4:end); + + M_alpha = [-A' eye(size(A,2))]*M_full*[-A; eye(size(A,2))]; + +end \ No newline at end of file diff --git a/InertialPurgatory/calc_partial_mass.m b/InertialPurgatory/calc_partial_mass.m new file mode 100644 index 00000000..e79fc3d2 --- /dev/null +++ b/InertialPurgatory/calc_partial_mass.m @@ -0,0 +1,18 @@ +function dM_alphadalpha = calc_partial_mass(s,shapelist) +% Inputs: +% s: sysplotter system struct for an inertial system +% shapelist: cell containing n values where each value represents the +% system's shape at which dM_alphadalpha should be calculated +% Outputs: +% dM_alphadalpha: Partial derivative of mass with respect to shape +% variables; cell with n matrices, one for each shape variable + + % Preallocate output + dM_alphadalpha = cell(size(shapelist)); + % Interpolate in the precalculated dM_alphadalpha grid based on values + % in shapelist + for i = 1:length(shapelist) + dM_alphadalpha{i} = cellfun(@(C) interpn(s.grid.coriolis_eval{:},C,... + shapelist{:},'spline'),s.coriolisfield.coriolis_eval.content.dM_alphadalpha{i}); + end +end \ No newline at end of file diff --git a/InertialPurgatory/calc_second_partial_mass.m b/InertialPurgatory/calc_second_partial_mass.m new file mode 100644 index 00000000..41d14f55 --- /dev/null +++ b/InertialPurgatory/calc_second_partial_mass.m @@ -0,0 +1,18 @@ +function ddM_alphadalpha = calc_second_partial_mass(s,shapelist) +% Inputs: +% s: sysplotter system struct for an inertial system +% shapelist: cell containing n values where each value represents the +% system's shape at which ddM_alphadalpha should be calculated +% Outputs: +% ddM_alphadalpha: Second partial derivative of mass with respect to shape +% variables; n-by-n cell of n-by-n matrices, one for each shape variable + + % Preallocate output + ddM_alphadalpha = cell(length(shapelist)); + % Interpolate in the precalculated dM_alphadalpha grid based on values + % in shapelist + for i = 1:numel(ddM_alphadalpha) + ddM_alphadalpha{i} = cellfun(@(C) interpn(s.grid.coriolis_eval{:},C,... + shapelist{:},'spline'),s.coriolisfield.coriolis_eval.content.ddM_alphadalpha{i}); + end +end \ No newline at end of file diff --git a/InertialPurgatory/coriolis_eval.m b/InertialPurgatory/coriolis_eval.m new file mode 100644 index 00000000..abb3a90e --- /dev/null +++ b/InertialPurgatory/coriolis_eval.m @@ -0,0 +1,42 @@ +function C = coriolis_eval(M,dMdq,A,jointangles,jointvelocities) + dmdalpha = pull_back_partial_mass(M,dMdq,A,jointangles); + C_from_vel = zeros(size(dmdalpha)); + jointvelocities = jointvelocities(:); + if isa(dmdalpha,'sym') + C_from_vel = sym(C_from_vel); + end + C_from_pos = C_from_vel; + for i = 1:numel(jointvelocities) + C_from_vel = C_from_vel + dmdalpha{i}*jointvelocities(i); + C_from_pos(i) = -(1/2)*jointvelocities'*dmdalpha{i}*jointvelocities; + end + C_from_vel = C_from_vel*jointvelocities; + C = C_from_pos + C_from_vel; + if isa(C,'sym') + C = simplify(C,10); + end +end + +function dmdalpha = pull_back_partial_mass(M,dMdq,A,jointangles) + dAdalpha = {zeros(size(A))}; + num_joints = size(A,2); + dAdalpha = repmat(dAdalpha,1,num_joints); + A_interp = zeros(size(A)); + for i = 1:numel(A) + % Need some way to make this number-of-joints-agnostic + [da1,da2] = gradient(A{i}); + dA = {da1,da2}; + for joint = 1:size(A,2) + dAdalpha{joint}(i) = interpn(X,Y,dA{joint},jointangles(1),jointangles(2)); + end + A_interp(i) = interpn(X,Y,A{i},jointangles(1),jointangles(2)); + end + + dmdalpha = cell(size(A,2),1); + for i = 1:numel(dmdalpha) + + dmdalpha{i} = [-dAdalpha{i}', zeros(size(A,2))]*M*[-A_interp; eye(size(A,2))] ... + + [-A_interp', eye(size(A,2))]*dMdq*[-A_interp; eye(size(A,2))] ... + + [-A_interp', eye(size(A,2))]*M*[-dAdalpha{i}; zeros(size(A,2))]; + end +end \ No newline at end of file diff --git a/InertialPurgatory/coriolis_from_partial_mass.m b/InertialPurgatory/coriolis_from_partial_mass.m new file mode 100644 index 00000000..efec55cd --- /dev/null +++ b/InertialPurgatory/coriolis_from_partial_mass.m @@ -0,0 +1,17 @@ +function C_alpha = coriolis_from_partial_mass(dMdalpha,jointvelocities) + C_from_vel = zeros(size(dMdalpha)); + jointvelocities = jointvelocities(:); + if isa(dMdalpha,'sym') + C_from_vel = sym(C_from_vel); + end + C_from_pos = C_from_vel; + for i = 1:numel(jointvelocities) + C_from_vel = C_from_vel + dMdalpha{i}*jointvelocities(i); + C_from_pos(i) = -(1/2)*jointvelocities'*dMdalpha{i}*jointvelocities; + end + C_from_vel = C_from_vel*jointvelocities; + C_alpha = C_from_pos + C_from_vel; + if isa(C_alpha,'sym') + C_alpha = simplify(C_alpha,10); + end +end \ No newline at end of file diff --git a/InertialPurgatory/curvature_calc4.m b/InertialPurgatory/curvature_calc4.m new file mode 100644 index 00000000..6f2a0e46 --- /dev/null +++ b/InertialPurgatory/curvature_calc4.m @@ -0,0 +1,319 @@ +function [kappa_final1,kappa_final2,F, dkappa1,dkappa2] = curvature_calc4(i,r,M,P,cons,Gamma,dM) %-----Removed method as fifth input + +a = P.alpha_field{1}; +b = P.alpha_field{2}; + +Mp_raw = P.M_t; + +% This code create the local normalization + +% Find the angle of horizental line +SE2z = @(t) [cos(t) -sin(t) 0;sin(t) cos(t) 0; 0 0 1]; +SE2y = @(t) [cos(t) 0 sin(t);0 1 0;-sin(t) 0 cos(t)]; +SE2x = @(t) [1 0 0;0 cos(t) -sin(t);0 sin(t) cos(t)]; + +SE2 = @(t) [cos(t) -sin(t);sin(t) cos(t)]; + +% Singular Value Decomposition +[u,s,v] = svd(M{i}); + +% Tissot Indicatrix +T = u*s^-0.5*v'; + +% Transform the metrix with Tissot + +% Transform the coordinate with Tissot +U_temp = r*inv(T); + +U_tempp1 = r*inv(T); + +U_temps1 = r*inv(T); + +th = atan2(U_temp(i+1,2)-U_temp(i-1,2), U_temp(i+1,1)-U_temp(i-1,1)); + +thp1 = atan2(U_tempp1(i+2,2)-U_tempp1(i,2), U_tempp1(i+2,1)-U_tempp1(i,1)); + +ths1 = atan2(U_temps1(i,2)-U_temps1(i-2,2), U_temps1(i,1)-U_temps1(i-2,1)); + +th1 = atan2(r(i+1,2)-r(i-1,2), r(i+1,1)-r(i-1,1)); +th1p1 = atan2(r(i+2,2)-r(i,2), r(i+2,1)-r(i,1)); +th1s1 = atan2(r(i,2)-r(i-2,2), r(i,1)-r(i-2,1)); + +% Fit the plane over the 3 points +% xnew = [(U_temp(i+1,:) - U_temp(i-1,:))/(norm(U_temp(i+1,:)-U_temp(i-1,:))) 0]; +% v_needed = [(U_temp(i+1,:) - U_temp(i,:))/(norm(U_temp(i+1,:) - U_temp(i,:))) 0]; +% +% znew_temp = cross(xnew,v_needed); +% znew = znew_temp/norm(znew_temp); +% +% ynew_temp = cross(znew,xnew); +% ynew = ynew_temp/norm(ynew_temp); +% +% if isnan(znew) +% znew = zeros(3,1); +% ynew = zeros(3,1); +% end +% +% +% x0 = [1 0 0]; +% y0 = [0 1 0]; +% z0 = [0 0 1]; +% % Make the transformation matrix +% Tr = [dot(x0,xnew) dot(x0,ynew) dot(x0,znew); +% dot(y0,xnew) dot(y0,ynew) dot(y0,znew); +% dot(z0,xnew) dot(z0,ynew) dot(z0,znew)]; +% +% Tr = Tr(1:2,1:2); + +Tr = SE2(th); + +Tr1 = SE2(th1); +Tr1p1 = SE2(th1p1); +Tr1s1 = SE2(th1s1); + +Trp1 = SE2(thp1); + +Trs1 = SE2(ths1); + +% Transform the coordinate with the above rotational angle +U = U_temp*Tr; + +Up1 = U_tempp1*Trp1; + +Us1 = U_temps1*Trs1; + +U1 = r*(SE2(th1)); +U1p1 = r*(SE2(th1p1)); +U1s1 = r*(SE2(th1s1)); + +%-----Pretty sure Hossein said that we'll only use Numerical case-----% +% switch method +% +% case 'Analytical' % If you have your metric equation you can use this option +% +% % Analytical function goes here + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% case 'Numerical' % When the metric is given for all the configuration you can use this option + + Gamma2 = Gamma{i}; + Gammap1 = Gamma{i+1}; + Gammas1 = Gamma{i-1}; + +% end +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Obtain the length of Triangle sides +L_i = sqrt((r(i+1,:)-r(i,:))*((M{i}+M{i+1})/2)*(r(i+1,:)-r(i,:))'); +L_is1 = sqrt((r(i,:)-r(i-1,:))*((M{i-1}+M{i})/2)*(r(i,:)-r(i-1,:))'); + +L_is2 = sqrt((r(i-1,:)-r(i-2,:))*((M{i-1}+M{i-2})/2)*(r(i-1,:)-r(i-2,:))'); +L_ip1 = sqrt((r(i+2,:)-r(i+1,:))*((M{i+2}+M{i+1})/2)*(r(i+2,:)-r(i+1,:))'); + + +% Speed squared required for finding Geodesic Curvature +v_it = (0.5*L_i + 0.5*L_is1)^2; + +% Find the change of angle +y1 = U(i,2) - U(i-1,2); +y2 = U(i+1,2) - U(i,2); +x1 = U(i,1) - U(i-1,1); +x2 = U(i+1,1) - U(i,1); + + +% next point +y1p1 = Up1(i+1,2) - Up1(i,2); +y2p1 = Up1(i+2,2) - Up1(i+1,2); +x1p1 = Up1(i+1,1) - Up1(i,1); +x2p1 = Up1(i+2,1) - Up1(i+1,1); + + +% % Previous point +y1s1 = Us1(i-1,2) - Us1(i-2,2); +y2s1 = Us1(i,2) - Us1(i-1,2); +x1s1 = Us1(i-1,1) - Us1(i-2,1); +x2s1 = Us1(i,1) - Us1(i-1,1); + + + +%% Find the middle point of triangle in main coordinate (U1) +Mid_pts = [U1(i,1) (U1(i+1,2)+U1(i-1,2))/2]; +Mid_ptsp1 = [U1p1(i+1,1) (U1p1(i+2,2)+U1p1(i,2))/2]; +Mid_ptss1 = [U1s1(i-1,1) (U1s1(i,2)+U1s1(i-2,2))/2]; + +% Take the point to original coordinate to find the metric +Mid_pts_org = Mid_pts*inv(Tr1); +Mid_pts_orgp1 = Mid_ptsp1*inv(Tr1p1); +Mid_pts_orgs1 = Mid_ptss1*inv(Tr1s1); + +% Find the metric +M_mid_org = Granular_metric_calc2({Mid_pts_org(1),Mid_pts_org(2)},Mp_raw,a,b); +M_mid_orgp1 = Granular_metric_calc2({Mid_pts_orgp1(1),Mid_pts_orgp1(2)},Mp_raw,a,b); +M_mid_orgs1 = Granular_metric_calc2({Mid_pts_orgs1(1),Mid_pts_orgs1(2)},Mp_raw,a,b); + +% Transform the metric to rotated coordinate +M_mid = inv(SE2(th1))*M_mid_org*inv(SE2(th1))'; +M1_i = inv(SE2(th1))*M{i}*inv(SE2(th1))'; +M1_ip1 = inv(SE2(th1))*M{i+1}*inv(SE2(th1))'; +M1_is1 = inv(SE2(th1))*M{i-1}*inv(SE2(th1))'; + +M_midp1 = inv(SE2(th1p1))*M_mid_orgp1*inv(SE2(th1p1))'; +M1p1_i = inv(SE2(th1p1))*M{i}*inv(SE2(th1p1))'; +M1p1_ip1 = inv(SE2(th1p1))*M{i+1}*inv(SE2(th1p1))'; +M1p1_ip2 = inv(SE2(th1p1))*M{i+2}*inv(SE2(th1p1))'; + +M_mids1 = inv(SE2(th1s1))*M_mid_orgs1*inv(SE2(th1s1))'; +M1s1_i = inv(SE2(th1s1))*M{i}*inv(SE2(th1s1))'; +M1s1_is1 = inv(SE2(th1s1))*M{i-1}*inv(SE2(th1s1))'; +M1s1_is2 = inv(SE2(th1s1))*M{i-2}*inv(SE2(th1s1))'; + +if isnan(M_mid) + M_mid = eye(2,2); +end + +y_org = sign(y1)*sqrt((U1(i,:) - Mid_pts)*((M1_i+M_mid)/2)*(U1(i,:) - Mid_pts)'); +x_org1 = sign(x1)*sqrt((Mid_pts - U1(i-1,:))*((M1_is1+M_mid)/2)*(Mid_pts - U1(i-1,:))'); +x_org2 = sign(x2)*sqrt((U1(i+1,:) - Mid_pts)*((M1_ip1+M_mid)/2)*(U1(i+1,:) - Mid_pts)'); + +y_orgp1 = sign(y1p1)*sqrt((U1p1(i+1,:) - Mid_ptsp1)*((M1p1_ip1+M_midp1)/2)*(U1p1(i+1,:) - Mid_ptsp1)'); +x_org1p1 = sign(x1p1)*sqrt((Mid_ptsp1 - U1p1(i,:))*((M1p1_i+M_midp1)/2)*(Mid_ptsp1 - U1p1(i,:))'); +x_org2p1 = sign(x2p1)*sqrt((U1p1(i+2,:) - Mid_ptsp1)*((M1p1_ip2+M_midp1)/2)*(U1p1(i+2,:) - Mid_ptsp1)'); + +y_orgs1 = sign(y1s1)*sqrt((U1s1(i-1,:) - Mid_ptss1)*((M1s1_is1+M_mids1)/2)*(U1s1(i-1,:) - Mid_ptss1)'); +x_org1s1 = sign(x1s1)*sqrt((Mid_ptss1 - U1s1(i-2,:))*((M1s1_is2+M_mids1)/2)*(Mid_ptss1 - U1s1(i-2,:))'); +x_org2s1 = sign(x2s1)*sqrt((U1s1(i,:) - Mid_ptss1)*((M1s1_i+M_mids1)/2)*(U1s1(i,:) - Mid_ptss1)'); + +delta_thtest = -(atan2(y_org,x_org1) + atan2(y_org,x_org2)); +delta_thtestp1 = (atan2(y_orgp1,x_org1p1) + atan2(y_orgp1,x_org2p1)); +delta_thtests1 = (atan2(y_orgs1,x_org1s1) + atan2(y_orgs1,x_org2s1)); + + +% Now take derivative of this +if y_org ~= 0 + dy_org = (2*[0 1]*((M1_i+M_mid)/2)*(U1(i,:) - Mid_pts)'/(2*y_org));% + 0.5*(U1(i,:) - Mid_pts)*((dMy ))*(U1(i,:) - Mid_pts)'/(2*y_org); + dx_org1 = 2*[1 0]*((M1_is1+M_mid)/2)*(Mid_pts - U1(i-1,:))'/(2*x_org1); +else + dy_org = 0; + dx_org1 = 0; +end + +if y_orgp1 ~= 0 + dy_org_p1 = (2*[0 -0.5]*((M1p1_ip1+M_midp1)/2)*(U1p1(i+1,:) - Mid_ptsp1)'/(2*y_orgp1));% + 0.5*(U1p1(i+1,:) - Mid_ptsp1)*((dMyp1 ))*(U1p1(i+1,:) - Mid_ptsp1)'/(2*y_orgp1); + dx_org1_p1 = 2*[-1 0]*((M1p1_i+M_midp1)/2)*(Mid_ptsp1 - U1p1(i,:))'/(2*x_org1p1); +else + dy_org_p1 = 0; + dx_org1_p1 = 0; +end + +if y_orgs1 ~= 0 + dy_org_s1 = (2*[0 -0.5]*((M1s1_is1+M_mids1)/2)*(U1s1(i-1,:) - Mid_ptss1)'/(2*y_orgs1));% + 0.5*(U1s1(i-1,:) - Mid_ptss1)*((dMys1 ))*(U1s1(i-1,:) - Mid_ptss1)'/(2*y_orgs1); + dx_org2_s1 = 2*[-1 0]*((M1s1_i+M_mids1)/2)*(U1s1(i,:) - Mid_ptss1)'/(2*x_org2s1); +else + dy_org_s1 = 0; + dx_org2_s1 = 0; +end + + +ddelta_thtest_y = ( dy_org*((1/x_org1 ) / (1 + (y_org/x_org1)^2)) + dy_org*(1/x_org2) / (1 + (y_org/x_org2)^2)); +ddelta_thtest_yp1 = ( dy_org_p1*((1/x_org1p1 ) / (1 + (y_orgp1/x_org1p1)^2)) + dy_org_p1*(1/x_org2p1) / (1 + (y_orgp1/x_org2p1)^2)); +ddelta_thtest_ys1 = ( dy_org_s1*((1/x_org1s1 ) / (1 + (y_orgs1/x_org1s1)^2)) + dy_org_s1*(1/x_org2s1) / (1 + (y_orgs1/x_org2s1)^2)); + +ddelta_thtest_x = ( -dx_org1*((y_org/(x_org1^2) ) / (1 + (y_org/x_org1)^2))) -dx_org1*((y_org/(x_org1^2) ) / (1 + (y_org/x_org1)^2)); +ddelta_thtest_xp1 = ( -dx_org1_p1*((y_orgp1/(x_org1p1^2) ) / (1 + (y_orgp1/x_org1p1)^2))); +ddelta_thtest_xs1 = ( -dx_org2_s1*(y_orgs1/(x_org2s1^2)) / (1 + (y_orgs1/x_org2s1)^2)) ; +%% + + +% Parameterized Curvature (Note: the true curvature is after normalization) +kappa = delta_thtest/(1*(0.5*L_i+0.5*L_is1)); + +%-----Pretty sure Hossein said we'll only use Numerical method-----% +% Calculate the curvature from geodesic line (curvature calculated from the christoffel symbols) +% if strcmp(method, 'Analytical') +% Gamma = Gamma1; +% else + Gamma = Gamma2; +% end + + +k1_temp2 = ((r(i+1,:)-r(i,:))/2 + (r(i,:)-r(i-1,:))/2)*Gamma(:, :, 1)*((r(i+1,:)-r(i,:))/2 + (r(i,:)-r(i-1,:))/2)'; +k2_temp2 = ((r(i+1,:)-r(i,:))/2 + (r(i,:)-r(i-1,:))/2)*Gamma(:, :, 2)*((r(i+1,:)-r(i,:))/2 + (r(i,:)-r(i-1,:))/2)'; +% k3_temp2 = ((r(i+1,:)-r(i,:))/2 + (r(i,:)-r(i-1,:))/2)*Gamma(:, :, 3)*((r(i+1,:)-r(i,:))/2 + (r(i,:)-r(i-1,:))/2)'; + + +k_temp = [k1_temp2 k2_temp2 ]*inv(T)*(Tr); + +k1_temp = k_temp(1); +k2_temp = k_temp(2); + +% Geodesic curvature which is how much a curve is away from being Geodesic +kappa_final2 = (kappa - k2_temp/v_it); +kappa_final1 = k1_temp/v_it; + +% Calculate the gradient of length +shv = size(r,2); +for j = 1:shv + + dr = zeros(1,shv); + dr(j) = 1; + + dL_i{j} = 1*(-dr*((M{i}+M{i+1} )/2)*(r(i+1,:)-r(i,:))' + (r(i+1,:)-r(i,:))*((M{i}+M{i+1} )/2)*(-dr') + 0.5*(r(i+1,:)-r(i,:))*((dM{i,j} ))*(r(i+1,:)-r(i,:))')/(2*L_i); + dL_is1{j} = 1*(dr*((M{i-1}+M{i} )/2)*(r(i,:)-r(i-1,:))' + (r(i,:)-r(i-1,:))*((M{i-1}+M{i} )/2)*(dr') + 0.5*(r(i,:)-r(i-1,:))*(( dM{i,j}))*(r(i,:)-r(i-1,:))')/(2*L_is1); + +end + + +dL_i = cell2mat(dL_i)*(Tr1); +dL_is1 = cell2mat(dL_is1)*(Tr1); + +dL_ix = dL_i(1); +dL_iy = dL_i(2); + +dL_is1x = dL_is1(1); +dL_is1y = dL_is1(2); + + + +dkappa_y_temp = ((ddelta_thtest_yp1 + ddelta_thtest_ys1 + ddelta_thtest_y)*(1*(0.5*L_i+0.5*L_is1)) - delta_thtest*(dL_iy+dL_is1y)) /(1*(0.5*L_i+0.5*L_is1))^2 ; +dkappa_x_temp = (0*(ddelta_thtest_xp1 + ddelta_thtest_xs1 + ddelta_thtest_x)*(1*(0.5*L_i+0.5*L_is1)) - delta_thtest*(0.5*dL_ix+0.5*dL_is1x)) /(1*(0.5*L_i+0.5*L_is1))^2 ; + + +dkappa2 = [0 dkappa_y_temp]*inv(Tr1); +dkappa1 = [dkappa_x_temp 0]*inv(Tr1); + + +% Check the points on the rail in order to make the constraints + +% Constraints check +% if ~isempty(cons) +% % First need to check the node +% for j = 1:length(cons.path_points) +% +% if ismember(i,cons.path_points{j}) +% +% T_rail_normalized = cons.T_rail{j}/norm(cons.T_rail{j}); +% dkappa1 = dot(dkappa1,T_rail_normalized)*T_rail_normalized; +% dkappa2 = dot(dkappa2,T_rail_normalized)*T_rail_normalized; +% +% % Find the gradiant to keep the path on the rail +% F = 10*cons.connecting_vector{j}; +% +% else +% +% F = 0; +% +% end +% +% end +% +% else +% +% F = 0; +% +% end + +F = 0; + + + +end diff --git a/InertialPurgatory/ellipse.m b/InertialPurgatory/ellipse.m new file mode 100644 index 00000000..cbbfee7c --- /dev/null +++ b/InertialPurgatory/ellipse.m @@ -0,0 +1,45 @@ +function [zcg,t,n,del] = ellipse(a,b,npts) +% ----------------- +% E Kanso, 14 april 2004 + + +% -----------------INPUT +% +% a & b major and minor axes of the ellipse +% +% npts number of panels used to discretize the ellipse +% +% ---------------- + +% -----------------OUTPUT +% +% zcg position of collocation pts w.r.t inertial +% frame attached at the c.o.m of the ellipse +% +% t components of vectors tangent to panels +% +% n components of outward normal vectors +% +% del panel length +% +% ---------------- + +beta = linspace(0,2*pi,npts+1); +alpha = beta'; + +Px = a*cos(alpha); Py = b*sin(alpha); +Px = Px(npts+1:-1:1); Py = Py(npts+1:-1:1); + +% control pts +xcg = (Px(1:1:npts) + Px(2:1:npts+1))/2; +ycg = (Py(1:1:npts) + Py(2:1:npts+1))/2; +zcg = [xcg,ycg]; + +% Panel length +Xrel = diff(Px); Yrel = diff(Py); +del = sqrt(Xrel.^2+Yrel.^2); + +% Tangent and normal vectors to panels +tx = Xrel./del; ty = Yrel./del; +t = [tx,ty]; n = [-ty,tx]; + diff --git a/InertialPurgatory/evaluate_displacement_and_cost.m b/InertialPurgatory/evaluate_displacement_and_cost.m new file mode 100644 index 00000000..8b05d6fb --- /dev/null +++ b/InertialPurgatory/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 \ No newline at end of file diff --git a/InertialPurgatory/evaluate_inertia_tensors_in_system_file.m b/InertialPurgatory/evaluate_inertia_tensors_in_system_file.m new file mode 100644 index 00000000..a5f38c17 --- /dev/null +++ b/InertialPurgatory/evaluate_inertia_tensors_in_system_file.m @@ -0,0 +1,60 @@ +function s = evaluate_inertia_tensors_in_system_file(s,component_list,zoom_list,destination) + + %loop over list of zoom levels, creating the vector fields + for i = 1:size(zoom_list,1) + + % Shape values in grid + a = s.grid.(zoom_list{i,2}); + + + + %iterate over list of possible components + for j = 1:length(component_list) + + %check if component is present for this system + if isfield(s,component_list{j}) + + % Check to see if the user has acknowledged the presence of + % a singularity in the connection and or metric + ignore_singularity_warning = isfield(s,'ignore_singularity_warning') && s.ignore_singularity_warning; + + + %Evaluate the function over the grid, as a cell array + %where the outer structure is that of the tensor, and the + %inner structure is that of the grid + + if strcmpi(component_list{j},'A') + [s.A,s.J,s.J_full,s.M_full] = evaluate_inertia_local_connection(s.(component_list{j}),a,ignore_singularity_warning); + T = evaluate_tensor_over_grid(s.(component_list{j}),a,ignore_singularity_warning); + + % If there are any NaN values and a singularity has not been called out + % inpaint the NaNs and warn the user + + nan_present = cellfun(@(Tc) any(isnan(Tc(:))),T); + if ~isfield(s,'singularity') && ... + any(nan_present(:)) + + + T = cellfun(@(Tc) inpaint_nans(Tc,4),T,'UniformOutput',false); + + if ~ignore_singularity_warning + warning('NaN values were inpainted on a tensor, but a singularity was not specified in the file') + end + end + + s.(destination).(zoom_list{i,1}).content.(component_list{j}) = T; + + % Mark what zoom level was used to create this field + s.(destination).(zoom_list{i,1}).type = zoom_list{i,2}; + + end + + end + + + + + + end + +end diff --git a/InertialPurgatory/evaluate_inertial_properties.m b/InertialPurgatory/evaluate_inertial_properties.m new file mode 100644 index 00000000..b6860523 --- /dev/null +++ b/InertialPurgatory/evaluate_inertial_properties.m @@ -0,0 +1,70 @@ +%Evaluate inertial properties over the fine grid for calculations and the coarse +%grid for vector display +function s = evaluate_inertial_properties(s) + + %list of all components of the local connection and metric that may be present + component_list = {{'M_alpha'},{'dM_alphadalpha'}}; + eval_list = {{'mass_eval','mass_eval'},{'coriolis_eval','coriolis_eval'}}; + display_list = {{'mass_display','mass_display'},{'coriolis_display','coriolis_display'}}; + field_list = {'massfield','coriolisfield'}; + + % Evaluate all components in the list + for i = 1:length(component_list) + s = evaluate_tensors_in_system_file(s,component_list{i},eval_list{i},field_list{i}); + end + % Use a numerical approach for the second partial derivative of the + % mass matrix + s = evaluate_mass_second_derivative_numerical(s); + % Validation of dM_alphadalpha using Matlab's grid function; used for + % debugging +% validate_mass_first_derivative(s); +end + +function validate_mass_first_derivative(s) + dM_alphadalpha = s.coriolisfield.coriolis_eval.content.dM_alphadalpha; + M = s.massfield.mass_eval.content.M_alpha; + M_grid = s.grid.mass_eval; + [dA2, dA1] = cellfun(@(C) gradient(C,M_grid{2}(1,:),M_grid{1}(:,1)),M,'UniformOutput',false); + err1 = cell(size(dA1)); + for i=1:numel(err1) + err1{i} = dM_alphadalpha{1}{i} - dA1{i}; + end + err2 = cell(size(dA2)); + for i=1:numel(err1) + err2{i} = dM_alphadalpha{2}{i} - dA2{i}; + end +end + +function s = evaluate_mass_second_derivative_numerical(s) + % This function assumes that the first partial derivative + % dM_alpha/dalpha has already been calculated over the grid specified + % by s.grid.coriolis_eval and stored in s.coriolisfield. Note that, + % because this is the second partial derivative, each partial + % derivative will need to have its own partial derivative taken with + % respect to each of the shape variables, which augments the space of + % the second partial derivative to be one greater than that of the + % partial derivative. + + % Square cell with dimension equal to the size of the dM_alphadalpha + % vector, since dimension is augmented + num_joints = length(s.coriolisfield.coriolis_eval.content.dM_alphadalpha); + grid = s.grid.coriolis_eval; + + grid_baseline = cell(size(grid)); + callout_template = num2cell(ones(size(grid_baseline))); + for idx = 1:numel(grid_baseline) + callout = callout_template; + callout{idx} = ':'; + grid_baseline{idx} = squeeze(grid{idx}(callout{:})); + end + + + ddM_alphadalpha = cell(num_joints); + for first_partial = 1:num_joints + dM_first_partial = s.coriolisfield.coriolis_eval.content.dM_alphadalpha{first_partial}; + [ddM_alphadalpha{first_partial,[2,1,3:num_joints]}] = cellfun(@(C) gradient(C,grid_baseline{[2,1,3:num_joints]}),dM_first_partial,'UniformOutput',false); +% ddM_alphadalpha{first_partial,1} = dA1; +% ddM_alphadalpha{first_partial,2} = dA2; + end + s.coriolisfield.coriolis_eval.content.ddM_alphadalpha = ddM_alphadalpha; +end diff --git a/InertialPurgatory/evaluate_mass_second_derivative_numerical.m b/InertialPurgatory/evaluate_mass_second_derivative_numerical.m new file mode 100644 index 00000000..1e5763a1 --- /dev/null +++ b/InertialPurgatory/evaluate_mass_second_derivative_numerical.m @@ -0,0 +1,23 @@ +function s = evaluate_mass_second_derivative_numerical(s) + % This function assumes that the first partial derivative + % dM_alpha/dalpha has already been calculated over the grid specified + % by s.grid.coriolis_eval and stored in s.coriolisfield. Note that, + % because this is the second partial derivative, each partial + % derivative will need to have its own partial derivative taken with + % respect to each of the shape variables, which augments the space of + % the second partial derivative to be one greater than that of the + % partial derivative. + + % Square cell with dimension equal to the size of the dM_alphadalpha + % vector, since dimension is augmented + num_joints = length(s.coriolisfield.coriolis_eval.content.dM_alphadalpha); + grid = s.grid.coriolis_eval; + ddM_alphadalpha = cell(num_joints); + for first_partial = 1:num_joints + dM_first_partial = s.coriolisfield.coriolis_eval.content.dM_alphadalpha{first_partial}; + [dA2, dA1] = cellfun(@(C) gradient(C,grid{2}(1,:),grid{1}(:,1)),dM_first_partial,'UniformOutput',false); + ddM_alphadalpha{first_partial,1} = dA1; + ddM_alphadalpha{first_partial,2} = dA2; + end + s.coriolisfield.coriolis_eval.content.ddM_alphadalpha = ddM_alphadalpha; +end \ No newline at end of file diff --git a/InertialPurgatory/jacobiandispcalculator3.m b/InertialPurgatory/jacobiandispcalculator3.m new file mode 100644 index 00000000..3c53517e --- /dev/null +++ b/InertialPurgatory/jacobiandispcalculator3.m @@ -0,0 +1,57 @@ +function a=jacobiandispcalculator3(p1,p2,p3,height) + +l = sqrt((p1(1)-p3(1))^2+(p1(2)-p3(2))^2);%+(p1(3)-p3(3))^2); + +%% local coordinates +xparallel1 = [p3(1)-p1(1),p3(2)-p1(2)];%,p3(3)-p1(3)]; +xparallel = [xparallel1/norm(xparallel1) 0]; + +xneeded1 = [p2(1)-p1(1),p2(2)-p1(2)];% ,p2(3)-p1(3)]; +xneeded = [xneeded1/norm(xneeded1) 0]; + +xper21 = cross(xparallel,xneeded); +xper2 = [xper21/norm(xper21)]; + +xper11 = cross(xper2,xparallel); +xper1 = [xper11/norm(xper11)]; + +%% jacobian calculation +per1flux = -xper2; +per2flux = xper1; + +heightper1 = height*per1flux'; +% heightper2=height*per2flux'; + +jacobian = 0.5*l*heightper1*xper1; %+0.5*l*heightper2*xper2; +jacobian(3) = []; +a = jacobian; + + +% l1=0; +% for i=1:1:dimension +% l1=l1+(p1(i)-p3(i))^2; +% base(1,i)=p3(i)-p1(i); +% end +% l=sqrt(l1); +% +% for i=1:1:dimension +% jacobian(1,i)=0; +% perp1=zeros(1,dimension); +% perp1(i)=1; +% %parcomp=base*perp1'/norm(base); +% perp=perp1;%perp1-parcomp*base/norm(base); %%recheck again +% for j=1:dimension-1 +% for k=1:dimension-j +% veca=zeros(1,dimension); +% vecb=zeros(1,dimension); +% veca(j)=1; +% vecb(j+k)=1; +% f=(j-1)*dimension-(j*(j-1))/2+k; +% jacobian(1,i)=jacobian(1,i)+0.5*height(f)*((veca*perp')*(vecb*base')-(vecb*perp')*(veca*base')); +% end +% end +% end +% +% a=jacobian; + +end diff --git a/InertialPurgatory/lagrangian_floating_snake.m b/InertialPurgatory/lagrangian_floating_snake.m new file mode 100644 index 00000000..41525d67 --- /dev/null +++ b/InertialPurgatory/lagrangian_floating_snake.m @@ -0,0 +1,148 @@ +clc +clear +syms m I alpha1 alpha2 dalpha1 dalpha2 c1 c2 s1 s2 + +M = [m 0 0;0 m 0; 0 0 I]; % Inertia Matrix +l = sym('l',[1,3]); % Links length + +dxb = sym('dxb',[1,3]); + +g_circ = sym('g_circ',[1,3]); + +% T = @(tt) [cos(tt) sin(tt) 0; -sin(tt) cos(tt) 0; 0 0 1]; + +Left_lifted_action = @(theta) [cos(theta) sin(theta) 0; -sin(theta) cos(theta) 0; 0 0 1]; + +Right_lifted_action = @(x,y) [1 0 -y; 0 1 x; 0 0 1]; + + +% LA1 = [c1 -s1 0;s1 c1 0;0 0 1]; +% LA2 = [c2 s2 0;-s2 c2 0;0 0 1]; + + +% Find the body velocity for left link +f2 = Right_lifted_action(-l(2)/2,0)*transpose(g_circ); +h1 = Left_lifted_action(-alpha1)*(f2 + [0;0;-dalpha1]); +% h1 = LA1*(f2 + [0;0;-dalpha1]); +g1_circ = Right_lifted_action(-l(1)/2,0)*h1; + +% Find the body velocity for left link +h2 = Right_lifted_action(l(2)/2,0)*transpose(g_circ); +f3 = Left_lifted_action(alpha2)*(h2 + [0;0;dalpha2]); +% f3 = LA2*(h2 + [0;0;dalpha2]); +g3_circ = Right_lifted_action(l(3)/2,0)*f3; + +% Find the body velocity for middle link +g2_circ = transpose(g_circ); + + +var = [g_circ dalpha1 dalpha2]; +var = [g_circ dalpha1 dalpha2;1:length(var)]; + +for j = 1:size(g1_circ,1) + for i = 1:length(var) + + [c1,t1] = coeffs(g1_circ(j),[var(1,i)]); + + if (t1(1)) ~= 1 + J1(j,i) = c1(1); + else + J1(j,i) = 0; + end + + [c2,t2] = coeffs(g2_circ(j),[var(1,i)]); + + if t2(1) ~= 1 + J2(j,i) = c2(1); + else + J2(j,i) = 0; + end + + [c3,t3] = coeffs(g3_circ(j),[var(1,i)]); + + if (t3(1)) ~= 1 + J3(j,i) = c3(1); + else + J3(j,i) = 0; + end + + end +end + +M_new = 0.5*transpose(J1)*M*J1 + 0.5*transpose(J2)*M*J2 +... + 0.5*transpose(J3)*M*J3; + +II = M_new(1:3,1:3); +IA = M_new(1:3,4:5); + +A = simplify(II\IA); + +% Find the Metric +M = transpose([-A; eye(2)])*M_new*[-A;eye(2)]; + +% ----------------------------------------------------------------------- % + +% % Lagrangian Equation +% L = 0.5*transpose(g1_circ)*M*g1_circ + 0.5*transpose(g2_circ)*M*g2_circ +... +% 0.5*transpose(g3_circ)*M*g3_circ; +% +% % Simplify the equation +% L = simplify (L); +% +% J1 = jacobian(L,[g_circ dalpha1 dalpha2]); +% +% +% J2 = jacobian(J1,[g_circ dalpha1 dalpha2]); +% +% for i =1:length(J2) +% J2(i,i) = 0.5*J2(i,i); +% end +% +% II1 = J2(1:3,1:3); +% IA1 = J2(1:3,4:5); +% +% A1 = simplify(II1\IA1) +% +% var = [g_circ dalpha1 dalpha2]; +% var = [g_circ dalpha1 dalpha2;1:length(var)]; +% +% % M = zeros(5,5); +% for i= 1:length(var) +% +% [c,t] = coeffs(L,[var(1,i)]); +% +% if length(t) > 2 +% M_k(i,i) = c(1); +% +% var_temp = var; +% var_temp(:,i) = []; +% for j=1:length(var_temp) +% [cs,ts] = coeffs(c(2),[var_temp(1,j)]); +% +% if length(ts) > 1 +% M_k(i,var_temp(2,j)) = cs(1); +% else +% M_k(i,var_temp(2,j)) = 0; +% end +% +% end +% +% end +% +% end +% +% +% II = M_k(1:3,1:3); +% IA = M_k(1:3,4:5); +% +% A = simplify(II\IA); +% +% % Find the Metric +% M = transpose([-A; eye(2)])*M_k*[-A;eye(2)]; + +f1 = matlabFunction(A1, 'file', 'A_func'); + +f2 = matlabFunction(M, 'file', 'M_func'); + +f2 = matlabFunction(M, 'file', 'M_func'); + diff --git a/InertialPurgatory/mass_matrix.m b/InertialPurgatory/mass_matrix.m new file mode 100644 index 00000000..24597859 --- /dev/null +++ b/InertialPurgatory/mass_matrix.m @@ -0,0 +1,24 @@ +function M_alpha = mass_matrix(geometry,physics,jointangles,s) +% Calculates the mass matrix necessary for optimizing the gaits of +% inertia-dominated swimmers. +% +% Inputs: +% geometry: Structure containing information about geometry of chain. See +% Inertial_connection_discrete for information about required fields. +% +% physics: Structure containing information about the system's physics. +% See Inertial_connection_discrete for information about required +% fields. +% +% jointangles: Array containing values of shape positions at which the +% pulled-back partial mass matrix should be evaluated +% +% Outputs for system with k shape variables: +% M_alpha: Pulled-back mass matrix, in terms of shape variables only. Of +% size (k by k). + +% Obtain the general mass matrix and local connection +[A, h, J, J_full, omega, M_full, local_inertias] = Inertial_connection_discrete(geometry,physics,jointangles); +% Pull back the mass matrix to be in terms of the shape variables only +M_alpha = [-A' eye(size(A,2))]*M_full*[-A; eye(size(A,2))]; +end \ No newline at end of file diff --git a/InertialPurgatory/mass_pull_back.m b/InertialPurgatory/mass_pull_back.m new file mode 100644 index 00000000..f66ca106 --- /dev/null +++ b/InertialPurgatory/mass_pull_back.m @@ -0,0 +1,23 @@ +function M_alpha = mass_pull_back(M_full,A) + +% if ~isa(A,'cell') +% A_calc = {A}; +% convertedA = 0; +% else +% A_calc = celltensorconvert(A); +% convertedA = 1; +% end + +% M_alpha = cell(size(A_calc)); + +% for i = 1:numel(A_calc) +% % Pull M_full back to be in terms of the shape variables +% M_alpha{i} = [-A_calc{i}' eye(size(A_calc{1},2))]*M_full*[-A_calc{i}; eye(size(A_calc{1},2))]; +% end +% +% if convertedA +% M_alpha = celltensorconvert(M_alpha); +% end + M_alpha = [-A' eye(size(A,2))]*M_full*[-A; eye(size(A,2))]; + +end \ No newline at end of file diff --git a/InertialPurgatory/metric_derivative_calc.m b/InertialPurgatory/metric_derivative_calc.m new file mode 100644 index 00000000..aa463acc --- /dev/null +++ b/InertialPurgatory/metric_derivative_calc.m @@ -0,0 +1,136 @@ +function [dM,Gamma2] = metric_derivative_calc(r,i,Mp_raw,alpha_field,shv,M)%-----Removed method + +%------------------------ Metric Derivation ------------------------------% +gi = inv(M); +%-----Pretty sure Hossein said we'll only use the numerical case-----% +% switch method +% +% case 'Analytical' % If you have your metric equation you can use this option +% +% dM{i,1} = dMx_func; +% dM{i,2} = dMy_func(r(i,1),r(i,2),1,1,1,1); +% % dM{i,3} = dMz_func(r(i,1),r(i,2),r(i,3),1,1,1,1); +% +% % dM_newis1x = dMx_func; +% % dM_newis1y = dMy_func(r(i-1,1),r(i-1,2),r(i-1,3),1,1,1,1); +% % dMis1z = dMz_func(r(i-1,1),r(i-1,2),r(i-1,3),1,1,1,1); +% % +% % dM_newip1x = dMx_func; +% % dM_newip1y = dMy_func(r(i+1,1),r(i+1,2),r(i+1,3),1,1,1,1); +% % dMip1z = dMz_func(r(i+1,1),r(i+1,2),r(i+1,3),1,1,1,1); + + +% case 'Numerical' % When the metric is given for all the configuration you can use this option + % Find the points around the current point + + for j = 1:shv + h_L_original = []; + h_R_original = []; + + L = r(i,j)-0.001; + R = r(i,j)+0.001; + + for k = 1:shv + if k ~=j + p1 = r(i,k); + p2 = r(i,k); + else + p1 = L; + p2 = R; + end + h_L_original{k} = [p1]; + h_R_original{k} = [p2]; + end + + M_L = Granular_metric_calc2(h_L_original,Mp_raw,alpha_field{:}); + M_R = Granular_metric_calc2(h_R_original,Mp_raw,alpha_field{:}); + + dg{j} = M_R-M_L; + + dU(j) = (h_R_original{j} - h_L_original{j}); + + dM{i,j} = dg{j}/dU(j); + + if isnan(dM{i,j}) + dM{i,j} = zeros(shv,shv); + end + + end + + n = size(r,2); + Gamma2 = zeros (n, n, n); + for l = 1:n % delta spans the variable labels over which symbols will be calculated + for ii = 1:n + for j = 1:n + for k = 1:n + Gamma2 (ii, j, l) = Gamma2 (ii, j, l) + ... + 0.5 * gi (l, k) * ... + ((dg{ii}(j,k))/dU(ii) + ... + (dg{j}(ii,k))/dU(j) - ... + (dg{k}(ii,j))/dU(k)); + end + end + end + Gamma2 (:, :, l); % display for verification + end + + + + +% h_L_original = [r(i,1)+0.001 r(i,2)]; +% h_R_original = [r(i,1)-0.001 r(i,2)]; +% h_U_original = [r(i,1) r(i,2)+0.001 ]; +% h_D_original = [r(i,1) r(i,2)-0.001]; +% % h_F_original = [r(i,1) r(i,2) r(i,3)+0.001]; +% % h_B_original = [r(i,1) r(i,2) r(i,3)-0.001]; +% +% +% % Find the metric coorespoding to the orginal coordinate +% M_L_temp = Granular_metric_calc(h_L_original(1),h_L_original(2),Mp_raw,a,b); +% M_R_temp = Granular_metric_calc(h_R_original(1),h_R_original(2),Mp_raw,a,b); +% M_U_temp = Granular_metric_calc(h_U_original(1),h_U_original(2),Mp_raw,a,b); +% M_D_temp = Granular_metric_calc(h_D_original(1),h_D_original(2),Mp_raw,a,b); +% % M_F_temp = Granular_metric_calc3(h_F_original(1),h_F_original(2),h_F_original(3),Mp_raw,a,b,c); +% % M_B_temp = Granular_metric_calc3(h_B_original(1),h_B_original(2),h_B_original(3),Mp_raw,a,b,c); +% +% +% % Transform the metric to the local coordinate +% M_L = M_L_temp; +% M_R = M_R_temp; +% M_U = M_U_temp; +% M_D = M_D_temp; +% % M_F = M_F_temp; +% % M_B = M_B_temp; +% +% +% % Find the difference of metrics in x and y direction +% dg{1} = M_R-M_L; +% dg{2} = M_U-M_D; +% % dg{3} = M_F-M_B; +% +% % Find the x and y distance in local coordinate +% dU(1) = (h_R_original(1) - h_L_original(1)); +% dU(2) = (h_U_original(2) - h_D_original(2)); +% % dU(3) = (h_F_original(3) - h_B_original(3)); +% +% +% dM{i,1} = dg{1}/dU(1); +% +% dM{i,2} = dg{2}/dU(2); +% +% % dM{i,3} = dg{3}/dU(3); +% +% if isnan(dM{i,1}) +% dM{i,1} = zeros(3,3); +% end +% +% if isnan(dM{i,2}) +% dM{i,2} = zeros(3,3); +% end +% +% % if isnan(dM{i,3}) +% % dM{i,3} = zeros(3,3); +% % end + + +end diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator3.m b/InertialPurgatory/optimalgaitgenerator3.m similarity index 94% rename from ProgramFiles/GaitOptimization/optimalgaitgenerator3.m rename to InertialPurgatory/optimalgaitgenerator3.m index 0776e5b4..33e92626 100644 --- a/ProgramFiles/GaitOptimization/optimalgaitgenerator3.m +++ b/InertialPurgatory/optimalgaitgenerator3.m @@ -1,4 +1,4 @@ -function y=optimalgaitgenerator3(s,dimension,npoints,a1,a2,a3,lb,ub) +function y=optimalgaitgenerator3(s,dimension,npoints,a1,a2,a3,lb,ub,direction,handles) %%%%%%%%%%%%%% % This function takes an input gait and runs fmincon to find the neareast locally % optimal gait @@ -14,6 +14,7 @@ % a2: Values of the points forming the gait along the second shape dimension % lb: Lower bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired % ub: Upper bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired +% direction: Direction to optimize travel: 1=x,2=y,3=theta % % % Outputs: @@ -71,7 +72,7 @@ end options = optimoptions('fmincon','SpecifyObjectiveGradient',true,'Display','iter','Algorithm','sqp','SpecifyObjectiveGradient',true,'CheckGradients',false,'FiniteDifferenceType','central','MaxIter',4000,'MaxFunEvals',20000,'TolCon',10^-2,'OutputFcn', @outfun); - [yf fval exitflag output]=fmincon(@(y) solvedifffmincon(y,s,n,dimension,lb,ub),y0,A,b,Aeq,beq,lb1,ub1,@(y) nonlcon(y,s,n,dimension,lb,ub),options); + [yf fval exitflag output]=fmincon(@(y) solvedifffmincon(y,s,n,dimension,direction,lb,ub),y0,A,b,Aeq,beq,lb1,ub1,@(y) nonlcon(y,s,n,dimension,lb,ub),options); %% Getting point position values from the result of fmincon @@ -106,7 +107,7 @@ end -function [f,g]=solvedifffmincon(y,s,n,dimension,lb,ub) +function [f,g]=solvedifffmincon(y,s,n,dimension,direction,lb,ub) %%%%%%%%%%%%% % This function calculates efficiency (or displacement, if % that is the objective function) and its gradient with respect to the coefficients obtained @@ -161,7 +162,7 @@ [net_disp_orig, net_disp_opt, cost] = evaluate_displacement_and_cost1(s,p,[0, g],'interpolated','ODE'); % Call to the function that obtains displacement, cost and efficiency of a gait -lineint=net_disp_opt(1); % displacement produced in the x-direction produced on executing the gait measured in the optimal coordinates +lineint=net_disp_opt(direction); % displacement produced in the chosen direction produced on executing the gait measured in the optimal coordinates totalstroke=cost; % Cost of executing the gait ones % If efficiency is negative, reversing the order of points so that @@ -205,7 +206,7 @@ for j=1:dimension*(dimension-1)/2 - ccf(:,j)=interpn(interpstateccf{:},s.DA_optimized{1,j},y(:,1),y(:,2),y(:,3),'spline'); + ccf(:,j)=interpn(interpstateccf{:},s.DA_optimized{directions,j},y(:,1),y(:,2),y(:,3),'spline'); end @@ -621,7 +622,7 @@ end -function stop=outfun(y,optimValues,state) +function stop=outfun(y,optimValues,state,handles) %%%%%%%%% % %This function plots the current state of the gait on the sysplotter GUI @@ -631,36 +632,39 @@ n=100; dimension=length(y(1,:)); -% The if else statement below deletes gaits 2 iterations after they have been plotted -if optimValues.iteration>2 - children=get(gca,'children'); - delete(children(2)); -else -end +for thisAxes = [1:numel(handles.plot_thumbnails.Children)] + + axes(handles.plot_thumbnails.Children(thisAxes)); + % The if else statement below deletes gaits 2 iterations after they have been plotted + if optimValues.iteration>2 + children=get(gca,'children'); + delete(children(2)); + else + end -% The if else statement below fades the gait plotted during the previous iteration -if optimValues.iteration>1 - children=get(gca,'children'); - children(1).Color=[0.5 0.5 0.5]; - children(1).LineWidth=4; -else -end -% -% % The if else statement below plots the gait after every iteration - if optimValues.iteration>0 - for i=1:1:n+1 - for j=1:dimension - y1(i,j)=y(1,j)+y(2,j)*cos(i*y(end,j))+y(3,j)*sin(i*y(end,j))+y(4,j)*cos(2*i*y(end,j))+... - +y(5,j)*sin(2*i*y(end,j))+y(6,j)*cos(3*i*y(end,j))+y(7,j)*sin(3*i*y(end,j))+... - +y(8,j)*cos(4*i*y(end,j))+y(9,j)*sin(4*i*y(end,j));%+y(10,j)*cos(5*i*y(end,j))+y(11,j)*sin(5*i*y(end,j));%+y(12,j)*cos(6*i*y(end,j))+y(13,j)*sin(6*i*y(end,j)); - end + % The if else statement below fades the gait plotted during the previous iteration + if optimValues.iteration>1 + children=get(gca,'children'); + children(1).Color=[0.5 0.5 0.5]; + children(1).LineWidth=4; + else end - hold on - handle1=plot3(y1(:,1),y1(:,2),y1(:,3),'k','linewidth',3); -else + % + % % The if else statement below plots the gait after every iteration + if optimValues.iteration>0 + for i=1:1:n+1 + for j=1:dimension + y1(i,j)=y(1,j)+y(2,j)*cos(i*y(end,j))+y(3,j)*sin(i*y(end,j))+y(4,j)*cos(2*i*y(end,j))+... + +y(5,j)*sin(2*i*y(end,j))+y(6,j)*cos(3*i*y(end,j))+y(7,j)*sin(3*i*y(end,j))+... + +y(8,j)*cos(4*i*y(end,j))+y(9,j)*sin(4*i*y(end,j));%+y(10,j)*cos(5*i*y(end,j))+y(11,j)*sin(5*i*y(end,j));%+y(12,j)*cos(6*i*y(end,j))+y(13,j)*sin(6*i*y(end,j)); + end + end + hold on + handle1=plot3(y1(:,1),y1(:,2),y1(:,3),'k','linewidth',3); + else + end end - pause(0.1) stop=false; end diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator4.m b/InertialPurgatory/optimalgaitgenerator4.m similarity index 94% rename from ProgramFiles/GaitOptimization/optimalgaitgenerator4.m rename to InertialPurgatory/optimalgaitgenerator4.m index 6bee27cb..1e5d3745 100644 --- a/ProgramFiles/GaitOptimization/optimalgaitgenerator4.m +++ b/InertialPurgatory/optimalgaitgenerator4.m @@ -1,4 +1,4 @@ -function y=optimalgaitgenerator4(s,dimension,npoints,a1,a2,a3,a4,lb,ub) +function y=optimalgaitgenerator4(s,dimension,npoints,a1,a2,a3,a4,lb,ub,direction,handles) %%%%%%%%%%%%%% % This function takes an input gait and runs fmincon to find the neareast locally % optimal gait @@ -14,6 +14,7 @@ % a2: Values of the points forming the gait along the second shape dimension % lb: Lower bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired % ub: Upper bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired +% direction: Direction to optimize travel: 1=x,2=y,3=theta % % % Outputs: @@ -66,7 +67,7 @@ end options = optimoptions('fmincon','SpecifyObjectiveGradient',true,'Display','iter','Algorithm','sqp','SpecifyObjectiveGradient',true,'CheckGradients',false,'FiniteDifferenceType','central','MaxIter',4000,'MaxFunEvals',20000,'TolCon',10^-2,'OutputFcn', @outfun); - [yf fval exitflag output]=fmincon(@(y) solvedifffmincon(y,s,n,dimension,lb,ub),y0,A,b,Aeq,beq,lb1,ub1,@(y) nonlcon(y,s,n,dimension,lb,ub),options); + [yf fval exitflag output]=fmincon(@(y) solvedifffmincon(y,s,n,dimension,direction,lb,ub),y0,A,b,Aeq,beq,lb1,ub1,@(y) nonlcon(y,s,n,dimension,lb,ub),options); %% Getting point position values from the result of fmincon @@ -156,7 +157,7 @@ [net_disp_orig, net_disp_opt, cost] = evaluate_displacement_and_cost1(s,p,[0, g],'interpolated','ODE'); % Call to the function that obtains displacement, cost and efficiency of a gait -lineint=net_disp_opt(1); % displacement produced in the x-direction produced on executing the gait measured in the optimal coordinates +lineint=net_disp_opt(direction); % displacement produced in the chosen direction produced on executing the gait measured in the optimal coordinates totalstroke=cost; % Cost of executing the gait ones % If efficiency is negative, reversing the order of points so that @@ -200,7 +201,7 @@ for j=1:dimension*(dimension-1)/2 - ccf(:,j)=interpn(interpstateccf{:},s.DA_optimized{1,j},y(:,1),y(:,2),y(:,3),y(:,4),'spline'); + ccf(:,j)=interpn(interpstateccf{:},s.DA_optimized{direction,j},y(:,1),y(:,2),y(:,3),y(:,4),'spline'); end @@ -616,7 +617,7 @@ end -function stop=outfun(y,optimValues,state) +function stop=outfun(y,optimValues,state,handles) %%%%%%%%% % %This function plots the current state of the gait on the sysplotter GUI @@ -626,33 +627,37 @@ n=100; dimension=length(y(1,:)); -% The if else statement below deletes gaits 2 iterations after they have been plotted -if optimValues.iteration>2 - children=get(gca,'children'); - delete(children(2)); -else -end +for thisAxes = [1:numel(handles.plot_thumbnails.Children)] + + axes(handles.plot_thumbnails.Children(thisAxes)); + % The if else statement below deletes gaits 2 iterations after they have been plotted + if optimValues.iteration>2 + children=get(gca,'children'); + delete(children(2)); + else + end -% The if else statement below fades the gait plotted during the previous iteration -if optimValues.iteration>1 - children=get(gca,'children'); - children(1).Color=[0.5 0.5 0.5]; - children(1).LineWidth=4; -else -end -% -% % The if else statement below plots the gait after every iteration - if optimValues.iteration>0 - for i=1:1:n+1 - for j=1:dimension - y1(i,j)=y(1,j)+y(2,j)*cos(i*y(end,j))+y(3,j)*sin(i*y(end,j))+y(4,j)*cos(2*i*y(end,j))+... - +y(5,j)*sin(2*i*y(end,j))+y(6,j)*cos(3*i*y(end,j))+y(7,j)*sin(3*i*y(end,j))+... - +y(8,j)*cos(4*i*y(end,j))+y(9,j)*sin(4*i*y(end,j));%+y(10,j)*cos(5*i*y(end,j))+y(11,j)*sin(5*i*y(end,j));%+y(12,j)*cos(6*i*y(end,j))+y(13,j)*sin(6*i*y(end,j)); - end + % The if else statement below fades the gait plotted during the previous iteration + if optimValues.iteration>1 + children=get(gca,'children'); + children(1).Color=[0.5 0.5 0.5]; + children(1).LineWidth=4; + else + end + % + % % The if else statement below plots the gait after every iteration + if optimValues.iteration>0 + for i=1:1:n+1 + for j=1:dimension + y1(i,j)=y(1,j)+y(2,j)*cos(i*y(end,j))+y(3,j)*sin(i*y(end,j))+y(4,j)*cos(2*i*y(end,j))+... + +y(5,j)*sin(2*i*y(end,j))+y(6,j)*cos(3*i*y(end,j))+y(7,j)*sin(3*i*y(end,j))+... + +y(8,j)*cos(4*i*y(end,j))+y(9,j)*sin(4*i*y(end,j));%+y(10,j)*cos(5*i*y(end,j))+y(11,j)*sin(5*i*y(end,j));%+y(12,j)*cos(6*i*y(end,j))+y(13,j)*sin(6*i*y(end,j)); + end + end + hold on + handle1=plot3(y1(:,1),y1(:,2),y1(:,3),'k','linewidth',3); + else end - hold on - handle1=plot3(y1(:,1),y1(:,2),y1(:,3),'k','linewidth',3); -else end diff --git a/InertialPurgatory/partial_mass_matrix.m b/InertialPurgatory/partial_mass_matrix.m new file mode 100644 index 00000000..467f832b --- /dev/null +++ b/InertialPurgatory/partial_mass_matrix.m @@ -0,0 +1,69 @@ +function dMdq = partial_mass_matrix(J_full,dJdq,local_inertias) +% Calculates the partial of the full mass matrix M with respect to the +% configuration variables q = [g_circ r]', where g_circ is the system's +% global coordinate configuration and r is the local shape-space +% configuration. +% +% Inputs for a system with m links and q of size (n by 1): +% +% J_full: Full Jacobian matrix for the system, including entries for all +% configuration variables q; (m by 1) cell array where each cell contains +% a (3 by n) matrix representing the Jacobian of link m. +% +% dJdq: Partial of full Jacobian with respect to the configuration +% variables q; (m by 1) cell array where each entry corresponds to +% the partial derivative of the Jacobian for link m, which is another +% cell array of size (n by n) where each entry (j,k) corresponds to a +% three-element vector that represents the partial of Jacobian column +% j with respect to configuration variable k. +% +% local_inertias: The inertia tensor of the link as measured in its fixed +% coordinate frame, which includes the added mass from the surrounding +% fluid. Cell array where the ith cell corresponds to the inertia +% matrix for link i. +% +% Output for a system with q of size (n by 1): +% +% dMdq: Partial of full mass matrix with respect to the configuration +% variables q; (n by 1) cell array where the ith cell contains an (n by n) +% matrix corresponding to dMdq_i, or the partial of the mass matrix with +% respect to the ith configuration variable in q + +% Number of links is equal to the number of Jacobian matrices in J_full, as +% it contains one matrix for each link +num_links = length(J_full); +% Number of configuration variables q +num_q = size(dJdq{1},1); + +% We need a partial mass matrix for each configuration variable in q, so +% create a temporary prototype with the appropriately sized matrix to enter +% into preallocated space +dMtemp = zeros(num_q,num_q); +% If we're working with symbolic variables, then we need to explicitly make +% the array symbolic, because matlab tries to cast items being inserted +% into an array into the array class, rather than converting the array to +% accomodate the class of the items being inserted +if or(isa(J_full{1},'sym'),isa(local_inertias{1},'sym')) + dMtemp = sym(dMtemp); + cell2func = @cell2sym; +else + cell2func = @cell2mat; +end +% Now fill the preallocated memory with the temporary prototypes +dMdq = cell(num_q,1); dMdq(1:end) = {dMtemp}; + +% The mass matrix for a mobile system is given by M = J'*mu*J, where J is +% the full Jacobian and mu is the local inertia tensor, summed across each +% of the links. We can obtain dMdq simply by performing the chain rule to +% obtain dMdq = dJdq'*mu*J + J'*mu*dJdq, summed across all of the links, +% and performed for each configuration variable q, as done below. +for q = 1:num_q + for link = 1:num_links + dJtemp = cell2func(dJdq{link}(:,q)'); + dMdq{q} = dMdq{q} + dJtemp' * local_inertias{link} * J_full{link} ... + + J_full{link}' * local_inertias{link} * dJtemp; + end + if isa(dMtemp,'sym') + dMdq{q} = simplify(dMdq{q},'Steps',10); + end +end \ No newline at end of file diff --git a/InertialPurgatory/shape_partial_mass.m b/InertialPurgatory/shape_partial_mass.m new file mode 100644 index 00000000..64a1d3cb --- /dev/null +++ b/InertialPurgatory/shape_partial_mass.m @@ -0,0 +1,124 @@ +function dmdalpha = shape_partial_mass(M_full,J_full,local_inertias,jointangles,A_eval,A_grid) +% Calculates the partial of the pulled-back mass matrix with respect to the +% shape variables alpha_i. This is done by calculating the partial of the +% full mass matrix with respect to the full set of configuration variables +% q = [g_circ, r]', where g_circ represents the world configuration and r +% represents the local shape variables. +% +% Inputs: +% geometry: Structure containing information about geometry of chain. See +% Inertial_connection_discrete for information about required fields. +% +% physics: Structure containing information about the system's physics. +% See Inertial_connection_discrete for information about required +% fields. +% +% jointangles: Array containing values of shape positions at which the +% pulled-back partial mass matrix should be evaluated +% +% A_eval: Cell array containing the local connection evaluated at +% discrete points determined by the values in A_grid. As of 1/21/20, +% this is given by s.vecfield.eval.content.A_num +% +% A_grid: Points at which the local connection in A_eval was evaluated. +% As of 1/21/20, this is given by s.grid.eval +% +% Output for a system with m links and k joints: +% +% dmdalpha: Pulled-back partial of the mass matrix, in terms of the shape +% variables only. (k by 1) cell array where the ith entry corresponds +% to the partial of the pulled-back mass matrix with respect to the +% ith shape variable. + +% Get the full jacobian and mass matrices as well as the links' local +% inertia tensors + +% Obtain the derivative of the Jacobian with respect to the configuration +dJdq = mobile_jacobian_derivative(J_full); +% Get the partial of the mass matrix with respect to the configuration +% variables +dMdq = partial_mass_matrix(J_full,dJdq,local_inertias); +% Pull back the partial of the mass matrix to be in terms of the shape +% variables only +dmdalpha = pull_back_partial_mass(M_full,dMdq,A_eval,A_grid,jointangles); +end + + +function dmdalpha = pull_back_partial_mass(M,dMdq,A,A_grid,jointangles) +% Pulls back the full partial mass matrix dMdq to be in terms of the shape +% variables only. +% +% Inputs for a system with m links and q of size (n by 1): +% M: Full mass matrix of size (n by n) +% +% dMdq: Partial of full mass matrix with respect to the configuration +% variables q; (n by 1) cell array where the ith cell contains an (n by n) +% matrix corresponding to dMdq_i, or the partial of the mass matrix with +% respect to the ith configuration variable in q +% +% A_eval: Cell array containing the local connection evaluated at +% discrete points determined by the values in A_grid. As of 1/21/20, +% this is given by s.vecfield.eval.content.A_num +% +% A_grid: Points at which the local connection in A_eval was evaluated. +% As of 1/21/20, this is given by s.grid.eval +% +% jointangles: Array containing values of shape positions at which the +% pulled-back partial mass matrix should be evaluated +% +% Output for a system with m links and k joints: +% +% dmdalpha: Pulled-back partial of the mass matrix, in terms of the shape +% variables only. (k by 1) cell array where the ith entry corresponds +% to the partial of the pulled-back mass matrix with respect to the +% ith shape variable. + + angle_cells = num2cell(jointangles); + num_joints = size(A,2); + dMdalpha = dMdq(4:end); % Get the partials for the joints, skipping the first three terms + % (Since these are the g-circ terms) + % Get the local connection at the given joint angles + ja = num2cell(jointangles); + A_interp = cellfun(@(C) interpn(A_grid{:},C,ja{:},'spline'),A); + % Get dA/dalpha at the given jointangles + dA_interp = cell(num_joints,1); + % TODO: Need some way to make this number-of-joints-agnostic. Note + % that results are given as [dA/dalpha2 dA/dalpha1] because the + % first term is difference in horizontal direction and second is + % difference in vertical direction; the grid has alpha1 vary by + % column and alpha2 vary by row. + + A_grid_mesh = A_grid([2,1,3:numel(A_grid)]); + + grid_baseline = cell(size(A_grid_mesh)); + callout_template = num2cell(ones(size(grid_baseline))); + for idx = 1:numel(grid_baseline) + callout = callout_template; + callout{idx} = ':'; + grid_baseline{idx} = squeeze(A_grid{idx}(callout{:})); + end + + dA = repmat({cell(size(A))},1,numel(jointangles)); + [dA{[2,1,3:numel(A_grid)]}] = cellfun(@(C) gradient(C,grid_baseline{[2,1,3:numel(A_grid)]}),A,'UniformOutput',false); + + for idx = 1:numel(dA) + dA_interp{idx} = cellfun(@(C) interpn(A_grid{:},C,angle_cells{:},'spline'),dA{idx}); + end +% dA_interp{2} = cellfun(@(C) interpn(A_grid{:},C,angle_cells{:},'spline'),dA2); + +% [dA2, dA1] = cellfun(@(C) gradient(C,A_grid{2}(1,:),A_grid{1}(:,1)),A,'UniformOutput',false); +% dA_interp{1} = cellfun(@(C) interpn(A_grid{:},C,angle_cells{:},'spline'),dA1); +% dA_interp{2} = cellfun(@(C) interpn(A_grid{:},C,angle_cells{:},'spline'),dA2); + + dmdalpha = cell(num_joints,1); + % The pulled-back mass matrix m_alpha is given by the matrix product: + % m_alpha = [-A' I]*M*[A; I] + % So the partial of the pulled-back mass matrix is found simply by + % performing the chain rule on this quantity, as is done below. + for i = 1:numel(dmdalpha) + + dmdalpha{i} = [-dA_interp{i}', zeros(num_joints)]*M*[-A_interp; eye(num_joints)] ... + + [-A_interp', eye(num_joints)]*dMdalpha{i}*[-A_interp; eye(num_joints)] ... + + [-A_interp', eye(num_joints)]*M*[-dA_interp{i}; zeros(num_joints)]; + end +end \ No newline at end of file diff --git a/InertialPurgatory/torqueJacobian.m b/InertialPurgatory/torqueJacobian.m new file mode 100644 index 00000000..ce2c60e9 --- /dev/null +++ b/InertialPurgatory/torqueJacobian.m @@ -0,0 +1,95 @@ +function [newJ,jacob] = torqueJacobian(M,Metric,r,i,T,sign_kappa) + + dr = (cdiff( r' ))'/T; + + if dr(i,1) == 0 + dr(i,:) = [1 0]; + + if i == 1 + M1 = (M{i}+M{i+1})/2; + elseif i == size(r,1) + M1 = (M{i}+M{i-1})/2; + else + M1 = M{i}; + end + else + M1 = (2*M{i}+M{i-1}+M{i+1})/4; + end + + % Make the jacobian + norm_u1 = sqrt([dr(i,1) dr(i,2)]*M1*[dr(i,1) dr(i,2)]'); + u1 = [dr(i,1) dr(i,2) 0]/norm_u1; + + % Find the normal vector to the u1 + u2_temp = [-u1(2)*sign_kappa u1(1)*sign_kappa 0];%cross(u1,z1);%/norm(cross(u1,z1)); + + % Find projection of u2 over u1 + proj_u2 = ((u2_temp(1:2)*M1*u1(1:2)')/(u1(1:2)*M1*u1(1:2)'))*u1(1:2); + + u2 = u2_temp(1:2) - proj_u2; + u2 = u2/sqrt(u2*M1*u2'); + + J = inv([u1(1:2)' u2']); + newJ = J*J'; + jacob = J; + + % Find the derivative of metrix + shv = 2; + +% for j = 1:2 +% h_L_original = []; +% h_R_original = []; +% +% L = r(i,j)-0.001; +% R = r(i,j)+0.001; +% +% for k = 1:shv +% if k ~=j +% p1 = r(i,k); +% p2 = r(i,k); +% else +% p1 = L; +% p2 = R; +% end +% h_L_original{k} = [p1]; +% h_R_original{k} = [p2]; +% end +% +% +% var = [cell2mat(h_L_original); cell2mat(h_R_original)]; +% +% for ite = 1:shv +% +% M2 = Metric(var(ite,1),var(ite,2)); +% % Make the jacobian +% norm_u1 = sqrt(var(ite,:)*M2*var(ite,:)'); +% u1 = [var(ite,:) 0]/norm_u1; +% +% % Find the normal vector to the u1 +% u2_temp = [-u1(2) u1(1) 0];%cross(u1,z1);%/norm(cross(u1,z1)); +% +% % Find projection of u2 over u1 +% proj_u2 = ((u2_temp(1:2)*M2*u1(1:2)')/(u1(1:2)*M2*u1(1:2)'))*u1(1:2); +% +% u2 = u2_temp(1:2) - proj_u2; +% u2 = u2/sqrt(u2*M2*u2'); +% +% J = inv([u1(1:2)' u2']); +% newJ_dir{ite} = J*J'; +% +% end +% +% dg{j} = newJ_dir{2}-newJ_dir{1}; +% +% dU(j) = (h_R_original{j} - h_L_original{j}); +% +% dnewJ{1,j} = dg{j}/dU(j); +% +% if isnan(dnewJ{1,j}) +% dnewJ{1,j} = zeros(shv,shv); +% end +% +% end + + + \ No newline at end of file diff --git a/ProgramFiles/Animation/animate_locomotor.m b/ProgramFiles/Animation/animate_locomotor.m index fdaffb79..282b66ca 100644 --- a/ProgramFiles/Animation/animate_locomotor.m +++ b/ProgramFiles/Animation/animate_locomotor.m @@ -94,7 +94,8 @@ function animate_locomotor(export,info_needed) info_needed); % Extract the position and shape data - load(fullfile(data_source,['sysf_' system_name '__shchf_' gait_name])); + gaitfile = fullfile(data_source,['sysf_' system_name '__shchf_' gait_name]); + load(gaitfile); shaperaw = p.phi_locus_full{1}.shape; if strcmp(info_needed.Coordinates,'minperturbation_coords') @@ -199,6 +200,7 @@ function animate_locomotor(export,info_needed) 'stretch',0,... 'stretchpath', 'null',... 'CCFtype','DA',... + 'stretch_name','none',... 'style','contour'); % Identify the data file to draw the plot information from @@ -278,10 +280,19 @@ function animate_locomotor(export,info_needed) %%%% % Place the locomotor at the position - % If a drawing baseframe has been specified, replace the original + % If a drawing baseframe has been specified, append the original % system baseframe with the one that has been specified - if isfield(frame_info{irobot},'drawing_baseframe') - frame_info{irobot}.s.geometry.baseframe = frame_info{irobot}.drawing_baseframe; + if isfield(frame_info{irobot},'drawing_baseframe') && ~isfield(frame_info{irobot},'drawing_baseframe_inserted') + if ~iscell(frame_info{irobot}.s.geometry.baseframe) + frame_info{irobot}.s.geometry.baseframe ... + = {frame_info{irobot}.s.geometry.baseframe}; + end + if ~iscell(frame_info{irobot}.s.geometry.baseframe) + frame_info{irobot}.drawing_baseframe ... + = {frame_info{irobot}.drawing_baseframe}; + end + frame_info{irobot}.s.geometry.baseframe = [frame_info{irobot}.s.geometry.baseframe frame_info{irobot}.drawing_baseframe]; + frame_info{irobot}.drawing_baseframe_inserted = 1; end % Use the configuration to place the locomotor diff --git a/ProgramFiles/Animation/animate_locomotor_race.m b/ProgramFiles/Animation/animate_locomotor_race.m new file mode 100644 index 00000000..7500d0a4 --- /dev/null +++ b/ProgramFiles/Animation/animate_locomotor_race.m @@ -0,0 +1,543 @@ +function [normalizedPeriod,netDisp] = animate_locomotor_race(export,info_needed,makevideo) + + % Look up the geometry specification for the systems: + for idx_system = 1:numel(info_needed.current_system2) + sysfile = fullfile(info_needed.datapath, ['sysf_', info_needed.current_system2{idx_system}, '_calc.mat']); + load(sysfile,'s') + s.optcostfunction = info_needed.optcostfunction{idx_system}; + s.evalcostfunction = info_needed.evalcostfunction{idx_system}; + info_needed.s{idx_system} = s; + end + + % Declare a directory name file names for the movies + destination_root = fullfile(info_needed.datapath,'Animation','swimrace'); + destination_list = fieldnames(info_needed.Movies); + + if strcmp(info_needed.Coordinates,'minperturbation_coords') + destinationsuffix = 'min_perturb'; + else + destinationsuffix = 'original_coords'; + end + + destination = cellfun(@(x) fullfile(destination_root,[info_needed.moviename '__' x '__' destinationsuffix]),destination_list,'UniformOutput',false); + + % Flag movies that should be generated, don't skip any movies + export_list = cellfun(@(x) info_needed.Movies.(x), destination_list); + skip_list = zeros(size(export_list)); + info_needed.export_list = export_list; + + + + %%%%%%%% + % Create animation elements, and store them in the frame_info structure + frame_info{1} = create_elements(info_needed); %setup function, defined below in file + netDisp = frame_info{1}.disp; + normalizedPeriod = frame_info{1}.normalizedPeriod; + + if makevideo + % Designate animation function + frame_gen_function... + = @execute_gait; % frame function, defined below in file + + % Declare timing + timing.duration = info_needed.Duration; % in seconds + timing.fps = info_needed.Framerate; % create frames for specified frame rate + timing.pacing = @(y) softspace(0,1,y); % Use a soft start and end, using the included softspace function + + + % Pass the system and gait names to the frame_info struct + frame_info{1}.sysname = info_needed.current_system2; + frame_info{1}.pathname = info_needed.current_shch2; + frame_info{1}.s = info_needed.s; + frame_info{1}.drawing_baseframe_inserted = repmat({0},[numel(info_needed.current_system2),1]); + + + % Animate the movie + [frame_info, endframe]... + = sysplotter_animation_race(frame_gen_function,frame_info,timing,destination,export_list,skip_list,0); + + end + +end + +function h = create_elements(info_needed) + + + + h.f = figure(17); % Designate a figure for this animation + clf(h.f) % Clear this figure + + set(h.f,... + 'position',... + [600 1500 800 350],... + 'paperposition',[0 0 10 4.5],... + 'papersize',[10 4.5],... + 'Color','w',... + 'renderer','painters'); + +% set(h.f,... +% 'position',[600 1500 800 600],... +% 'paperposition',[0 0 8 6],... +% 'papersize',[8 6],... +% 'Color','w',... +% 'renderer','painters'); + + h.ax = axes('Parent',h.f); % Create axes for the plot + axis(h.ax,'equal','off'); % Make the plot isometric and make the axes invisible + set(h.ax,... + 'XLim',[-1,4]*.7/.45*info_needed.s{1}.geometry.length,... % Axes scaled to system scale + 'YLim',[-2.5,2.5]*.7*info_needed.s{1}.geometry.length); + set(h.ax,'Position',[0 0 1 1]) % Make the axes fill the whole window + + data_source = info_needed.datapath; + system_name = info_needed.current_system2; + seed_name = info_needed.current_shch2; + optcostfunction = info_needed.optcostfunction; + evalcostfunction = info_needed.evalcostfunction; + + gait_name = cell(size(seed_name)); + + for idx_system = 1:numel(system_name) + paramfilehash = hash(['opt_',system_name{idx_system},'_',seed_name{idx_system},'_',optcostfunction{idx_system},'_' 'X' 'eff'],'md5'); + gait_name{idx_system} = ['opt_',paramfilehash]; + end + + + for idx_system = 1:numel(info_needed.current_system2) + + %Create the locomotor + info_needed2 = info_needed; + info_needed2.s = info_needed.s{idx_system}; + h.robot{idx_system} = create_locomotor(... + h.ax,... + data_source,... + system_name{idx_system},... + info_needed2); + + %%%%%%%% + % Create the tracer objects + load('sysplotter_config','Colorset') + h.tld{idx_system} = line('Parent',h.ax,'XData',[],'YData',[],'ZData',[],'LineWidth',5,'Color',Colorset.spot,'Marker','o','MarkerFaceColor',Colorset.spot,'MarkerSize',5,'LineStyle','none'); + h.tl{idx_system} = line('Parent',h.ax,'XData',[],'YData',[],'ZData',[],'LineWidth',5,'Color',Colorset.spot); %Tracer line for translation + + % Extract the position, shape, and cost data + gaitfile = fullfile(data_source,['sysf_' system_name{idx_system} '__shchf_' gait_name{idx_system}]); + load(gaitfile); + shaperaw{idx_system} = p.phi_locus_full{1}.shape; + + if strcmp(info_needed.Coordinates,'minperturbation_coords') + posraw{idx_system} = p.G_locus_full{1}.G_opt; + h.drawing_baseframe{idx_system} = info_needed.current_system2{idx_system}; + else + posraw{idx_system} = p.G_locus_full{1}.G; + end + + h.normalizedPeriod{idx_system} = getNormalizedPeriod(s,p,info_needed.s{idx_system}.evalcostfunction); + h.disp{idx_system} = norm(p.G_locus_full{1}.G(end,1:2)); + + h.speed{idx_system} = h.disp{idx_system}/h.normalizedPeriod{idx_system}; + + + end + + %speedraw = + + [maxspeed,fastestsystem] = max(cell2mat(h.speed)); + + + + + + % Specify the number of times to repeat the cycle, and the number of + % negative cycle-displacements to start from + maxdisp = 3; + n_gaits_max = ceil(maxdisp/h.disp{fastestsystem}); + t_max = h.normalizedPeriod{fastestsystem}*n_gaits_max; + start_pos = 0; + + % get full configuration history + [h.shapedata, h.posdata] = gait_concatenator(shaperaw,posraw,start_pos,t_max,h.normalizedPeriod); + + +end + + + +%%%%%%%%%%%%%%%%%% +% Frame content to draw during the gait +function frame_info = execute_gait(frame_info,tau) + + % Call out the frame_info pieces that contain robot verse shapespace + % data + irobot = 1; + + + %%%%%% + % Get the system position and shape at fractional time tau + for idx_system = 1:numel(frame_info{irobot}.shapedata) + + % Generate a core timing vector from zero to 1, with as many entries as + % data points in the kinematic history + timing_base = linspace(0,1,size(frame_info{irobot}.shapedata{idx_system},1)); + + % Interpolate the shape variables at the specified time + config.shape = zeros(size(frame_info{irobot}.shapedata{idx_system},2),1); + for idx = 1:numel(config.shape) + config.shape(idx) = interp1(timing_base,frame_info{irobot}.shapedata{idx_system}(:,idx),tau); + end + + % Interpolate the position variables at the specified time + config.x = interp1(timing_base,frame_info{irobot}.posdata{idx_system}(:,1),tau); + config.y = interp1(timing_base,frame_info{irobot}.posdata{idx_system}(:,2),tau); + config.theta = interp1(timing_base,frame_info{irobot}.posdata{idx_system}(:,3),tau); + + % offset the y values to put the systems next to each other + yoffset = + (idx_system - 1) - (numel(frame_info{irobot}.shapedata)-1)/2; + config.y = config.y + yoffset; + + %%%% + % Place the locomotor at the position + + % If a drawing baseframe has been specified, append the original + % system baseframe with the one that has been specified + if isfield(frame_info{irobot},'drawing_baseframe') && ~frame_info{irobot}.drawing_baseframe_inserted{idx_system} + if ~iscell(frame_info{irobot}.s{idx_system}.geometry.baseframe) + frame_info{irobot}.s{idx_system}.geometry.baseframe ... + = {frame_info{irobot}.s{idx_system}.geometry.baseframe}; + end + if ~iscell(frame_info{irobot}.s{idx_system}.geometry.baseframe) + frame_info{irobot}.drawing_baseframe ... + = {frame_info{irobot}.drawing_baseframe}; + end + frame_info{irobot}.s{idx_system}.geometry.baseframe = [frame_info{irobot}.s{idx_system}.geometry.baseframe frame_info{irobot}.drawing_baseframe{idx_system}]; + frame_info{irobot}.drawing_baseframe_inserted{idx_system} = 1; + end + + % Use the configuration to place the locomotor + frame_info{irobot}.robot{idx_system} = place_locomotor(frame_info{irobot}.robot{idx_system},config,frame_info{irobot}.s{idx_system}); + + % draw the locomotor + frame_info{irobot}.robot{idx_system} = draw_locomotor(frame_info{irobot}.robot{idx_system},0); + + % Draw the tracer dot + set(frame_info{irobot}.tld{idx_system},'XData',[frame_info{irobot}.posdata{idx_system}(1,1) config.x],'YData',[frame_info{irobot}.posdata{idx_system}(1,2)+yoffset config.y],'ZData',[5,5]); + + % Draw the tracer + set(frame_info{irobot}.tl{idx_system},'XData',frame_info{irobot}.posdata{idx_system}(1:floor(tau*end),1),'YData',frame_info{irobot}.posdata{idx_system}(1:floor(tau*end),2)+yoffset,'ZData',5*ones(size(frame_info{irobot}.posdata{idx_system}(1:floor(tau*end),1)))); + + + end + + % Declare a print method (in this case, print 150dpi png files of + frame_info{irobot}.printmethod = @(dest) print(frame_info{irobot}.f,'-r 150','-painters',dest); + + +end + +%%%%%% +function [shapedata, posdata] = gait_concatenator(shaperaw,posraw,start_pos,t_max,normalizedPeriod) + + + + % Store the shape and data to the frameinfo structure + for idx_system = 1:numel(shaperaw) + + f_gaits = t_max/normalizedPeriod{idx_system}(end); + n_gaits = ceil(f_gaits); + lastfrac = 1-(n_gaits-f_gaits); + + + + %%%%%% + % Concatenate the gait displacements together. + + % Get the displacement over one cycle + cyclic_displacement = posraw{idx_system}(end,:); + + % Remove theta component + cyclic_displacement(3) = 0; + + % convert this cyclic displacement into an SE(2) matrix + cyclic_displacement_m = vec_to_mat_SE2(cyclic_displacement); + + % Add a set of ones onto the xy values, so that they can be + % translated/rotated by SE(2) matrices + posraw_xy_augmented = cat(1, posraw{idx_system}(:,1:2)', ones(1,size(posraw{idx_system},1))); + + % Chain n iterations of the gait together, offsetting the start of each + % by the displacement at the end of the previous gait, and avoiding a + % double frame at the transition from one cycle to the next + + % Initialize the position data with an empty matrix + posdata{idx_system} = []; + shapedata{idx_system} = []; + + % Iterate over the n gait cycles + for i = 1:n_gaits + + % Skip first value if not the starting cycle (so that we don't get + % a pause from a doubled frame at the changeover from one cycle to + % the next + if i == 1 + first_index = 1; + else + first_index = 2; + end + + % For the last gait, only include a fraction of the gait + if i == n_gaits + + last_index = ceil(size(posraw{idx_system},1)*lastfrac); + + else + + last_index = size(posraw{idx_system},1); + + end + + shapedata_new = shaperaw{idx_system}(first_index:last_index,:); + shapedata{idx_system} = cat(1,shapedata{idx_system},shapedata_new); + + % Offset the displacements, using low-level implementation of SE(2) + % action + + % xy components (uses the start-pos to offset the starting xy + % position so that it passes through the center of the frame at the + % halfway point of the movie + posdata_xy_augmented = ... + powerm_pade(cyclic_displacement_m,... % Take the displacement over one gait cycle + +start_pos... % Raise it to the negative power of the starting offset (so that we start back from the origin + +(i-1))... % For each subsequent gait, shift if forward by one gait cycle + *posraw_xy_augmented(:,first_index:last_index); % Apply the starting offset for this cycle (calculated in the lines above) to the within-cycle displacements + + % theta component (always starts at zero orientation) + posdata_theta = ... + posraw{idx_system}(first_index:last_index,3)'... % Take the theta values from the within-gait displacement + +cyclic_displacement(3)*(i-1); % Add as many net theta changes as there are previous gait cycles + + % merge xy and theta data (stripping off ones from xy position at the same time) + posdata_new = cat(1,posdata_xy_augmented(1:2,:),posdata_theta); + + % Concatenate the displacements + posdata{idx_system} = cat(2,posdata{idx_system},posdata_new); + + end + + + %return position data to column format + posdata{idx_system} = posdata{idx_system}'; + + end + +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 ischar(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); +% +% %Prep interpolation inputs for velocities function +% shape = zeros(size(s.grid.eval)); +% shape_gait_def_0 = readGait(gait.phi_def,0); +% actual_size = min(numel(shape),numel(shape_gait_def_0)); +% +% samplePoints = {}; +% for dim = 1:actual_size +% samplePoints{dim} = []; +% end +% +% for time = tpoints +% shape_gait_def = readGait(gait.phi_def,time); +% shape(1:actual_size) = shape_gait_def(1:actual_size); +% for dim = 1:numel(shape) +% samplePoints{dim}(end+1) = shape(dim); +% end +% end +% +% indexList = 1:numel(tpoints); +% id = eye(actual_size); +% +% As = cellfun(@(C) -interpn(s.grid.eval{:},C,... +% samplePoints{:},'spline'),s.vecfield.eval.content.Avec,... +% 'UniformOutput',false); +% As = celltensorconvert(As); +% +% switch s.costfunction +% case {'pathlength coord','acceleration coord'} +% %In the case of these two cost functions, we only care about +% %the connection field, and the metric is always identity. +% %dM is passed a filler value +% [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,As{i},id,1),... +% tpoints,indexList,'UniformOutput',false); +% case {'torque','covariant acceleration','power quality'} +% %In the inertial cases, we need to calculate dM, and the metric +% metrics = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... +% samplePoints{:},'spline'),s.metricfield.metric_eval.content.metric,... +% 'UniformOutput',false); +% metrics = celltensorconvert(metrics); +% +% dM_set = {}; +% for dim = 1:actual_size +% dM_holder = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... +% samplePoints{:},'spline'),s.coriolisfield.coriolis_eval.content.dM{dim},... +% 'UniformOutput',false); +% dM_holder = celltensorconvert(dM_holder); +% dM_set{dim} = dM_holder; +% end +% dMs = {}; +% for i = 1:numel(tpoints) +% dMs{i} = {}; +% for dim = 1:actual_size +% dMs{i}{dim} = dM_set{dim}{i}; +% end +% end +% +% [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,... +% As{i},metrics{i},dMs{i}),tpoints,indexList,'UniformOutput',false); +% otherwise +% %Otherwise, we're not doing inertial so don't need dM, but we +% %do care about the metric and connection +% metrics = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... +% samplePoints{:},'spline'),s.metricfield.metric_eval.content.metric,... +% 'UniformOutput',false); +% metrics = celltensorconvert(metrics); +% +% [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,... +% As{i},metrics{i},1),tpoints,indexList,'UniformOutput',false); +% end +% +% %%%%%%% +% % 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 state = readGait(gaitFun,t) +% +% state = gaitFun{1}{1}(t); +% +% end +% +% % Evaluate the body velocity and cost velocity (according to system metric) +% % at a given time +% function [gcirc, dcost] = get_velocities(t,s,gait,ConnectionEval,A,metric,dM) +% +% % Get the shape and shape derivative at the current time +% shape = zeros(size(s.grid.eval)); +% dshape = zeros(size(s.grid.eval)); +% %ddshape = zeros(size(s.grid.eval)); +% +% shape_gait_def = readGait(gait.phi_def,t); +% dshape_gait_def = readGait(gait.dphi_def,t); +% %ddshape_gait_def = readGait(gait.ddphi_def,t); +% +% actual_size = min(numel(shape),numel(shape_gait_def)); +% shape(1:actual_size) = shape_gait_def(1:actual_size); +% dshape(1:actual_size) = dshape_gait_def(1:actual_size); +% %ddshape(1:actual_size) = ddshape_gait_def(1:actual_size); +% +% M_a = metric; +% +% shapelist = num2cell(shape); +% +% % If doing functional eval of system (not recommended) +% % Get the local connection and metric at the current time, in the new coordinates +% if strcmpi(ConnectionEval,'functional') +% +% A = s.A_num(shapelist{:})./s.A_den(shapelist{:}); +% +% switch s.system_type +% case 'drag' +% metric = s.metric(shapelist{:}); +% case 'inertia' +% error('Functional ConnectionEval method not supported for inertia systems!') +% end +% +% end +% +% % Get the body velocity at the current time +% %t; +% gcirc = - A * dshape(:); +% +% switch s.costfunction +% case {'pathlength metric','pathlength coord'} +% dcost = sqrt(dshape(:)'*metric*dshape(:)); +% case 'pathlength metric2' +% dcost = sqrt(dshape(:)'*metric*metric*dshape(:)); +% case 'torque' +% dcost = torque_cost(M_a,dM,shape,dshape,ddshape,metric); +% case 'covariant acceleration' +% dcost = acceleration_cost(M_a,dM,shape,dshape,ddshape,metric); +% case 'acceleration coord' +% dcost = ddshape(:)'*metric*ddshape(:); +% case 'power quality' +% dcost = power_quality_cost(M_a,dM,shape,dshape,ddshape); +% end +% +% 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 \ No newline at end of file diff --git a/ProgramFiles/Animation/efficiencyplot.m b/ProgramFiles/Animation/efficiencyplot.m new file mode 100644 index 00000000..eb444476 --- /dev/null +++ b/ProgramFiles/Animation/efficiencyplot.m @@ -0,0 +1,26 @@ +function efficiencyplot(vp) + + +syslist = {'links','cc','serp'}; + +f = figure(1337); +clf(f,'reset') +cost = 'pathlength'; +for idx = 1:numel(syslist) + line([vp.(syslist{idx}).(cost).normalizedPeriod{:}],[vp.(syslist{idx}).(cost).netDisp{:}]) +end +lims = axis; +lims(1) = 0; +lims(3) = 0; +axis(lims); + +f = figure(1338); +clf(f,'reset') +cost = 'covaccel'; +for idx = 1:numel(syslist) + line([vp.(syslist{idx}).(cost).normalizedPeriod{:}],[vp.(syslist{idx}).(cost).netDisp{:}]) +end +lims = axis; +lims(1) = 0; +lims(3) = 0; +axis(lims); \ No newline at end of file diff --git a/ProgramFiles/Animation/generate_locomotor_race.m b/ProgramFiles/Animation/generate_locomotor_race.m new file mode 100644 index 00000000..814047d2 --- /dev/null +++ b/ProgramFiles/Animation/generate_locomotor_race.m @@ -0,0 +1,191 @@ +function vp = generate_locomotor_race(makevideo) + +load sysplotter_config.mat + +info_needed = struct('Framerate',15,... + 'Duration',3,... + 'Coordinates','minperturbation_coords',... + 'sysplotterpath', sysplotterpath,... + 'datapath', datapath,... + 'UserFile_path', [syspath '/..'],... + 'syspath', syspath,... + 'current_system2', {{'three_link_lowRe','three_link_lowRe','serpenoid_lowRe'}},... + 'current_shch2', {{'LowRe_MaxDisplacement','LowRe_MaxEfficiency','circle_6'}},... + 'costfunction',{{'pathlength metric','pathlength metric','pathlength metric'}}); + +info_needed.Movies = struct('system', 1,... + 'CCFx', 0,... + 'CCFy', 0,... + 'CCFtheta', 0,... + 'vfieldx', 0,... + 'vfieldy', 0,... + 'vfieldtheta', 0); + + +% systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe'}; +% gaitlist = {'opt_181021dcd641302d8c00a64cae111ac7','opt_a5004c97c4eebdb9b2658e839901b216','opt_36f65157e963179771c57115cf0271bb'}; +% costlist = {'covariant acceleration','covariant acceleration','covariant acceleration'}; +% +% systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe'}; +% gaitlist = {'opt_81e2fb7d09f4b17ab39587bc9fb83f59','opt_8045ea7a2eea0e4a3f6be1ed182ddf27','opt_07a0c5552b3d0852d5f87f6fcad6d30c'}; +% costlist = {'pathlength metric','pathlength metric','pathlength metric'}; + +% systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe'}; +% gaitlist = {'opt_81e2fb7d09f4b17ab39587bc9fb83f59','opt_8045ea7a2eea0e4a3f6be1ed182ddf27_2','opt_07a0c5552b3d0852d5f87f6fcad6d30c_2'}; +% costlist = {'pathlength metric','pathlength metric','pathlength metric'}; + +% 3-, 4-, and 5-link swimmers, pathlength +vp.links.pathlength.systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe','six_link_HighRe'}; +vp.links.pathlength.gaitlist = {'opt_81dc07b81f8b5e5976cbdbcfbd5bc222','opt_78744a0b8b59ededa15fcc94a80c8050','opt_175268cfa591e8fdb4ea2c9348a503b7','opt_7aaf7309edd7d3778771a86c163869a5'}; +vp.links.pathlength.costlist = {'pathlength metric','pathlength metric','pathlength metric','pathlength metric'}; + +% 3-, 4-, and 5-link swimmers, covariant acceleration +vp.links.covaccel.systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe','six_link_HighRe'}; +vp.links.covaccel.gaitlist = {'opt_074cc8a8fe958909eda886934de4aa0d','opt_e367cce7e4018f9fd1a898ac68837563','opt_d50c438fe7b2469e5ffc67ecd329204a','opt_981d5a890c2f08705c8871d5546b37a0'}; +vp.links.covaccel.costlist = {'covariant acceleration','covariant acceleration','covariant acceleration','covariant acceleration'}; + +% Comparisons of link systems between pathlength and covariant-acceleration +% gaits + +sys_idx = 1; +vp.links.comp1.systemlist = {vp.links.pathlength.systemlist{sys_idx}, vp.links.covaccel.systemlist{sys_idx}}; +vp.links.comp1.gaitlist = {vp.links.pathlength.gaitlist{sys_idx},vp.links.covaccel.gaitlist{sys_idx}}; +vp.links.comp1.costlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 2; +vp.links.comp2.systemlist = {vp.links.pathlength.systemlist{sys_idx}, vp.links.covaccel.systemlist{sys_idx}}; +vp.links.comp2.gaitlist = {vp.links.pathlength.gaitlist{sys_idx},vp.links.covaccel.gaitlist{sys_idx}}; +vp.links.comp2.costlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 3; +vp.links.comp3.systemlist = {vp.links.pathlength.systemlist{sys_idx}, vp.links.covaccel.systemlist{sys_idx}}; +vp.links.comp3.gaitlist = {vp.links.pathlength.gaitlist{sys_idx},vp.links.covaccel.gaitlist{sys_idx}}; +vp.links.comp3.costlist = {'covariant acceleration','covariant acceleration'}; + + +% 2-, 3-, and 4-segment constant-curvature swimmers, pathlength +vp.cc.pathlength.systemlist = {'two_mode_CC','three_mode_CC','four_mode_CC','five_mode_CC'}; +vp.cc.pathlength.gaitlist = {'opt_14da671eded328d3635cc4b3b8a2e7c2','opt_08a9f975bb9ae77540d7a6ecdf728d3b','opt_74d3b3d293d9707eb8afbca804ee22ff','opt_50381ee845fff0a72bef8c64221e6927'}; +vp.cc.pathlength.costlist = {'pathlength metric','pathlength metric','pathlength metric','pathlength metric'}; + + +% 2-, 3-, and 4-segment constant-curvature swimmers, covariant acceleration +vp.cc.covaccel.systemlist = {'two_mode_CC','three_mode_CC','four_mode_CC','five_mode_CC'}; +vp.cc.covaccel.gaitlist = {'opt_becaae5749ff7603a88791d0df178ef7','opt_91c1f2fc8d41ae5783b5486b6073aa54','opt_bc286eed659533fc39d891b0dbb19ddb','opt_cfc708f21b8915e131239b2a4c8f0f0b'}; +vp.cc.covaccel.costlist = {'covariant acceleration','covariant acceleration','covariant acceleration','covariant acceleration'}; + + + +% Comparisons of piecewise-constant systems between pathlength and covariant-acceleration +% gaits + +sys_idx = 1; +vp.cc.comp1.systemlist = {vp.cc.pathlength.systemlist{sys_idx}, vp.cc.covaccel.systemlist{sys_idx}}; +vp.cc.comp1.gaitlist = {vp.cc.pathlength.gaitlist{sys_idx},vp.cc.covaccel.gaitlist{sys_idx}}; +vp.cc.comp1.costlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 2; +vp.cc.comp2.systemlist = {vp.cc.pathlength.systemlist{sys_idx}, vp.cc.covaccel.systemlist{sys_idx}}; +vp.cc.comp2.gaitlist = {vp.cc.pathlength.gaitlist{sys_idx},vp.cc.covaccel.gaitlist{sys_idx}}; +vp.cc.comp2.costlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 3; +vp.cc.comp3.systemlist = {vp.cc.pathlength.systemlist{sys_idx}, vp.cc.covaccel.systemlist{sys_idx}}; +vp.cc.comp3.gaitlist = {vp.cc.pathlength.gaitlist{sys_idx},vp.cc.covaccel.gaitlist{sys_idx}}; +vp.cc.comp3.costlist = {'covariant acceleration','covariant acceleration'}; + + + +% 2- and 4-mode serpenoid swimmers, pathlength +vp.serp.pathlength.systemlist = {'two_mode_serpenoid','four_mode_serpenoid'}; +vp.serp.pathlength.gaitlist = {'opt_33bf8d4335665da9b9362bc55f4a5c4b','opt_0f798d8a3cb62f89a869c7990e77220e'}; +vp.serp.pathlength.costlist = {'pathlength metric','pathlength metric'}; + + +% 2- and 4-mode serpenoid swimmers, covariant acceleration +vp.serp.covaccel.systemlist = {'two_mode_serpenoid','four_mode_serpenoid'}; +vp.serp.covaccel.gaitlist = {'opt_c7573722a48f8767276a7cdee661e66b','opt_a033f8e1a5a5750d2b8f640a22ecf2dd'}; +vp.serp.covaccel.costlist = {'covariant acceleration','covariant acceleration'}; + +% Comparisons of piecewise-constant systems between pathlength and covariant-acceleration +% gaits + +sys_idx = 1; +vp.serp.comp1.systemlist = {vp.serp.pathlength.systemlist{sys_idx}, vp.serp.covaccel.systemlist{sys_idx}}; +vp.serp.comp1.gaitlist = {vp.serp.pathlength.gaitlist{sys_idx},vp.serp.covaccel.gaitlist{sys_idx}}; +vp.serp.comp1.costlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 2; +vp.serp.comp2.systemlist = {vp.serp.pathlength.systemlist{sys_idx}, vp.serp.covaccel.systemlist{sys_idx}}; +vp.serp.comp2.gaitlist = {vp.serp.pathlength.gaitlist{sys_idx},vp.serp.covaccel.gaitlist{sys_idx}}; +vp.serp.comp2.costlist = {'covariant acceleration','covariant acceleration'}; + +% vp.serp.deg.systemlist = {vp.serp.pathlength.systemlist{1}, vp.serp.covaccel.systemlist{2}}; +% vp.serp.deg.gaitlist = {vp.serp.covaccel.gaitlist{1},vp.serp.covaccel.gaitlist{1}}; +% vp.serp.deg.costlist = {'covariant acceleration','covariant acceleration'}; +% +% Comparisons of two-mode performance, pathlength +vp.twomodes.pathlength.systemlist = {vp.links.pathlength.systemlist{1}, vp.cc.pathlength.systemlist{1}, vp.serp.pathlength.systemlist{1}}; +vp.twomodes.pathlength.gaitlist = {vp.links.pathlength.gaitlist{1}, vp.cc.pathlength.gaitlist{1}, vp.serp.pathlength.gaitlist{1}}; +vp.twomodes.pathlength.costlist = {'pathlength metric','pathlength metric','pathlength metric'}; + +% Comparisons of two-mode performance, covariant acceleration +vp.twomodes.covaccel.systemlist = {vp.links.covaccel.systemlist{1}, vp.cc.covaccel.systemlist{1}, vp.serp.covaccel.systemlist{1}}; +vp.twomodes.covaccel.gaitlist = {vp.links.covaccel.gaitlist{1}, vp.cc.covaccel.gaitlist{1}, vp.serp.covaccel.gaitlist{1}}; +vp.twomodes.covaccel.costlist = {'covariant acceleration','covariant acceleration','covariant acceleration'}; + + +% Comparisons of four-mode performance, pathlength +vp.fourmodes.pathlength.systemlist = {vp.links.pathlength.systemlist{3}, vp.cc.pathlength.systemlist{3}, vp.serp.pathlength.systemlist{2}}; +vp.fourmodes.pathlength.gaitlist = {vp.links.pathlength.gaitlist{3}, vp.cc.pathlength.gaitlist{3}, vp.serp.pathlength.gaitlist{2}}; +vp.fourmodes.pathlength.costlist = {'pathlength metric','pathlength metric','pathlength metric'}; + +% Comparisons of four-mode performance, covariant acceleration +vp.fourmodes.covaccel.systemlist = {vp.links.covaccel.systemlist{3}, vp.cc.covaccel.systemlist{3}, vp.serp.covaccel.systemlist{2}}; +vp.fourmodes.covaccel.gaitlist = {vp.links.covaccel.gaitlist{3}, vp.cc.covaccel.gaitlist{3}, vp.serp.covaccel.gaitlist{2}}; +vp.fourmodes.covaccel.costlist = {'covariant acceleration','covariant acceleration','covariant acceleration'}; + + +% syslist = fieldnames(vp); +% +% for idx = 1:numel(syslist) +% sys = syslist{idx}; +% +% costlist = fieldnames(vp.(syslist{idx})); +% +% for idx2 = 1:numel(costlist) +% cost = costlist{idx2}; +% +% info_needed.current_system2 = vp.(sys).(cost).systemlist; +% info_needed.current_shch2 = vp.(sys).(cost).gaitlist; +% info_needed.costfunction = vp.(sys).(cost).costlist; +% info_needed.moviename = [sys '_' cost]; +% +% +% [vp.(sys).(cost).normalizedPeriod, vp.(sys).(cost).netDisp] = animate_locomotor_race(0,info_needed,makevideo); +% end +% +% end + +% vp.links6.pathlength.systemlist = {'six_link_HighRe'}; +% vp.links6.pathlength.gaitlist = {'opt_7aaf7309edd7d3778771a86c163869a5'}; +% vp.links6.pathlength.costlist = {'pathlength metric'}; +% +% vp.links5.pathlength.systemlist = {'five_link_HighRe','five_link_HighRe'}; +% vp.links5.pathlength.gaitlist = {'circle_1p0_4serial','opt_175268cfa591e8fdb4ea2c9348a503b7'}; +% vp.links5.pathlength.costlist = {'pathlength metric','pathlength metric'}; +% +% vp.serp4.pathlength.systemlist = {'four_mode_serpenoid','four_mode_serpenoid'}; +% vp.serp4.pathlength.gaitlist = {'circle_6p0_4oddeven','opt_3b51a14e53557544541f4431219ea451'}; +% vp.serp4.pathlength.costlist = {'pathlength metric','pathlength metric'}; +% +sys = 'serp'; +cost = 'covaccel'; + + info_needed.current_system2 = vp.(sys).(cost).systemlist; + info_needed.current_shch2 = vp.(sys).(cost).gaitlist; + info_needed.costfunction = vp.(sys).(cost).costlist; + info_needed.moviename = [sys '_' cost]; + + + [vp.(sys).(cost).normalizedPeriod, vp.(sys).(cost).netDisp] = animate_locomotor_race(0,info_needed,makevideo); diff --git a/ProgramFiles/Animation/generate_locomotor_race_final.m b/ProgramFiles/Animation/generate_locomotor_race_final.m new file mode 100644 index 00000000..4f3a12c9 --- /dev/null +++ b/ProgramFiles/Animation/generate_locomotor_race_final.m @@ -0,0 +1,215 @@ +function vp = generate_locomotor_race_final(makevideo) + +load sysplotter_config.mat + +info_needed = struct('Framerate',30,... + 'Duration',3,... + 'Coordinates','minperturbation_coords',... + 'sysplotterpath', sysplotterpath,... + 'datapath', datapath,... + 'UserFile_path', [syspath '/..'],... + 'syspath', syspath,... + 'current_system2', {{}},... + 'current_shch2', {{}},... + 'optcostfunction',{{}}); + +info_needed.Movies = struct('system', 1,... + 'CCFx', 0,... + 'CCFy', 0,... + 'CCFtheta', 0,... + 'vfieldx', 0,... + 'vfieldy', 0,... + 'vfieldtheta', 0); + + +% systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe'}; +% seedlist = {'opt_181021dcd641302d8c00a64cae111ac7','opt_a5004c97c4eebdb9b2658e839901b216','opt_36f65157e963179771c57115cf0271bb'}; +% optcostlist = {'covariant acceleration','covariant acceleration','covariant acceleration'}; +% +% systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe'}; +% seedlist = {'opt_81e2fb7d09f4b17ab39587bc9fb83f59','opt_8045ea7a2eea0e4a3f6be1ed182ddf27','opt_07a0c5552b3d0852d5f87f6fcad6d30c'}; +% optcostlist = {'pathlength metric','pathlength metric','pathlength metric'}; + +% systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe'}; +% seedlist = {'opt_81e2fb7d09f4b17ab39587bc9fb83f59','opt_8045ea7a2eea0e4a3f6be1ed182ddf27_2','opt_07a0c5552b3d0852d5f87f6fcad6d30c_2'}; +% optcostlist = {'pathlength metric','pathlength metric','pathlength metric'}; + +% 3-, 4-, and 5-link swimmers, pathlength +vp.links.pathlength.systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe','six_link_HighRe'}; +vp.links.pathlength.seedlist = {'circle_1p0_2serial','circle_1p0_3serial','circle_1p0_4serial','circle_1p0_5serial'}; +vp.links.pathlength.optcostlist = {'pathlength metric','pathlength metric','pathlength metric','pathlength metric'}; + +% 3-, 4-, and 5-link swimmers, covariant acceleration +vp.links.covaccel.systemlist = {'three_link_HighRe','four_link_HighRe','five_link_HighRe','six_link_HighRe'}; +vp.links.covaccel.seedlist = {'circle_1p0_2serial','circle_1p0_3serial','circle_1p0_4serial','circle_1p0_5serial'}; +vp.links.covaccel.optcostlist = {'covariant acceleration','covariant acceleration','covariant acceleration','covariant acceleration'}; + +% Comparisons of link systems between pathlength and covariant-acceleration +% gaits + +sys_idx = 1; +vp.links.comp1.systemlist = {vp.links.pathlength.systemlist{sys_idx}, vp.links.covaccel.systemlist{sys_idx}}; +vp.links.comp1.seedlist = {vp.links.pathlength.seedlist{sys_idx},vp.links.covaccel.seedlist{sys_idx}}; +vp.links.comp1.optcostlist = {'pathlength metric','covariant acceleration'}; +vp.links.comp1.evalcostlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 2; +vp.links.comp2.systemlist = {vp.links.pathlength.systemlist{sys_idx}, vp.links.covaccel.systemlist{sys_idx}}; +vp.links.comp2.seedlist = {vp.links.pathlength.seedlist{sys_idx},vp.links.covaccel.seedlist{sys_idx}}; +vp.links.comp2.optcostlist = {'pathlength metric','covariant acceleration'}; +vp.links.comp2.evalcostlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 3; +vp.links.comp3.systemlist = {vp.links.pathlength.systemlist{sys_idx}, vp.links.covaccel.systemlist{sys_idx}}; +vp.links.comp3.seedlist = {vp.links.pathlength.seedlist{sys_idx},vp.links.covaccel.seedlist{sys_idx}}; +vp.links.comp3.optcostlist = {'pathlength metric','covariant acceleration'}; +vp.links.comp3.evalcostlist = {'covariant acceleration','covariant acceleration'}; + + +% 2-, 3-, and 4-segment constant-curvature swimmers, pathlength +vp.cc.pathlength.systemlist = {'two_mode_CC','three_mode_CC','four_mode_CC','five_mode_CC'}; +vp.cc.pathlength.seedlist = {'circle_4p0_2serial','circle_4p0_3serial','circle_4p0_4serial','circle_4p0_5serial'}; +vp.cc.pathlength.optcostlist = {'pathlength metric','pathlength metric','pathlength metric','pathlength metric'}; + + +% 2-, 3-, and 4-segment constant-curvature swimmers, covariant acceleration +vp.cc.covaccel.systemlist = {'two_mode_CC','three_mode_CC','four_mode_CC','five_mode_CC'}; +vp.cc.covaccel.seedlist = {'circle_4p0_2serial','circle_4p0_3serial','circle_4p0_4serial','circle_4p0_5serial'}; +vp.cc.covaccel.optcostlist = {'covariant acceleration','covariant acceleration','covariant acceleration','covariant acceleration'}; + + + +% Comparisons of piecewise-constant systems between pathlength and covariant-acceleration +% gaits + +sys_idx = 1; +vp.cc.comp1.systemlist = {vp.cc.pathlength.systemlist{sys_idx}, vp.cc.covaccel.systemlist{sys_idx}}; +vp.cc.comp1.seedlist = {vp.cc.pathlength.seedlist{sys_idx},vp.cc.covaccel.seedlist{sys_idx}}; +vp.cc.comp1.optcostlist = {'pathlength metric','covariant acceleration'}; +vp.cc.comp1.evalcostlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 2; +vp.cc.comp2.systemlist = {vp.cc.pathlength.systemlist{sys_idx}, vp.cc.covaccel.systemlist{sys_idx}}; +vp.cc.comp2.seedlist = {vp.cc.pathlength.seedlist{sys_idx},vp.cc.covaccel.seedlist{sys_idx}}; +vp.cc.comp2.optcostlist = {'pathlength metric','covariant acceleration'}; +vp.cc.comp2.evalcostlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 3; +vp.cc.comp3.systemlist = {vp.cc.pathlength.systemlist{sys_idx}, vp.cc.covaccel.systemlist{sys_idx}}; +vp.cc.comp3.seedlist = {vp.cc.pathlength.seedlist{sys_idx},vp.cc.covaccel.seedlist{sys_idx}}; +vp.cc.comp3.optcostlist = {'pathlength metric','covariant acceleration'}; +vp.cc.comp3.evalcostlist = {'covariant acceleration','covariant acceleration'}; + + + +% 2- and 4-mode serpenoid swimmers, pathlength +vp.serp.pathlength.systemlist = {'two_mode_serpenoid','four_mode_serpenoid'}; +vp.serp.pathlength.seedlist = {'circle_6p0_2oddeven','circle_6p0_4oddeven'}; +vp.serp.pathlength.optcostlist = {'pathlength metric','pathlength metric'}; + + +% 2- and 4-mode serpenoid swimmers, covariant acceleration +vp.serp.covaccel.systemlist = {'two_mode_serpenoid','four_mode_serpenoid'}; +vp.serp.covaccel.seedlist = {'circle_6p0_2oddeven','circle_6p0_4oddeven'}; +vp.serp.covaccel.optcostlist = {'covariant acceleration','covariant acceleration'}; + +% Comparisons of piecewise-constant systems between pathlength and covariant-acceleration +% gaits + +sys_idx = 1; +vp.serp.comp1.systemlist = {vp.serp.pathlength.systemlist{sys_idx}, vp.serp.covaccel.systemlist{sys_idx}}; +vp.serp.comp1.seedlist = {vp.serp.pathlength.seedlist{sys_idx},vp.serp.covaccel.seedlist{sys_idx}}; +vp.serp.comp1.optcostlist = {'pathlength metric','covariant acceleration'}; +vp.serp.comp1.evalcostlist = {'covariant acceleration','covariant acceleration'}; + +sys_idx = 2; +vp.serp.comp2.systemlist = {vp.serp.pathlength.systemlist{sys_idx}, vp.serp.covaccel.systemlist{sys_idx}}; +vp.serp.comp2.seedlist = {vp.serp.pathlength.seedlist{sys_idx},vp.serp.covaccel.seedlist{sys_idx}}; +vp.serp.comp2.optcostlist = {'pathlength metric','covariant acceleration'}; +vp.serp.comp2.evalcostlist = {'covariant acceleration','covariant acceleration'}; + +% vp.serp.deg.systemlist = {vp.serp.pathlength.systemlist{1}, vp.serp.covaccel.systemlist{2}}; +% vp.serp.deg.seedlist = {vp.serp.covaccel.seedlist{1},vp.serp.covaccel.seedlist{1}}; +% vp.serp.deg.optcostlist = {'covariant acceleration','covariant acceleration'}; +% +% Comparisons of two-mode performance, pathlength +vp.twomodes.pathlength.systemlist = {vp.links.pathlength.systemlist{1}, vp.cc.pathlength.systemlist{1}, vp.serp.pathlength.systemlist{1}}; +vp.twomodes.pathlength.seedlist = {vp.links.pathlength.seedlist{1}, vp.cc.pathlength.seedlist{1}, vp.serp.pathlength.seedlist{1}}; +vp.twomodes.pathlength.optcostlist = {'pathlength metric','pathlength metric','pathlength metric'}; + +% Comparisons of two-mode performance, covariant acceleration +vp.twomodes.covaccel.systemlist = {vp.links.covaccel.systemlist{1}, vp.cc.covaccel.systemlist{1}, vp.serp.covaccel.systemlist{1}}; +vp.twomodes.covaccel.seedlist = {vp.links.covaccel.seedlist{1}, vp.cc.covaccel.seedlist{1}, vp.serp.covaccel.seedlist{1}}; +vp.twomodes.covaccel.optcostlist = {'covariant acceleration','covariant acceleration','covariant acceleration'}; + + +% Comparisons of four-mode performance, pathlength +vp.fourmodes.pathlength.systemlist = {vp.links.pathlength.systemlist{3}, vp.cc.pathlength.systemlist{3}, vp.serp.pathlength.systemlist{2}}; +vp.fourmodes.pathlength.seedlist = {vp.links.pathlength.seedlist{3}, vp.cc.pathlength.seedlist{3}, vp.serp.pathlength.seedlist{2}}; +vp.fourmodes.pathlength.optcostlist = {'pathlength metric','pathlength metric','pathlength metric'}; + +% Comparisons of four-mode performance, covariant acceleration +vp.fourmodes.covaccel.systemlist = {vp.links.covaccel.systemlist{3}, vp.cc.covaccel.systemlist{3}, vp.serp.covaccel.systemlist{2}}; +vp.fourmodes.covaccel.seedlist = {vp.links.covaccel.seedlist{3}, vp.cc.covaccel.seedlist{3}, vp.serp.covaccel.seedlist{2}}; +vp.fourmodes.covaccel.optcostlist = {'covariant acceleration','covariant acceleration','covariant acceleration'}; + + +syslist = fieldnames(vp); + +for idx = 1:numel(syslist) + sys = syslist{idx}; + + optcostlist = fieldnames(vp.(syslist{idx})); + + for idx2 = 1:numel(optcostlist) + cost = optcostlist{idx2}; + + info_needed.current_system2 = vp.(sys).(cost).systemlist; + info_needed.current_shch2 = vp.(sys).(cost).seedlist; + info_needed.optcostfunction = vp.(sys).(cost).optcostlist; + if isfield(vp.(sys).(cost),'evalcostlist') + info_needed.evalcostfunction = vp.(sys).(cost).evalcostlist; + else + info_needed.evalcostfunction = vp.(sys).(cost).optcostlist; + end + info_needed.moviename = [sys '_' cost]; + + + [vp.(sys).(cost).normalizedPeriod, vp.(sys).(cost).netDisp] = animate_locomotor_race(0,info_needed,makevideo); + end + +end + +% vp.links6.pathlength.systemlist = {'six_link_HighRe'}; +% vp.links6.pathlength.seedlist = {'opt_7aaf7309edd7d3778771a86c163869a5'}; +% vp.links6.pathlength.optcostlist = {'pathlength metric'}; +% +% vp.links5.pathlength.systemlist = {'five_link_HighRe','five_link_HighRe'}; +% vp.links5.pathlength.seedlist = {'circle_1p0_4serial','opt_175268cfa591e8fdb4ea2c9348a503b7'}; +% vp.links5.pathlength.optcostlist = {'pathlength metric','pathlength metric'}; +% +% vp.serp4.pathlength.systemlist = {'four_mode_serpenoid','four_mode_serpenoid'}; +% vp.serp4.pathlength.seedlist = {'circle_6p0_4oddeven','opt_3b51a14e53557544541f4431219ea451'}; +% vp.serp4.pathlength.optcostlist = {'pathlength metric','pathlength metric'}; +% + +% vp.test.pathlength.systemlist = {'three_mode_CC'}; +% vp.test.pathlength.seedlist = {'circle_4p0_3serial'}; +% vp.test.pathlength.optcostlist = {'pathlength metric'}; +% +% +% sys = 'fourmodes'; +% cost = 'pathlength'; +% +% info_needed.current_system2 = vp.(sys).(cost).systemlist; +% info_needed.current_shch2 = vp.(sys).(cost).seedlist; +% info_needed.optcostfunction = vp.(sys).(cost).optcostlist; +% if isfield(vp.(sys).(cost),'evalcostlist') +% info_needed.evalcostfunction = vp.(sys).(cost).evalcostlist; +% else +% info_needed.evalcostfunction = vp.(sys).(cost).optcostlist; +% end +% info_needed.moviename = [sys '_' cost]; +% +% +% [vp.(sys).(cost).normalizedPeriod, vp.(sys).(cost).netDisp] = animate_locomotor_race(0,info_needed,makevideo); diff --git a/ProgramFiles/Animation/locomotor_animation_tools/getNormalizedPeriod.m b/ProgramFiles/Animation/locomotor_animation_tools/getNormalizedPeriod.m new file mode 100644 index 00000000..7d153aca --- /dev/null +++ b/ProgramFiles/Animation/locomotor_animation_tools/getNormalizedPeriod.m @@ -0,0 +1,369 @@ +%s = system struct exported from sysplotter +%p = gait struct exported from sysplotter +%costfunction = 'torque' or 'covariant acceleration' +function normalizedPeriod = getNormalizedPeriod(s,gait,costfun) + + s.costfunction = costfun; + + t = gait.time_full{1}; + phi_def = gait.phi_def{1}{1}(t); + t = linspace(0,1,numel(t)); + + dimension = size(phi_def,2); + + fa=cell(1,dimension); + % The bounds ensure the fourier series terms have the right period + % Bound only the frequency to be 2*pi, such that period = 1 + options = fitoptions('fourier4'); + options.Lower = [-Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf 2*pi]; + options.Upper = -[-Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -2*pi]; + + for i=1:1:dimension + fa{i}=fit(t',phi_def(:,i),'fourier4',options); + end + + nu={'a0';'a1';'b1';'a2';'b2';'a3';'b3';'a4';'b4';'w'}; + + y = zeros(numel(nu),dimension); + for i=1:dimension + for j=1:length(nu) + y(j,i)=fa{i}.(nu{j}); + end + end + + p = makeGait(y); + + [~,cost] = fixed_step_integrator(s,p,[0,1],'interpolated',100); + + if strcmpi(s.costfunction,'torque') || strcmpi(s.costfunction,'covariant acceleration') || strcmpi(s.costfunction,'acceleration coord') || strcmpi(s.costfunction,'power quality') + % With cost as time period, period of the gait is the cost to execute + % the gait at unit torque squared to the 1/4th power + normalizedPeriod = cost^(1/4); + else + % Drag systems have cost as path length, so no modification is needed + normalizedPeriod = cost; + end + +end + + +% 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 ischar(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); +% +% %Prep interpolation inputs for velocities function +% shape = zeros(size(s.grid.eval)); +% shape_gait_def_0 = readGait(gait.phi_def,0); +% actual_size = min(numel(shape),numel(shape_gait_def_0)); +% +% samplePoints = {}; +% for dim = 1:actual_size +% samplePoints{dim} = []; +% end +% +% for time = tpoints +% shape_gait_def = readGait(gait.phi_def,time); +% shape(1:actual_size) = shape_gait_def(1:actual_size); +% for dim = 1:numel(shape) +% samplePoints{dim}(end+1) = shape(dim); +% end +% end +% +% indexList = 1:numel(tpoints); +% id = eye(actual_size); +% +% As = cellfun(@(C) -interpn(s.grid.eval{:},C,... +% samplePoints{:},'spline'),s.vecfield.eval.content.Avec,... +% 'UniformOutput',false); +% As = celltensorconvert(As); +% +% switch s.costfunction +% case {'pathlength coord','acceleration coord'} +% %In the case of these two cost functions, we only care about +% %the connection field, and the metric is always identity. +% %dM is passed a filler value +% [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,As{i},id,1),... +% tpoints,indexList,'UniformOutput',false); +% case {'torque','covariant acceleration','power quality'} +% %In the inertial cases, we need to calculate dM, and the metric +% metrics = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... +% samplePoints{:},'spline'),s.metricfield.metric_eval.content.metric,... +% 'UniformOutput',false); +% metrics = celltensorconvert(metrics); +% +% dM_set = {}; +% for dim = 1:actual_size +% dM_holder = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... +% samplePoints{:},'spline'),s.coriolisfield.coriolis_eval.content.dM{dim},... +% 'UniformOutput',false); +% dM_holder = celltensorconvert(dM_holder); +% dM_set{dim} = dM_holder; +% end +% dMs = {}; +% for i = 1:numel(tpoints) +% dMs{i} = {}; +% for dim = 1:actual_size +% dMs{i}{dim} = dM_set{dim}{i}; +% end +% end +% +% [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,... +% As{i},metrics{i},dMs{i}),tpoints,indexList,'UniformOutput',false); +% otherwise +% %Otherwise, we're not doing inertial so don't need dM, but we +% %do care about the metric and connection +% metrics = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... +% samplePoints{:},'spline'),s.metricfield.metric_eval.content.metric,... +% 'UniformOutput',false); +% metrics = celltensorconvert(metrics); +% +% [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,... +% As{i},metrics{i},1),tpoints,indexList,'UniformOutput',false); +% end +% +% %%%%%%% +% % 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 gait = makeGait(y) + + ndim = size(y,2); + + gait.phi_def = cell(1,ndim); + gait.dphi_def = cell(1,ndim); + gait.ddphi_def = cell(1,ndim); + + for dim = 1:ndim + + w = y(end,dim); + gait.phi_def{dim} = @(t) y(1,dim)+y(2,dim)*cos(w*t)+y(3,dim)*sin(w*t)+y(4,dim)*cos(2*w*t)+... + +y(5,dim)*sin(2*w*t)+y(6,dim)*cos(3*w*t)+y(7,dim)*sin(3*w*t)+... + +y(8,dim)*cos(4*w*t)+y(9,dim)*sin(4*w*t); + gait.dphi_def{dim} = @(t) -w*y(2,dim)*sin(w*t)+w*y(3,dim)*cos(w*t)-2*w*y(4,dim)*sin(2*w*t)+... + +2*w*y(5,dim)*cos(2*w*t)-3*w*y(6,dim)*sin(3*w*t)+3*w*y(7,dim)*cos(3*w*t)+... + -4*w*y(8,dim)*sin(4*w*t)+4*w*y(9,dim)*cos(4*w*t); + gait.ddphi_def{dim} = @(t) -w^2*y(2,dim)*cos(w*t)-w^2*y(3,dim)*sin(w*t)-4*w^2*y(4,dim)*cos(2*w*t)+... + -4*w^2*y(5,dim)*sin(2*w*t)-9*w^2*y(6,dim)*cos(3*w*t)-9*w^2*y(7,dim)*sin(3*w*t)+... + -16*w^2*y(8,dim)*cos(4*w*t)-16*w^2*y(9,dim)*sin(4*w*t); + + end + +end + +function state = readGait(gaitFun,t) + + ndim = numel(gaitFun); + state = zeros(1,ndim); + + for i = 1:ndim + state(i) = gaitFun{i}(t); + end + +end + +% Evaluate the body velocity and cost velocity (according to system metric) +% at a given time +function [gcirc, dcost] = get_velocities(t,s,gait,ConnectionEval,A,metric,dM) + + % Get the shape and shape derivative at the current time + shape = zeros(size(s.grid.eval)); + dshape = zeros(size(s.grid.eval)); + ddshape = zeros(size(s.grid.eval)); + + shape_gait_def = readGait(gait.phi_def,t); + dshape_gait_def = readGait(gait.dphi_def,t); + ddshape_gait_def = readGait(gait.ddphi_def,t); + + actual_size = min(numel(shape),numel(shape_gait_def)); + shape(1:actual_size) = shape_gait_def(1:actual_size); + dshape(1:actual_size) = dshape_gait_def(1:actual_size); + ddshape(1:actual_size) = ddshape_gait_def(1:actual_size); + + M_a = metric; + + shapelist = num2cell(shape); + + % If doing functional eval of system (not recommended) + % Get the local connection and metric at the current time, in the new coordinates + if strcmpi(ConnectionEval,'functional') + + A = s.A_num(shapelist{:})./s.A_den(shapelist{:}); + + switch s.system_type + case 'drag' + metric = s.metric(shapelist{:}); + case 'inertia' + error('Functional ConnectionEval method not supported for inertia systems!') + end + + end + + % Get the body velocity at the current time + %t; + gcirc = - A * dshape(:); + + switch s.costfunction + case {'pathlength metric','pathlength coord'} + dcost = sqrt(dshape(:)'*metric*dshape(:)); + case 'pathlength metric2' + dcost = sqrt(dshape(:)'*metric*metric*dshape(:)); + case 'torque' + dcost = torque_cost(M_a,dM,shape,dshape,ddshape,metric); + case 'covariant acceleration' + dcost = acceleration_cost(M_a,dM,shape,dshape,ddshape,metric); + case 'acceleration coord' + dcost = ddshape(:)'*metric*ddshape(:); + case 'power quality' + dcost = power_quality_cost(M_a,dM,shape,dshape,ddshape); + end + +end + +function dcost = torque_cost(M,dM,shape,dshape,ddshape,metric) +% Calculates the incremental cost for an inertial system where cost is torque squared. +% Inputs: +% M: Mass matrix +% dM_alphadalpha: Partial of mass matrix with respect to shape variables; +% must be cell where the ith entry is the partial with respect to the +% ith shape variable +% shape: Current shape configuration of system, at which M and +% dM_alphadalpha were evaluated +% dshape: Current shape velocity of system +% ddshape: Current shape acceleration of system + + % Start by calculating the coriolis matrix + C = calc_coriolis_matrix(dM,shape,dshape); + % Calculate the torque for this instant of time and return the inner + % product of the torque with itself + dtau = M*ddshape(:) + C; + dcost = dtau'*dtau; +end + +function dcost = acceleration_cost(M,dM,shape,dshape,ddshape,metric) +% Calculates the incremental cost for an inertial system where cost is covariant acceleration. +% Inputs: +% M: Mass matrix +% dM_alphadalpha: Partial of mass matrix with respect to shape variables; +% must be cell where the ith entry is the partial with respect to the +% ith shape variable +% shape: Current shape configuration of system, at which M and +% dM_alphadalpha were evaluated +% dshape: Current shape velocity of system +% ddshape: Current shape acceleration of system + C = calc_coriolis_matrix(dM,shape,dshape); + cov_acc = ddshape(:) + inv(M)*C; + dcost = cov_acc'*metric*cov_acc; + +end + +function C = calc_coriolis_matrix(dM,shape,dshape) + % Start with (dM_alpha/dalpha*qdot)*qdot terms + C_temp = zeros(length(shape)); + if isa(dM{1},'sym') + C_temp = sym(C_temp); + end + for i = 1:length(dM) + C_temp = C_temp + dM{i}*dshape(i); + end + C = C_temp*dshape(:); + % Add on the (-1/2)*qdot'*dM_alpha/dalpha*qdot terms + C_temp = zeros(length(shape),1); + if isa(dM{1},'sym') + C_temp = sym(C_temp); + end + for i = 1:length(dM) + C_temp(i) = -(1/2)*dshape(:)'*dM{i}*dshape(:); + end + C = C + C_temp; + if isa(dM{1},'sym') + C = simplify(C); + end +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 + +function dcost = power_quality_cost(M,dM,shape,dshape,ddshape) +% Calculates the incremental cost for an inertial system where cost is power quality. +% Inputs: +% M: Mass matrix +% dM_alphadalpha: Partial of mass matrix with respect to shape variables; +% must be cell where the ith entry is the partial with respect to the +% ith shape variable +% shape: Current shape configuration of system, at which M and +% dM_alphadalpha were evaluated +% dshape: Current shape velocity of system +% ddshape: Current shape acceleration of system + + % Start by calculating the coriolis matrix + C = calc_coriolis_matrix(dM,shape,dshape); + % Calculate the torque for this instant of time + dtau = M*ddshape(:) + C; + % Calculate power quality + dcost = (dshape(:)'*dtau)^2 - ((dshape(:)').^2*dtau.^2); + dcost = dcost + 100; +end \ No newline at end of file diff --git a/ProgramFiles/Animation/sysplotter_animation.m b/ProgramFiles/Animation/sysplotter_animation.m index b11cdab3..d9bb8799 100755 --- a/ProgramFiles/Animation/sysplotter_animation.m +++ b/ProgramFiles/Animation/sysplotter_animation.m @@ -1,4 +1,4 @@ -function [frame_info, endframe] = animation(frame_gen_function,frame_info,timing,destination,export,skip,startframe) +function [frame_info, endframe] = sysplotter_animation(frame_gen_function,frame_info,timing,destination,export,skip,startframe) % An animation framework for writing a series of frames into a directory, % at which point ffmpeg, QuicktimePro, or the like can be used to turn them % into a movie. diff --git a/ProgramFiles/Animation/sysplotter_animation_race.m b/ProgramFiles/Animation/sysplotter_animation_race.m new file mode 100644 index 00000000..019db34f --- /dev/null +++ b/ProgramFiles/Animation/sysplotter_animation_race.m @@ -0,0 +1,286 @@ +function [frame_info, endframe] = sysplotter_animation_race(frame_gen_function,frame_info,timing,destination,export,skip,startframe) +% An animation framework for writing a series of frames into a directory, +% at which point ffmpeg, QuicktimePro, or the like can be used to turn them +% into a movie. +% +% +% ================ +% +% Inputs are: +% +% frame_gen_function: A function that takes the frame_info structure and +% normalized time (from 0 to 1) and draws +% draws the animation frame for this movie at that time, returning a frame_info +% structure that may be updated with any new information about the frame +% content. *Note that frame_gen_function should indicate the desired printing +% mode for the frame in the frame_info structure as noted below* +% +% frame_info: A structure containing: +% frame_info.printmethod = the print function, with all parameters set +% except for destination, which will be set based on the 'destination' +% parameter here +% any other fields (such as axes, or information about the content of the +% previous frame) used within frame_gen_function. +% +% timing: A structure containing the timing information for the animation: +% timing.duration: How many seconds the animation should run for +% timing.fps: How many frames per second in the animation +% timing.pacing: Handle to a pacing function. This should be something +% like +% @(y) linspace(0,1,y) +% i.e. taking a number of frames to plot, and sampling them across a +% specified number of frames according to a desired distribution. The +% interval will be normalized to a unit interval post-sampling. +% +% destination: string indicating the path (local or global) of the +% directory in which to place the images. +% +% export: optional argument, boolean for whether to export the frames or +% just draw them. Defaults to 0 (no export) +% +% skip: optional argument, will draw only the last frame of the animation. +% defaults to 0 (do not skip the animation) +% +% startframe: optional argument to set the starting frame. Defaults to 1. +% +% ======================== +% +% Outputs are: +% +% frame_info: last frame_info output by the frame_gen_function +% +% endframe: last frame number written by the program +% +% +% ======================== +% Copyright Ross L. Hatton, 2011 + + % Create the object for saving the file +% writerObj = VideoWriter('swimmer.avi'); + + + %%%% + % Handle default arguments + + + % Set the export to default if necessary + if ~exist('export','var') + export = 0; + end + + % Set the skip to default if necessary + if ~exist('skip','var') + skip = 0; + end + + + % Set the startframe to default if necessary + if ~exist('startframe','var') + startframe = 1; + end + +% % view will be used like export, but for which animations you want to +% % have play live (unexported, in a matlab figure) +% view = export; % For now, all exported animations are also played after. +% % animations that are either viewed or exported need frames calculated +% calc = arrayfun(@or,export,view); +% % warning: export and view must be the same size + + + % If the destination is a single string, then wrap it in a cell array + if isstr(destination) + destination = {destination}; + end + + % Set output video filename + if isfield(frame_info{1}, 'sysname') && isfield(frame_info{1}, 'pathname') + outputfname = [frame_info{1}.sysname '_' frame_info{1}.pathname]; + else + outputfname = 'gait_animation'; + end + + % Globalize the path if necessary (some printing commands may require + % global destinations). Checks to see if destination starts with an + % absolute-path specifier, and prepends the working directory if this + % is not present + Animation_dir = 'Animation'; + + if ~ ( (ispc && strcmp(':\',destination{1}(2:3))) || (isunix && strncmp('/',destination{1},1) ) ) + + destination = cellfun(@(x)fullfile(pwd, x, Animation_dir),destination,'UniformOutput',false); + + end + + % Only ensure directories are present if export is set +% for i = 1:numel(export) + for i = 1:numel(destination) + +% if export(i) + + % Ensure that destination directory exists + [parent,movie] = fileparts(destination{i}); + if ~exist(parent,'dir') + mkdir(parent) + end + + + +% end + + end + + + %%%%%%%%%%%%%%%%% + % Work out the timing. Get the framepoints (fractions within the span of + % the movie, normalized to a unit interval) + + % Generate a set of points from the timing function + framepoints_raw = timing.pacing(round(timing.duration * timing.fps)); + + % Put these points on a zero-to-one scale (necessary if, e.g., the + % timing vector is logarithmic) + framepoints_zerostart = framepoints_raw-framepoints_raw(1); + framepoints = framepoints_zerostart/framepoints_zerostart(end); + + %%%%%%%%%%%%%% + % Write the frames + + % Prime the framenumber + framecounter = startframe; + + % Prepare datastructure to store frames + frame_info = frame_gen_function(frame_info,framepoints(1)); + + firstframe = frame_info{1}.printmethod('-RGBImage'); + for j = 1:numel(destination) + %if calc(j) + F{j} = struct('cdata',num2cell(zeros([length(framepoints) size(firstframe)],class(firstframe)),[2,3,4]),'colormap',[]); + %end + end + + % Print out the frames + if ~skip % If skipping this movie, don't draw frames + for i = 1:length(framepoints) + + % Draw the frame, and get the information about how to print the + % frame + frame_info = frame_gen_function(frame_info,framepoints(i)); + + % Ensure that all elements of the figure are updated. + drawnow + + % Turn framecounter into a string. String is set up to prepend + % zeros to turn this into a 8-digit number. +% frame_str = num2str(framecounter); +% frame_str = ['fr' repmat('0',1,8-length(frame_str)) frame_str]; %#ok + +% % Print out the frame into the newframes directory if the +% % export flag is set + % Save animation datastructures for all + for j = 1%:numel(destination) + +% if calc(j) + + % Save current frame + % Compatability with old code warning: Remove the image type + % input from the printmethod function and it should work. +% F{j}(i).cdata = rgb2ind(frame_info{j}.printmethod('-RGBImage'),colormap,'nodither'); + F{j}(i).cdata = frame_info{j}.printmethod('-RGBImage'); + % TODO: change F into a cell array of these sorts + % of things so that the gait tracing animations can be + % saved in it too + +% destination_str = fullfile(destination{j},'newframes',frame_str); +% frame_info{1}.printmethod(destination_str); + +% end + end + + % Update the frame counter + framecounter = startframe+i; + +% % Open the object +% open(writerObj); +% % Get the frame +% M = getframe(171); +% % Make the AVi file +% writeVideo(writerObj,M) + + end + + else % execute the last frame, to update the image, and adjust the framecounter + + % Draw the last frame of the movie + frame_info = frame_gen_function(frame_info,framepoints(end)); + + % clear the drawing queue + drawnow + + % Ensure that the framecounter is the same as if the movie hadn't + % been skipped + framecounter = startframe+length(framepoints); + end + + % If the export flag was set, remove the old frames, and move the new + % frames to the main directory +% for i = 1:numel(export) +% if export(i) +% try +% rmdir(fullfile(destination{i},'oldframes')); +% end +% +% movefile(fullfile(destination{i},'newframes','fr*'),destination{i}); +% +% end +% end + + % Return the last frame printed + endframe = framecounter-1; + + + warning('off','MATLAB:audiovideo:VideoWriter:mp4FramePadded') + % Export: + for j = 1%:numel(destination) + %if export(j) + + if ispc || ismac + videoformat = 'MPEG-4'; + else + videoformat = 'Motion JPEG AVI'; + end + + v = VideoWriter(destination{j},videoformat); + v.FrameRate = timing.fps; + open(v) + writeVideo(v,F{j}) + close(v) + + %end + end + +% % Close the figures used during animation making: +% fignums = [17; 171; 172; 173]; % Hardcoded from animate_backbone... +% for fig = fignums +% close(fig) +% end + +% % Play: +% for j = 1:numel(calc) +% if view(j) +% % Create a figure and resize to fit the animation +% animfig=figure(17+j); +% clf(animfig) +% animsize=size(F{j}(end).cdata); +% animfig.Position(3:4)=[animsize(2) animsize(1)]; +% movegui(animfig) % ensure after resizing that figure is still onscreen +% % Play the movie: +% % Maybe resize in order to show live animation smaller? +% % Can't get the code to work right now... +% % Fsm{j}.cdata = arrayfun(@(x) imresize(x.cdata,0.5),F{j},'UniformOutput',false); +% % Fsm{j}.colormap = F{j}.colormap; +% % movie(animfig,Fsm{j},1,timing.fps) +% movie(animfig,F{j},1,timing.fps) +% end +% end + +end \ No newline at end of file diff --git a/ProgramFiles/HodgeHelmholtz/fasthelmholtz.m b/ProgramFiles/CoordinateOptimization/HodgeHelmholtz/fasthelmholtz.m similarity index 100% rename from ProgramFiles/HodgeHelmholtz/fasthelmholtz.m rename to ProgramFiles/CoordinateOptimization/HodgeHelmholtz/fasthelmholtz.m diff --git a/ProgramFiles/HodgeHelmholtz/helmholtz.m b/ProgramFiles/CoordinateOptimization/HodgeHelmholtz/helmholtz.m similarity index 100% rename from ProgramFiles/HodgeHelmholtz/helmholtz.m rename to ProgramFiles/CoordinateOptimization/HodgeHelmholtz/helmholtz.m diff --git a/ProgramFiles/RefPointOptimizer/ref_point_optimizer.m b/ProgramFiles/CoordinateOptimization/RefPointOptimizer/ref_point_optimizer.m similarity index 54% rename from ProgramFiles/RefPointOptimizer/ref_point_optimizer.m rename to ProgramFiles/CoordinateOptimization/RefPointOptimizer/ref_point_optimizer.m index d220831d..f7163559 100644 --- a/ProgramFiles/RefPointOptimizer/ref_point_optimizer.m +++ b/ProgramFiles/CoordinateOptimization/RefPointOptimizer/ref_point_optimizer.m @@ -350,9 +350,265 @@ save(fullfile(datadir,[input_hash '.mat']),'Sinv','grid_params','nodes','cubes') %If not recalculating, load Ainv - else + else - load(fullfile(datadir, [input_hash '.mat']),'Sinv','nodes','cubes') + try + load(fullfile(datadir, [input_hash '.mat']),'Sinv','nodes','cubes') + %Sometimes Sinv doesn't load if the file is too large. This is to make it trigger an + %error to recalc if it doesn't + Sinv; + catch + + + %%%%%%%% + %Construct a cuboid mesh -- assume regular underlying grid + + % Stages numbered according to Becker Carey Oden page 5.3.2 + + + + %%%%%%%%%%%%% + % Stage 1: Get the node list for the mesh and for each hypercube + [nodes,cubes] = hypercube_mesh(grid); + + + %Separate out useful values from grid size + N = size(gridcolumns,1); %number of nodes + n_dim = size(gridcolumns,2); % number of dimensions over which fields are defined + + %Prime the S1 and S2 matrices + S1 = zeros(2*N); + S2 = zeros(2*N,2*n_dim*N); + + % Define the (n-)linear shape functions for the finite elements + [shape_functions, dshape_functions] = hypercube_element_shape_functions(n_dim); + + % Extract the number of shape functions + n_shape = numel(shape_functions); + + %%%%%%%%%%%%%% + % Stage 2: Get the quadrature points and weights for the elements + [quad_points, quad_weights] = hypercube_quadrature_init(n_dim); + + + %%%%%%%%%%%% + % Stage 3: Calculate the values of the shape functions and their + % derivatives at the quadrature points + + shape_at_quad = cellfun(@(f) f(quad_points),shape_functions,'UniformOutput',false); + + dshape_at_quad = cellfun(@(f) f(quad_points),dshape_functions,'UniformOutput',false); + + % Construct interpolating matrices + shape_interpolator = cat(2,shape_at_quad{:}); + + dshape_interpolator = cell(1,size(dshape_at_quad,2)); + for i = 1:size(dshape_interpolator,2) + dshape_interpolator{i} = cat(2,dshape_at_quad{:,i}); + end + + + %%%%%%%%%%% + % Stage 4: Calculate the values of the problem coordinates at the + % quadrature points. We're on a regular grid, so can just use the + % first element + + quad_points_real = cell(n_dim,1); % x(eta), etc + quad_deriv_real = cell(n_dim,n_dim); % dx/d eta , etc + for i = 1:size(quad_points_real,1) % Rows are each one dimension + + quad_points_real{i} = shape_interpolator*nodes(cubes(1,:),i); + + for j = 1:size(quad_deriv_real,2) % Columns are derivatives with respect to the jth dimension + + quad_deriv_real{i,j} = dshape_interpolator{j}*nodes(cubes(1,:),i); + + end + + end + + + %%%%%%%%%%%%% + % Stage 5: Construct the Jacobian + + % Build the Jacobian at all nodes + + % Reshape and merge the quadrature derivatives + quad_deriv_reshape = cell2mat(cellfun(@(x) permute(x,[3 2 1]),quad_deriv_real,'UniformOutput',false)); + + % Take the first slice as the Jacobian, since we're on a regular + % grid and the jacobian is thus constant. + J = quad_deriv_reshape(:,:,1); %dx/d eta + + + %%%%%%%%%%%% + % Stage 6: Get shape function derivatives with respect to problem + % coordinates + + % First, merge dshape_at_quad into a single array + dshape_at_quad_merged = cell2mat(dshape_at_quad); + + % Multiply through by the jacobian + dshape_at_quad_merged_real = (J\dshape_at_quad_merged')'; + + % reseparate + dshape_at_quad_real = mat2cell(dshape_at_quad_merged_real,length(quad_points{1})*ones(n_shape,1),ones(1,n_dim)); + + % Reconstruct interpolating matrix for the derivatives + dshape_interpolator = cell(1,size(dshape_at_quad,2)); + for i = 1:size(dshape_interpolator,2) + dshape_interpolator{i} = cat(2,dshape_at_quad_real{:,i}); + end + + %%%%%%%%%%%%%%%%%%%%%%%%%%% + + % Construct the elements of the solution matrices (_q is for + % versions with quadrature components broken out + + % Quadrature integration vector + q_dim = repmat(quad_weights',1,n_dim); + + % Del operator + Del_q = cat(1,dshape_interpolator{:}); + + % gradPhi operators (same as columns of Del, but splitting the name + % for clarity below + gradPhi_q = Del_q; + + % gradPhi_dot_Del operators + gradPhi_dot_Del_q = cell(1,size(Del_q,2)); + for i = 1:size(gradPhi_dot_Del_q,2) + gradPhi_dot_Del_q{i} = repmat(gradPhi_q(:,i),1,size(Del_q,2)).*Del_q; + end + % Apply quadrature integration + gradPhi_dot_Del = cellfun(@(x) q_dim*x,gradPhi_dot_Del_q,'UniformOutput',false); + + % "Selection" operator -- get values of function at quad points + Sel_q = shape_interpolator; + Sel_q_dim = repmat(Sel_q,n_dim,1); + + % Phi operator (same as selection operator, but splitting for + % clarity + Phi_q = Sel_q; + Phi_q_dim = Sel_q_dim; + + % gradPhi times a selection matrix + gradPhi_dot_R = cell(n_dim,size(Del_q,2)); % one row per dimension, one column per shape function + for i = 1:size(gradPhi_dot_R,1) + for j = 1:size(gradPhi_dot_R,2) + gradPhi_dot_R{i,j} = quad_weights(:)' *... + (repmat(dshape_interpolator{i}(:,j),1,n_shape).* Sel_q); + end + end + + + + + % Loop over each node, filling in the integral data from + % the appropriate precomputed value + for i = 1:N + + % Get the list of cubes containing this node + cubes_at_node = find(any(cubes==i,2)); + cube_list = cubes(cubes_at_node,:); %#ok + + % Loop over each cube attached to the node + for j = 1:size(cube_list,1) + + % Get which corner (and thus basis function) of the cube is + % the current node + cornerI = cube_list(j,:)==i; + + % Build matrix contributions that depend on the cell we're + % in + + % Vector values at nodal points + W_selected = vecentries(cube_list(j,:),:);% cell2mat(cellfun(@(x) x(cube_list(j,:)),Vt(:),'UniformOutput',false)); + + % Get the vector-field values at the quadrature points + W_sel_q = Sel_q * W_selected; + + % Get the dot-product of W with itself + W_dot_W_q = W_sel_q(:).^2; + + % Get Phi_W_dot_W + phiW_dot_W_q = Phi_q_dim(:,cornerI).*W_dot_W_q; + + % Multiply this by the quadrature weights and another + % selection function to generate the map from potential + % function to matrix element + phiW_dot_W = q_dim * (repmat(phiW_dot_W_q,1,n_shape) ... + .* Sel_q_dim); + + + % gradPhi dot W + gradPhi_dot_W_q = gradPhi_q(:,cornerI) .* W_sel_q(:); + gradPhi_dot_W = q_dim * (repmat(gradPhi_dot_W_q,1,n_shape) ... + .* Sel_q_dim); + + + + % phiW_dot_del + phiW_dot_Del_q = repmat(Phi_q_dim(:,cornerI),1,n_shape).*repmat(W_sel_q(:),1,n_shape).*Del_q; + phiW_dot_Del = q_dim * phiW_dot_Del_q; + + + % phiW_dot_R + phiW_dot_R = cell(n_dim,1); + for k = 1:size(phiW_dot_R,1) + phiW_dot_R{k} = quad_weights(:)' * (repmat(Phi_q(:,cornerI),1,n_shape).*(repmat(W_sel_q(:,k),1,n_shape).* Sel_q)); + end + + + + + % Slot the dotproduct values into a vector that pairs them + % with the nodal values they correspond to + + % pull out current nodes for easier readability + cn = cube_list(j,:); + + + % Left hand matrix + + S1(i, cn) = S1(i, cn) + ( gradPhi_dot_Del{cornerI} + phiW_dot_W); % Top, left block: + S1(i, cn+N) = S1(i, cn+N) + (-gradPhi_dot_W + phiW_dot_Del); % Top, right block + + S1(i+N, cn) = S1(i+N, cn) + ( gradPhi_dot_W - phiW_dot_Del); % Bottom, left block + S1(i+N, cn+N) = S1(i+N, cn+N) + ( gradPhi_dot_Del{cornerI} + phiW_dot_W); % Bottom, right block + + + + + % Offset each component's-worth of R values by the number + % of nodes + for m = 1:n_dim + S2(i, cn+(m-1)*N) = S2(i, cn+(m-1)*N) - gradPhi_dot_R{m,cornerI}; % Top left block + S2(i, cn+(m-1)*N+(N*n_dim)) = S2(i, cn+(m-1)*N+(N*n_dim)) - phiW_dot_R{m}; % Top right block + + S2(i+N, cn+(m-1)*N) = S2(i+N, cn+(m-1)*N) + phiW_dot_R{m}; % Bottom left block + S2(i+N, cn+(m-1)*N+(N*n_dim)) = S2(i+N, cn+(m-1)*N+(N*n_dim)) - gradPhi_dot_R{m,cornerI}; % Bottom right block + end + + + end + + + + end + + % Multiply the matrices by the node weighting factors + %S1 = diag([weight(:);weight(:)])*S1; + S2 = diag([weight(:);weight(:)])*S2; + + %Combine the S1 and S2 matrices + Sinv = S1\S2; + + %Save the Sinv matrix and grid parameters to the hashnamed data + %file + grid_params = input_params; %#ok + save(fullfile(datadir,[input_hash '.mat']),'Sinv','grid_params','nodes','cubes') + end end diff --git a/ProgramFiles/CoordinateOptimization/SO3_Optimizer/optimize_so3.m b/ProgramFiles/CoordinateOptimization/SO3_Optimizer/optimize_so3.m new file mode 100644 index 00000000..2848f37f --- /dev/null +++ b/ProgramFiles/CoordinateOptimization/SO3_Optimizer/optimize_so3.m @@ -0,0 +1,292 @@ +% finds SO(3) minimum perturbation coordinates for an n-dim system + +% objective function based on Ross Hatton's SO(3) coordinate optimization +% quadrature rules based on Becker Carey Oden 5.3.2 + +% inputs: + % grid_points: cell array containing vector dimension(s) of points + % A_orig: + % local connection tensoral object + % ought to be a (n_dim)x(n_dim)x...x(n_dim) cell of 3x... matrices + % ref: + % choice of origin configuration (in the shape space) +% outputs: + % grid: + % ndgrid cell array representaion of grid_points + % A_opt: + % local connection tensoral object, in optimal coordinates + % X, Y, Z: + % beta transform in each SO(3) dimension, expressed at grid points + % grad_X, grad_Y, grad_Z: + % gradients of the beta transform, in each dimension + +function [grid, A_opt, X, Y, Z, grad_X, grad_Y, grad_Z] = optimize_so3(grid_points, A_orig, ref) + % dimensionality + n_dim = length(grid_points); + + %% hypercube mesh setup + % generate hypercube mesh to work over + grid = cell(1, n_dim); + [grid{:}] = ndgrid(grid_points{:}); + [nodes, cubes] = hypercube_mesh(grid); + % generate shape functions + [basis_fns, d_basis_fns] = hypercube_element_shape_functions(n_dim); + % generate points, weights for quadrature integration + [quad_points, quad_weights] = hypercube_quadrature_init(n_dim); + % variables for clarity + n_nodes = size(nodes, 1); + n_points = size(quad_points{1},1); + n_vertices = size(cubes, 2); + + %% compute values of basis fns/derivatives at quad vertices + % matrix of basis values at quadpoints (n_points rows of 2^n_dim columns) + % for ea. quad point, value of surrounding points in a hypercube + basis_cell = cellfun(@(fn) fn(quad_points),basis_fns,... + 'UniformOutput', false); + bases_at_quad = cat(2, basis_cell{:}); + % 2d cell of basis derivatives + d_basis_cell = cellfun(@(fn) fn(quad_points), d_basis_fns,... + 'UniformOutput', false); + % convert to cell of derivative matrices (n_dim matrices) + % value of derivative of basis, for each quad point's surroundings, + % along each dimension + d_bases_at_quad = cell(1, n_dim); + for i = 1:n_dim + % concat all (2^n_dim) derivative points into matrix + % stored in cell corresp. to dimension + d_bases_at_quad{i} = cat(2, d_basis_cell{:,i}); + end + + %% scale quadrature points according to problem coordinates + % big assumption: regular grid spacing + % preallocate space + bases_at_quad_act = cell(n_dim, 1); % points in each dimension + quad_deriv_points_act = cell(n_dim, n_dim); % points w.r.t. ea. dim. + for i = 1:n_dim + % SCALES ea. value by size of cube in ea. dim + % assumes a regular grid, and can be done because linear basis + bases_at_quad_act{i} = bases_at_quad * nodes(cubes(1,:), i); + + % do derivatives for this dim + for j =1:n_dim + % derivatives along all other (jth) dimensions for ith dim + quad_deriv_points_act{i, j} = d_bases_at_quad{j} * nodes(cubes(1,:), i); + end + end + + %% scale basis function derivatives using the Jacobian + % get Jacobian from first quadpoint (invariant across regular grid) + J_all = celltensorconvert(quad_deriv_points_act); %nxn cell -> cell of nxn + J = J_all{1}; + % scale derivatives w.r.t. problem coordinates + d_basis_mat = cell2mat(d_basis_cell); + d_basis_mat_act = (J\d_basis_mat')'; %effectively "divides" by J + % split into (n_bases x n_dim) 2d cell of n_quad_points vectors + d_basis_cell_act = mat2cell(d_basis_mat_act,... + n_points*ones(n_vertices,1),... + ones(1,n_dim)); + % finally, construct rescaled cell of derivative matrices + for i = 1:n_dim %loop along ea. dimension + d_bases_at_quad{i} = cat(2, d_basis_cell_act{:,i}); + end + + %% collect ingredients for calculus over the field + % _dim concatenates ea. dimension into a single matrix + + % quadrature weights (for integration across dimensions) + quad_weights_dim = repmat(quad_weights', 1, n_dim); %1x(n_points)n_dim + + % nabla (del, gradient) + % derivative of each basis fn, in each direction, at each vertex + del_dim = cat(1, d_bases_at_quad{:}); %(n_points)(n_dim) x n_vertices + + % gradient of basis functions + grad_rho_dim = del_dim; % equal; all fns over field are linear w.r.t. basis fns + + % rho + rho_q = bases_at_quad; + rho_q_dim = repmat(rho_q, n_dim, 1); + + % gradient dot product, integrated early + detJ = det(J); + grad_rho_dot_del = cell(1, n_vertices); %values at each vertex + for i = 1:n_vertices + grad = repmat(grad_rho_dim(:,i), 1, n_vertices) .* del_dim; + grad_rho_dot_del{i} = detJ * quad_weights_dim * grad; + end + + %% construct linear system from objective functions + % organization: + % rows are blocked out by equation + % columns are blocked out by weights (x, y, z) + % LHS: + % 3 equations, evaluated at n_nodes points + % assigning 3 weights (x, y, z) for each node + LHS = zeros(3*n_nodes); + % RHS: vector result of an area integral + % manifests as a matrix, so rows of bases/dimensions will be summed + RHS = zeros(3*n_nodes, n_nodes*n_dim); + + % reorganize LC (by rotational component, and by node, for later) + % each (n_nodes)x(n_dim) organized in x,y,z pages + A_nodes = cat(3, grid_to_columns(A_orig(1,:)),... + grid_to_columns(A_orig(2,:)),... + grid_to_columns(A_orig(3,:))); + + % add contributions from each node (rows of LHS, RHS) + for n = 1:n_nodes + cubes_containing_node = cubes(any(cubes==n, 2),:); + + % contributions are integral values at this node, summed over all + % adjacent hypercubes + for c = 1:size(cubes_containing_node, 1) + % identify corner with node of interest + corner_idx = cubes_containing_node(c, :) == n; + + % get LC at cube vertices (breaking into components) + A_x_vert = A_nodes(cubes_containing_node(c,:),:,1); + A_y_vert = A_nodes(cubes_containing_node(c,:),:,2); + A_z_vert = A_nodes(cubes_containing_node(c,:),:,3); + % get LC at quadrature points + A_x_q = bases_at_quad * A_x_vert; + A_y_q = bases_at_quad * A_y_vert; + A_z_q = bases_at_quad * A_z_vert; + % get componentwise dot products + A_xx = A_x_q(:).^2; + A_yy = A_y_q(:).^2; + A_zz = A_z_q(:).^2; + + + % compute "vector field squared" terms + x_rho_A_q = rho_q_dim(:, corner_idx) .* (A_zz + A_yy); + y_rho_A_q = rho_q_dim(:, corner_idx) .* (A_zz + A_xx); + z_rho_A_q = rho_q_dim(:, corner_idx) .* (A_yy + A_xx); + % integrate + x_rho_A = detJ * quad_weights_dim * (repmat(x_rho_A_q, 1, n_vertices) .* rho_q_dim); + y_rho_A = detJ * quad_weights_dim * (repmat(y_rho_A_q, 1, n_vertices) .* rho_q_dim); + z_rho_A = detJ * quad_weights_dim * (repmat(z_rho_A_q, 1, n_vertices) .* rho_q_dim); + + % compute "leading gradient" terms + grad_rho_A_x_q = grad_rho_dim(:, corner_idx) .* A_x_q(:); + grad_rho_A_y_q = grad_rho_dim(:, corner_idx) .* A_y_q(:); + grad_rho_A_z_q = grad_rho_dim(:, corner_idx) .* A_z_q(:); + % integrate + grad_rho_A_x_rho = detJ * quad_weights_dim * (repmat(grad_rho_A_x_q, 1, n_vertices) .* rho_q_dim); + grad_rho_A_y_rho = detJ * quad_weights_dim * (repmat(grad_rho_A_y_q, 1, n_vertices) .* rho_q_dim); + grad_rho_A_z_rho = detJ * quad_weights_dim * (repmat(grad_rho_A_z_q, 1, n_vertices) .* rho_q_dim); + + % compute "leading rho" terms + rho_A_x_grad_q = repmat(rho_q_dim(:, corner_idx).*A_x_q(:), 1, n_vertices) .* del_dim; + rho_A_y_grad_q = repmat(rho_q_dim(:, corner_idx).*A_y_q(:), 1, n_vertices) .* del_dim; + rho_A_z_grad_q = repmat(rho_q_dim(:, corner_idx).*A_z_q(:), 1, n_vertices) .* del_dim; + % integrate + rho_A_x_grad = detJ * quad_weights_dim * rho_A_x_grad_q; + rho_A_y_grad = detJ * quad_weights_dim * rho_A_y_grad_q; + rho_A_z_grad = detJ * quad_weights_dim * rho_A_z_grad_q; + + + % build each term (everywhere in cube) + x.xcols = grad_rho_dot_del{corner_idx} + x_rho_A; + x.ycols = grad_rho_A_z_rho - rho_A_z_grad; + x.zcols = -grad_rho_A_y_rho + rho_A_y_grad; + + y.xcols = -grad_rho_A_z_rho + rho_A_z_grad; + y.ycols = grad_rho_dot_del{corner_idx} + y_rho_A; + y.zcols = grad_rho_A_x_rho - rho_A_x_grad; + + z.xcols = grad_rho_A_y_rho - rho_A_y_grad; + z.ycols = -grad_rho_A_x_rho + rho_A_x_grad; + z.zcols = grad_rho_dot_del{corner_idx} + z_rho_A; + + % identify indices + nodes_in_cube = cubes_containing_node(c,:); + x.row = n; + x.col = nodes_in_cube; + y.row = n + n_nodes; + y.col = nodes_in_cube + n_nodes; + z.row = n + 2*n_nodes; + z.col = nodes_in_cube + 2*n_nodes; + + % construct LHS matrix + LHS(x.row, x.col) = LHS(x.row, x.col) + x.xcols; + LHS(x.row, y.col) = LHS(x.row, y.col) + x.ycols; + LHS(x.row, z.col) = LHS(x.row, z.col) + x.zcols; + + LHS(y.row, x.col) = LHS(y.row, x.col) + y.xcols; + LHS(y.row, y.col) = LHS(y.row, y.col) + y.ycols; + LHS(y.row, z.col) = LHS(y.row, z.col) + y.zcols; + + LHS(z.row, x.col) = LHS(z.row, x.col) + z.xcols; + LHS(z.row, y.col) = LHS(z.row, y.col) + z.ycols; + LHS(z.row, z.col) = LHS(z.row, z.col) + z.zcols; + + % compute RHS gradient + for d = 1:n_dim + % assign RHS + rhs_cols = nodes_in_cube + (d-1) * n_nodes; + RHS(x.row, rhs_cols) = RHS(x.row, rhs_cols) + detJ * quad_weights' * (repmat(d_bases_at_quad{d}(:,corner_idx).*A_x_q(:,d), 1, n_vertices) .* rho_q); + RHS(y.row, rhs_cols) = RHS(y.row, rhs_cols) + detJ * quad_weights' * (repmat(d_bases_at_quad{d}(:,corner_idx).*A_y_q(:,d), 1, n_vertices) .* rho_q); + RHS(z.row, rhs_cols) = RHS(z.row, rhs_cols) + detJ * quad_weights' * (repmat(d_bases_at_quad{d}(:,corner_idx).*A_z_q(:,d), 1, n_vertices) .* rho_q); + end + end + end + + %% solve for weights + % solve matrix eqn + beta = lsqminnorm(LHS,RHS); + % add contributions from each interpolating function + beta_eval = sum(beta,2); + + %% construct X, Y, Z interpolating grids + % reshape to X, Y, Z matrices + X = reshape(beta_eval(1:n_nodes), size(grid{1})); + Y = reshape(beta_eval(1+n_nodes:2*n_nodes), size(grid{1})); + Z = reshape(beta_eval(1+2*n_nodes:end), size(grid{1})); + + % compute gradients + [grad_X, grad_Y, grad_Z] = deal(cell(1,n_dim)); + input_order = [2 1 3:n_dim]; %respect gradient's meshgrid desire + [grad_X{input_order}] = gradient(X, grid_points{input_order}); + [grad_Y{input_order}] = gradient(Y, grid_points{input_order}); + [grad_Z{input_order}] = gradient(Z, grid_points{input_order}); + + %% calculate optimized local connection + % make sure we have exp derivative. if not, generate it + if exist('exp_deriv.m','file') == 0 + generate_exp_deriv; + end + % fill optimized LC + A_opt = cell(size(grid{1})); + A_orig = celltensorconvert(A_orig); + for i = 1:n_nodes + % compute new LC in ea. dim + for d = 1:n_dim + % compute rotation term + beta_vec = [X(i) Y(i) Z(i)]; + trans = expm(vec_to_mat_SO3(beta_vec)); + rot = mat_to_vec_SO3(trans'*vec_to_mat_SO3(A_orig{i}(:,d))*trans); + % get time derivative of exponential map + grad_vec = [grad_X{d}(i) grad_Y{d}(i) grad_Z{d}(i)]; + d_exp_mat = exp_deriv(beta_vec, grad_vec); + d_exp_vec = mat_to_vec_SO3(d_exp_mat); + % calc new LC + A_opt{i}(:,d) = rot - d_exp_vec; + end + end + A_opt = celltensorconvert(A_opt); + + %% zero reference nodes + % done after A_opt so we don't introduce NaN + % introduces a static rotation to coordinates + if exist('ref', 'var') + % find reference configuration + ref_node = find(all(nodes == ref', 2)); + else + % use center + ref_node = ceil(n_nodes/2); + end + ref_vals = beta_eval(ref_node + [0 1 2]*n_nodes); + X = X-ref_vals(1); + Y = Y-ref_vals(2); + Z = Z-ref_vals(3); +end \ No newline at end of file diff --git a/ProgramFiles/CoordinateOptimization/SO3_Optimizer/optimize_so3_sys.m b/ProgramFiles/CoordinateOptimization/SO3_Optimizer/optimize_so3_sys.m new file mode 100644 index 00000000..476bddcc --- /dev/null +++ b/ProgramFiles/CoordinateOptimization/SO3_Optimizer/optimize_so3_sys.m @@ -0,0 +1,15 @@ +% optimize_so3_sys.m +% sysplotter-relevant wrapper for general optimize_so3 function + +function s = optimize_so3_sys(s) + % pull out grid spacing as cell of vectors + grid = s.grid.finite_element; + samples = cellfun(@(x) unique(x), grid, 'UniformOutput', false); + % optimize + A_orig = s.vecfield.eval.content.Avec; + [~, A_opt, X, Y, Z, grad_X, grad_Y, grad_Z] = optimize_so3(samples, A_orig); + % save results in sys struct + s.vecfield.eval.content.Avec_optimized = A_opt; + s.B_optimized.eval.Beta = {X, Y, Z}; + s.B_optimized.eval.gradBeta = [grad_X; grad_Y; grad_Z]; +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/Tools/Granular_metric_calc2.m b/ProgramFiles/GaitOptimization/Tools/Granular_metric_calc2.m new file mode 100644 index 00000000..e09b6bc2 --- /dev/null +++ b/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 diff --git a/ProgramFiles/GaitOptimization/Tools/cdiff.m b/ProgramFiles/GaitOptimization/Tools/cdiff.m new file mode 100644 index 00000000..e043f791 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/Tools/celltensorconvert.m b/ProgramFiles/GaitOptimization/Tools/celltensorconvert.m new file mode 100644 index 00000000..6a815ff4 --- /dev/null +++ b/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 diff --git a/ProgramFiles/GaitOptimization/Tools/ddiff.m b/ProgramFiles/GaitOptimization/Tools/ddiff.m new file mode 100644 index 00000000..10ac4b50 --- /dev/null +++ b/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 + diff --git a/ProgramFiles/GaitOptimization/Tools/metricellipsefield.m b/ProgramFiles/GaitOptimization/Tools/metricellipsefield.m new file mode 100644 index 00000000..d7926837 --- /dev/null +++ b/ProgramFiles/GaitOptimization/Tools/metricellipsefield.m @@ -0,0 +1,138 @@ +function h = metricellipsefield(x,y,M,style,varargin) +%Plot an ellipse field based on the singular values of a metric +%tensor M. M should be specified as a cell array with with the same +%dimensions as x and y, and each cell containing a 2x2 matrix containing +%its value at the corresponding x,y location + + +%%%%%%%%%%%%% +% Construct the ellipses for each location + +% Make the circle primitive +th = linspace(0,2*pi,50); +xc = cos(th); +yc = sin(th); + +% Replicate the circle primitive into a cell array at each x,y location +circles = repmat({[xc;yc]},size(x)); + + +switch style + + % The tissot indicatrix, showing linear stretch + case 'tissot' + + % Calculate the svd of the metric tensor + [u,s,v] = cellfun(@(m) svd(inv(m)),M,'UniformOutput',false); + + % Apply the transform corresponding to M to the circles + circles = cellfun(@(u,s,v,c)(u*sqrt(s)*v')*c,u,s,v,circles,'UniformOutput',false); + %circles = cellfun(@(u,s,v,c)(sqrt(s)*v')*c,u,s,v,circles,'UniformOutput',false); + + plot_options = varargin{1}; + + % the tissot indicatrix, with major and minor axis crosses added + case 'tissot-cross' + + % Calculate the svd of the metric tensor + [u,s,v] = cellfun(@(m) svd(inv(m)),M,'UniformOutput',false); + + % Apply the transform corresponding to M to the circles + circles = cellfun(@(u,s,v,c)(u*sqrt(s)*v')*c,u,s,v,circles,'UniformOutput',false); + + % Create the crosses + crosses = cellfun(@(u,s)... + [u(1,1)*s(1,1) -u(1,1)*s(1,1);...% NaN u(1,2)*s(2,2) u(1,2)*s(2,2);... + u(2,1)*s(1,1) -u(2,1)*s(1,1);... NaN u(2,2)*s(2,2) -u(2,2)*s(2,2) + ],u,s,'UniformOutput',false); + + plot_options = varargin{1}; + plot_options_crosses = varargin{2}; + + + +end + + + +% Find the greatest x or y range in the circles +xrange = cellfun(@(u)range(u(1,:)),circles); +yrange = cellfun(@(u)range(u(2,:)),circles); + +xrange = max(xrange(:)); +yrange = max(yrange(:)); + +% Find the smallest spacing in each direction +xspacing = min(diff(x(:,1))); +yspacing = min(diff(y(1,:))); + +% Set the percentage of the spacing that the largest element should occupy +max_fill = 0.6; + +% Determine if x or y fitting is the limiting factor for the plot and set +% the scaling accordingly +scale_factor = min(xspacing/xrange, yspacing/yrange)*max_fill; + +% Multiply all the circles by the scale factor +circles = cellfun(@(u)u*scale_factor,circles,'UniformOutput',false); + +if exist('crosses','var') + crosses = cellfun(@(u)u*scale_factor,crosses,'UniformOutput',false); +end + +% Recenter the circles +circles = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w],circles,num2cell(x),num2cell(y),'UniformOutput',false); + +if exist('crosses','var') + crosses = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w],crosses,num2cell(x),num2cell(y),'UniformOutput',false); +end + +%%%%%%%%%%%%%%%%% +% Fill in default values for plot options + +% Ensure plot options exists +if ~exist('plot_options','var') + + plot_options = {}; + +end + + +% Color +if ~any(strcmpi('edgecolor',plot_options(1:2:end))) + + plot_options = [plot_options,{'EdgeColor','k'}]; + +end +if ~any(strcmpi('facecolor',plot_options(1:2:end))) + + plot_options = [plot_options,{'FaceColor','none'}]; + +end + +% Parent +if isempty(plot_options) || ~any(strmatch('parent',lower(plot_options(1:2:end)))) + + f = figure; + ax = axes('Parent',f); + plot_options = [plot_options,{'Parent',ax}]; + +end + +%%%%%%%%%%%%%%%%%% +% Make the ellipses + +switch style + + case 'tissot' + + h = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),plot_options{:}),circles,'UniformOutput',false); + + case 'tissot-cross' + + h_cross = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),plot_options_crosses{:}),crosses); + + h_ellipse = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),plot_options{:}),circles); + + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/calc_coriolis_matrix.m b/ProgramFiles/GaitOptimization/calc_coriolis_matrix.m new file mode 100644 index 00000000..2b675e21 --- /dev/null +++ b/ProgramFiles/GaitOptimization/calc_coriolis_matrix.m @@ -0,0 +1,23 @@ +function C = calc_coriolis_matrix(dM,shape,dshape) + % Start with (dM_alpha/dalpha*qdot)*qdot terms + C_temp = zeros(length(shape)); + if isa(dM{1},'sym') + C_temp = sym(C_temp); + end + for i = 1:length(dM) + C_temp = C_temp + dM{i}*dshape(i); + end + C = C_temp*dshape(:); + % Add on the (-1/2)*qdot'*dM_alpha/dalpha*qdot terms + C_temp = zeros(length(shape),1); + if isa(dM{1},'sym') + C_temp = sym(C_temp); + end + for i = 1:length(dM) + C_temp(i) = -(1/2)*dshape(:)'*dM{i}*dshape(:); + end + C = C + C_temp; + if isa(dM{1},'sym') + C = simplify(C); + end +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/gait_gui_optimize.m b/ProgramFiles/GaitOptimization/gait_gui_optimize.m index 7ae91a31..21b8ffd6 100644 --- a/ProgramFiles/GaitOptimization/gait_gui_optimize.m +++ b/ProgramFiles/GaitOptimization/gait_gui_optimize.m @@ -3,6 +3,9 @@ function gait_gui_optimize(hAx,hObject,eventdata,handles) % Load the sysplotter configuration information load sysplotter_config +optimization_handles = handles; +handles = handles.main_gui; + % Upsample and plot to show gait to user system_index = get(handles.systemmenu,'Value'); @@ -15,137 +18,265 @@ function gait_gui_optimize(hAx,hObject,eventdata,handles) current_shch = shch_names{shch_index}; +stretch = get(handles.stretchmenu,'Value')-1; + % insert warning/adjustment if file doesn't exist here. % save(fullfile([sysplotter_inputpath '\Shape_Changes'],paramfilename),'alpha1','alpha2','t'); +% Path to the shape change definition g1 = fullfile(shchpath,strcat(current_shch(7:end),'.mat')); + +% Path to the output of running the shape change through the system g2 = fullfile(datapath,strcat(current_system,'__',current_shch,'.mat')); + +% If the output of running the gait exists if exist(g2,'file') == 2 - load(g2); + + % Load the path data from the output file + load(g2,'p','s'); + + % Get the number of dimensions from the size of the system's shape + % space n_dim=length(s.vecfield.eval.content.Avec_optimized(1,:)); + + if numel(p.phi_fun_full) == 1 t = p.time_full{1,1}; - a12 = p.phi_locus_full{1,1}.shape; - if n_dim>=2 - alpha1 = a12(:,1); - alpha2 = a12(:,2); - end - if n_dim>=3 - alpha3 = a12(:,3); - end - if n_dim>=4 - alpha4 = a12(:,4); - end -elseif exist(g1,'file') == 2 - load(g1); - if exist('alpha4')==1 - n_dim=4; - elseif exist('alpha3')==1 - n_dim=3; - else - n_dim=2; - end + alpha = p.phi_locus_full{1,1}.shape; +% if n_dim>=2 +% alpha1 = a12(:,1); +% alpha2 = a12(:,2); +% end +% if n_dim>=3 +% alpha3 = a12(:,3); +% end +% if n_dim>=4 +% alpha4 = a12(:,4); +% end else error('Selected gait file has more than one cycle. Optimizer only works on single cycles.') end +else + error('Execute the gait on the system before optimizing') end -if n_dim==2 - % Interpolating to increase number of points forming the gait - endslope1 = (alpha1(2)-alpha1(end-1))/(t(end)-t(end-2)); - endslope2 = (alpha2(2)-alpha2(end-1))/(t(end)-t(end-2)); - spline_alpha1 = spline(t,[endslope1;alpha1(:);endslope1]); - spline_alpha2 = spline(t,[endslope2;alpha2(:);endslope2]); - period = 2*pi; - n_plot = 100; - t_plot = linspace(0,period,n_plot+1); - alpha1_plot = ppval(spline_alpha1,t_plot); - alpha2_plot = ppval(spline_alpha2,t_plot); -elseif n_dim==3 - endslope1 = (alpha1(2)-alpha1(end-1))/(t(end)-t(end-2)); - endslope2 = (alpha2(2)-alpha2(end-1))/(t(end)-t(end-2)); - endslope3 = (alpha3(2)-alpha3(end-1))/(t(end)-t(end-2)); - spline_alpha1 = spline(t,[endslope1;alpha1(:);endslope1]); - spline_alpha2 = spline(t,[endslope2;alpha2(:);endslope2]); - spline_alpha3 = spline(t,[endslope3;alpha3(:);endslope3]); - period = 2*pi; - n_plot = 100; - t_plot = linspace(0,period,n_plot+1); - alpha1_plot = ppval(spline_alpha1,t_plot); - alpha2_plot = ppval(spline_alpha2,t_plot); - alpha3_plot = ppval(spline_alpha3,t_plot); -elseif n_dim==4 - endslope1 = (alpha1(2)-alpha1(end-1))/(t(end)-t(end-2)); - endslope2 = (alpha2(2)-alpha2(end-1))/(t(end)-t(end-2)); - endslope3 = (alpha3(2)-alpha3(end-1))/(t(end)-t(end-2)); - endslope4 = (alpha4(2)-alpha4(end-1))/(t(end)-t(end-2)); - spline_alpha1 = spline(t,[endslope1;alpha1(:);endslope1]); - spline_alpha2 = spline(t,[endslope2;alpha2(:);endslope2]); - spline_alpha3 = spline(t,[endslope3;alpha3(:);endslope3]); - spline_alpha4 = spline(t,[endslope4;alpha4(:);endslope3]); - period = 2*pi; - n_plot = 100; - t_plot = linspace(0,period,n_plot+1); - alpha1_plot = ppval(spline_alpha1,t_plot); - alpha2_plot = ppval(spline_alpha2,t_plot); - alpha3_plot = ppval(spline_alpha3,t_plot); - alpha4_plot = ppval(spline_alpha4,t_plot); +% % If the output of running the gait does not exist, but the +% elseif exist(g1,'file') == 2 +% load(g1); +% if exist('alpha4')==1 +% n_dim=4; +% elseif exist('alpha3')==1 +% n_dim=3; +% else +% n_dim=2; +% end +% else +% error('Selected gait file has more than one cycle. Optimizer only works on single cycles.') +% end + +% Interpolate gait to increase number of points forming the gait +endslope = (alpha(2,:)-alpha(end-1,:))/(t(end)-t(end-2)); +spline_alpha = spline(t,[endslope;alpha;endslope]'); +period = 2*pi; +n_plot = 100; +t_plot = linspace(0,period,n_plot+1); +alpha_plot = ppval(spline_alpha,t_plot)'; + + + +% if n_dim==2 +% % Interpolating to increase number of points forming the gait +% endslope1 = (alpha1(2)-alpha1(end-1))/(t(end)-t(end-2)); +% endslope2 = (alpha2(2)-alpha2(end-1))/(t(end)-t(end-2)); +% spline_alpha1 = spline(t,[endslope1;alpha1(:);endslope1]); +% spline_alpha2 = spline(t,[endslope2;alpha2(:);endslope2]); +% period = 2*pi; +% n_plot = 100; +% t_plot = linspace(0,period,n_plot+1); +% alpha1_plot = ppval(spline_alpha1,t_plot); +% alpha2_plot = ppval(spline_alpha2,t_plot); +% elseif n_dim==3 +% endslope1 = (alpha1(2)-alpha1(end-1))/(t(end)-t(end-2)); +% endslope2 = (alpha2(2)-alpha2(end-1))/(t(end)-t(end-2)); +% endslope3 = (alpha3(2)-alpha3(end-1))/(t(end)-t(end-2)); +% spline_alpha1 = spline(t,[endslope1;alpha1(:);endslope1]); +% spline_alpha2 = spline(t,[endslope2;alpha2(:);endslope2]); +% spline_alpha3 = spline(t,[endslope3;alpha3(:);endslope3]); +% period = 2*pi; +% n_plot = 100; +% t_plot = linspace(0,period,n_plot+1); +% alpha1_plot = ppval(spline_alpha1,t_plot); +% alpha2_plot = ppval(spline_alpha2,t_plot); +% alpha3_plot = ppval(spline_alpha3,t_plot); +% elseif n_dim==4 +% endslope1 = (alpha1(2)-alpha1(end-1))/(t(end)-t(end-2)); +% endslope2 = (alpha2(2)-alpha2(end-1))/(t(end)-t(end-2)); +% endslope3 = (alpha3(2)-alpha3(end-1))/(t(end)-t(end-2)); +% endslope4 = (alpha4(2)-alpha4(end-1))/(t(end)-t(end-2)); +% spline_alpha1 = spline(t,[endslope1;alpha1(:);endslope1]); +% spline_alpha2 = spline(t,[endslope2;alpha2(:);endslope2]); +% spline_alpha3 = spline(t,[endslope3;alpha3(:);endslope3]); +% spline_alpha4 = spline(t,[endslope4;alpha4(:);endslope3]); +% period = 2*pi; +% n_plot = 100; +% t_plot = linspace(0,period,n_plot+1); +% alpha1_plot = ppval(spline_alpha1,t_plot); +% alpha2_plot = ppval(spline_alpha2,t_plot); +% alpha3_plot = ppval(spline_alpha3,t_plot); +% alpha4_plot = ppval(spline_alpha4,t_plot); +% end + +%Direction to optimize: 1-x, 2-y, 3-theta +direction = 0; +if optimization_handles.xbutton.Value + direction = 1; +elseif optimization_handles.ybutton.Value + direction = 2; +elseif optimization_handles.thetabutton.Value + direction = 3; end + +%Cost function to optimize over +costfunction = 'none'; +if optimization_handles.pathlengthmetricbutton.Value + costfunction = 'pathlength metric'; +elseif optimization_handles.torquebutton.Value + costfunction = 'torque'; +elseif optimization_handles.covaccbutton.Value + costfunction = 'covariant acceleration'; +elseif optimization_handles.pathlengthcoordinatebutton.Value + costfunction = 'pathlength coord'; +elseif optimization_handles.accelerationbutton.Value + costfunction = 'acceleration coord'; +elseif optimization_handles.pathlengthmetric2button.Value + costfunction = 'pathlength metric2'; +elseif optimization_handles.powerqualitybutton.Value + costfunction = 'power quality'; +end + +%Sanity check radiobutton selection +if strcmpi(costfunction,'none') + error('Something went wrong with the costfunction selection'); +end + % loading sysf file f=fullfile(datapath,strcat(current_system,'_calc.mat')); -load(f); +load(f,'s'); -if n_dim==2 - % Calling the optimizer - lb=0.95*[s.grid_range(1)*ones(n_plot+1,1);s.grid_range(3)*ones(n_plot+1,1)];%0.9 was points value - ub=0.95*[s.grid_range(2)*ones(n_plot+1,1);s.grid_range(4)*ones(n_plot+1,1)]; - y=optimalgaitgenerator(s,2,n_plot,alpha1_plot,alpha2_plot,lb,ub); - alpha1 = [y(1:100)',y(1)]'; - alpha2 = [y(101:200)',y(101)]'; - t=t_plot; - tnew=t(1):(t(2)-t(1))/4:t(end); - alpha12=interp1(t,alpha1,tnew,'spline'); - alpha22=interp1(t,alpha2,tnew,'spline'); -elseif n_dim==3 - % Calling the optimizer - lb=0.95*[s.grid_range(1)*ones(n_plot+1,1);s.grid_range(3)*ones(n_plot+1,1);s.grid_range(5)*ones(n_plot+1,1)];%0.9 was points value - ub=0.95*[s.grid_range(2)*ones(n_plot+1,1);s.grid_range(4)*ones(n_plot+1,1);s.grid_range(6)*ones(n_plot+1,1)]; - y=optimalgaitgenerator3(s,3,n_plot,alpha1_plot,alpha2_plot,alpha3_plot,lb,ub); - alpha1 = [y(1:100)',y(1)]'; - alpha2 = [y(101:200)',y(101)]'; - alpha3 = [y(201:300)',y(201)]'; - t=t_plot; - tnew=t(1):(t(2)-t(1))/4:t(end); - alpha12=interp1(t,alpha1,tnew,'spline'); - alpha22=interp1(t,alpha2,tnew,'spline'); -elseif n_dim==4 - % Calling the optimizer - lb=0.95*[s.grid_range(1)*ones(n_plot+1,1);s.grid_range(3)*ones(n_plot+1,1);s.grid_range(5)*ones(n_plot+1,1);s.grid_range(7)*ones(n_plot+1,1)];%0.9 was points value - ub=0.95*[s.grid_range(2)*ones(n_plot+1,1);s.grid_range(4)*ones(n_plot+1,1);s.grid_range(6)*ones(n_plot+1,1);s.grid_range(8)*ones(n_plot+1,1)]; - y=optimalgaitgenerator4(s,4,n_plot,alpha1_plot,alpha2_plot,alpha3_plot,alpha4_plot,lb,ub); - alpha1 = [y(1:100)',y(1)]'; - alpha2 = [y(101:200)',y(101)]'; - alpha3 = [y(201:300)',y(201)]'; - alpha4 = [y(301:400)',y(301)]'; - t=t_plot; - tnew=t(1):(t(2)-t(1))/4:t(end); - alpha12=interp1(t,alpha1,tnew,'spline'); - alpha22=interp1(t,alpha2,tnew,'spline'); -end +%%%% Set lower and upper bounds on optimization + +% Get the lower and upper bounds on the grid +lvals = s.grid_range(1:2:end); +uvals = s.grid_range(2:2:end); + +% Get the center of the grid +mvals = (uvals + lvals)/2; + +% Get the lower and upper grid bounds relative to the center of the grid +dlvals = lvals-mvals; +duvals = uvals-mvals; + +% scale the differences by .95 +sdlvals = dlvals * 0.95; +sduvals = duvals * 0.95; + +% Add the scaled differences to the center +slvals = mvals+sdlvals; +suvals = mvals+sduvals; +% Tile these out to a matrix with as many rows as there are time points +lb = 0.95 * repmat(slvals,[(n_plot+1),1]); +ub = 0.95 * repmat(suvals,[(n_plot+1),1]); -% Provide zdata to line if necessary -maxZ = 0; -hAxChildren = get(hAx,'Children'); -if ~isempty(hAxChildren) - for idx = 1:numel(hAxChildren) - if ~isempty(hAxChildren(idx).ZData) - maxZ = max(maxZ,max(hAxChildren(idx).ZData(:))); +% Stack the lower bound values and the upper bound values +lb = lb(:); +ub = ub(:); + +%%%%% Call the optimizer +y = optimalgaitgenerator(s,n_dim,n_plot,alpha_plot,lb,ub,stretch,direction,costfunction,handles); + +% reshape the output and add the start point to the end to close the loop +alpha_out = reshape(y,[numel(y)/n_dim,n_dim]); +alpha_out = [alpha_out;alpha_out(1,:)]; + +% Spline the output +t = t_plot; +t_new = t(1):(t(2)-t(1))/4:t(end); +alpha = interp1(t,alpha_out,t_new,'spline'); +alpha12 = alpha(:,1); +alpha22 = alpha(:,2); + + +% if n_dim==2 +% % Calling the optimizer +% lb=0.95*[s.grid_range(1)*ones(n_plot+1,1);s.grid_range(3)*ones(n_plot+1,1)];%0.9 was points value +% ub=0.95*[s.grid_range(2)*ones(n_plot+1,1);s.grid_range(4)*ones(n_plot+1,1)]; +% % if strcmpi(s.system_type,'drag') +% y=optimalgaitgenerator(s,2,n_plot,alpha1_plot,alpha2_plot,lb,ub,stretch,direction,costfunction,handles); +% % elseif strcmpi(s.system_type,'inertia') +% % % Need to take a subset of the space since the optimizer is too +% % % slow for now; interpolate gait at n_interp points +% % n_interp = 23; +% % y = inertial_optimalgaitgenerator(s,2,n_plot,alpha1_plot,alpha2_plot,lb,ub,n_interp); +% % else +% % error('Unexpected system_type field in struct s.') +% % end +% alpha1 = [y(1:n_plot)',y(1)]'; +% alpha2 = [y(n_plot+1:2*n_plot)',y(n_plot+1)]'; +% t=t_plot; +% tnew=t(1):(t(2)-t(1))/4:t(end); +% alpha12=interp1(t,alpha1,tnew,'spline'); +% alpha22=interp1(t,alpha2,tnew,'spline'); +% elseif n_dim==3 +% % Calling the optimizer +% lb=0.95*[s.grid_range(1)*ones(n_plot+1,1);s.grid_range(3)*ones(n_plot+1,1);s.grid_range(5)*ones(n_plot+1,1)];%0.9 was points value +% ub=0.95*[s.grid_range(2)*ones(n_plot+1,1);s.grid_range(4)*ones(n_plot+1,1);s.grid_range(6)*ones(n_plot+1,1)]; +% y=optimalgaitgenerator3(s,3,n_plot,alpha1_plot,alpha2_plot,alpha3_plot,lb,ub,direction,costfunction,handles); +% alpha1 = [y(1:n_plot)',y(1)]'; +% alpha2 = [y(n_plot+1:2*n_plot)',y(n_plot+1)]'; +% alpha3 = [y(2*n_plot+1:3*n_plot)',y(2*n_plot+1)]'; +% t=t_plot; +% tnew=t(1):(t(2)-t(1))/4:t(end); +% alpha12=interp1(t,alpha1,tnew,'spline'); +% alpha22=interp1(t,alpha2,tnew,'spline'); +% elseif n_dim==4 +% % Calling the optimizer +% lb=0.95*[s.grid_range(1)*ones(n_plot+1,1);s.grid_range(3)*ones(n_plot+1,1);s.grid_range(5)*ones(n_plot+1,1);s.grid_range(7)*ones(n_plot+1,1)];%0.9 was points value +% ub=0.95*[s.grid_range(2)*ones(n_plot+1,1);s.grid_range(4)*ones(n_plot+1,1);s.grid_range(6)*ones(n_plot+1,1);s.grid_range(8)*ones(n_plot+1,1)]; +% y=optimalgaitgenerator4(s,4,n_plot,alpha1_plot,alpha2_plot,alpha3_plot,alpha4_plot,lb,ub,direction,costfunction,handles); +% alpha1 = [y(1:n_plot)',y(1)]'; +% alpha2 = [y(n_plot+1:2*n_plot)',y(n_plot+1)]'; +% alpha3 = [y(2*n_plot+1:3*n_plot)',y(2*n_plot+1)]'; +% alpha4 = [y(3*n_plot+1:4*n_plot)',y(3*n_plot+1)]'; +% t=t_plot; +% tnew=t(1):(t(2)-t(1))/4:t(end); +% alpha12=interp1(t,alpha1,tnew,'spline'); +% alpha22=interp1(t,alpha2,tnew,'spline'); +% end + + +if stretch && isfield(s,'convert') + stretchnames = {'stretch','surface'}; + stretchname = stretchnames{stretch}; + + [x_temp,y_temp,z_temp] = s.convert.(stretchname).old_to_new_points(alpha12,alpha22); +else + % Provide zdata to line if necessary + maxZ = 0; + hAxChildren = get(hAx,'Children'); + if ~isempty(hAxChildren) + for idx = 1:numel(hAxChildren) + if ~isempty(hAxChildren(idx).ZData) + maxZ = max(maxZ,max(hAxChildren(idx).ZData(:))); + end end - end + end + x_temp = alpha12; + y_temp = alpha22; + z_temp = maxZ*ones(size(alpha12)); end -gaitline = line('Parent',hAx,'XData',alpha12,'YData',alpha22,'ZData',maxZ*ones(size(alpha12)),'Color',Colorset.spot,'LineWidth',5); +gaitline = line('Parent',hAx,'XData',x_temp,'YData',y_temp,'ZData',z_temp,'Color',Colorset.spot,'LineWidth',5); %%%% Ask the user for a filename current_dir = pwd; % Remember where we started @@ -160,8 +291,10 @@ function gait_gui_optimize(hAx,hObject,eventdata,handles) sysf_func = str2func(current_system); shch_func = str2func(current_shch); -paramfiledisplaytext = ['Opt: [',sysf_func('name'),'] [',shch_func('name'),'] Xeff']; -paramfiletext = hash(['opt_',current_system(6:end),'_',current_shch(7:end),'_Xeff'],'md5'); +effnames = {'X','Y','Theta'}; +paramfiledisplaytext = ['Opt: [',sysf_func('name'),'] [',shch_func('name'),'] [',costfunction,'] ' effnames{direction} 'eff']; +paramfiletext = hash(['opt_',current_system(6:end),'_',current_shch(7:end),'_',costfunction,'_' effnames{direction} 'eff'],'md5'); +paramfiletext = ['opt_',paramfiletext]; % If the user didn't hit cancel, save the data and create a shchf file that % reads the data and interprets it as a gait. @@ -171,11 +304,27 @@ function gait_gui_optimize(hAx,hObject,eventdata,handles) % save(fullfile(shchpath,paramfilename),'alpha1','alpha2','t') % save(fullfile(shchpath,strcat(paramfilenamebare,'_optimal.mat')),'alpha1','alpha2','t') if n_dim==2 + alpha1 = alpha_out(:,1); + alpha2 = alpha_out(:,2); save(fullfile(shchpath,strcat(paramfiletext,'.mat')),'alpha1','alpha2','t') elseif n_dim==3 + alpha1 = alpha_out(:,1); + alpha2 = alpha_out(:,2); + alpha3 = alpha_out(:,3); save(fullfile(shchpath,strcat(paramfiletext,'.mat')),'alpha1','alpha2','alpha3','t') elseif n_dim==4 + alpha1 = alpha_out(:,1); + alpha2 = alpha_out(:,2); + alpha3 = alpha_out(:,3); + alpha4 = alpha_out(:,4); save(fullfile(shchpath,strcat(paramfiletext,'.mat')),'alpha1','alpha2','alpha3','alpha4','t') +elseif n_dim==5 + alpha1 = alpha_out(:,1); + alpha2 = alpha_out(:,2); + alpha3 = alpha_out(:,3); + alpha4 = alpha_out(:,4); + alpha5 = alpha_out(:,5); + save(fullfile(shchpath,strcat(paramfiletext,'.mat')),'alpha1','alpha2','alpha3','alpha4','alpha5','t') end % Create the file if it doesn't already exist; future work could be diff --git a/ProgramFiles/GaitOptimization/gradientstroke1.m b/ProgramFiles/GaitOptimization/gradientstroke1.m new file mode 100644 index 00000000..7ebba947 --- /dev/null +++ b/ProgramFiles/GaitOptimization/gradientstroke1.m @@ -0,0 +1,79 @@ +function [grad,delta_vee,acc] = gradientstroke1(i,P,r,M,dM,cons,shv,Gamma,acc,Metric) %-----Removed method as fifth input + + + T = P.dt; + % Calculate the Kappa and Gradient of Kappa + [kappa1,kappa2,F, dkappa1,dkappa2] = curvature_calc4(i,r,M,P,cons,Gamma,dM); %-----Removed method + + % Calculate the velocity and Gradient of velocity + [L_i,L_is1,L_is2,L_ip1, dL_i_x,dL_is1_x,dL_i_y,dL_is1_y] = velocity_calc1(i,r,M,dM,shv,cons); + + % Calculate the tangential acceleation + a_i = (L_i - L_is1); + acc(i) = a_i; + a_s1 = L_is1 - L_is2; + a_ip1 = (L_ip1 - L_i); + + % Calculate the gradient of pathlength + grad_L = (dL_i_y + dL_is1_y + dL_i_x + dL_is1_x); + + % Calculate the Gradient for acceleration + grad_ax1 = (dL_i_x+0*dL_i_y - dL_is1_x-0*dL_is1_y); + grad_axp1 = -(dL_i_x+0*dL_i_y); + grad_axs1 = (dL_is1_x+0*dL_is1_y); + + + grada = grad_ax1;%(a_i*grad_ax1 + a_ip1*grad_axp1 + a_s1*grad_axs1); %(grad_ax1 + grad_axp1 + grad_axs1);% grad_ax1 + + % Check type of the cost function whether the velcity is contant or + % variable + %-----Pretty sure Hossein said to only use noncte case-----% +% if strcmp(P.initial_velocity,'cte') +% +% T = 1; +% +% L = (L_is1+L_i)/2; +% +% a_n = kappa2*(L)^2/(T^2); +% a_t = a_i; +% +% delta_vee = ([a_t a_n]*[a_t;a_n])^0.25; +% +% delta_vee4 = ([a_t a_n]*[a_t;a_n])*T; +% +% grad_an = (dkappa2*L^2)/(T^2) + 2*grad_L*kappa2*L/(T^2); +% grad_at = grada; +% +% grad1 = 0.25*2*[grad_at(1) grad_an(1)]*[a_t;a_n]*delta_vee4^(-3/4); +% grad2 = 0.25*2*[grad_at(2) grad_an(2)]*[a_t;a_n]*delta_vee4^(-3/4); +% +% +% grad = [grad1 grad2]; +% +% +% else % variable velocity + + L = (L_is1+L_i)/2; + + a_n = kappa2*(L)^2; + a_t = a_i; + + % calculate the JJ^T + [newJ,~] = torqueJacobian(M,P.M_t,r,i,1,-1); + + delta_vee = ([a_t a_n]*newJ*[a_t;a_n])^0.25; + + delta_vee4 = ([a_t a_n]*newJ*[a_t;a_n]); + + grad_an = dkappa2*L^2 + 2*grad_L*kappa2*L; + grad_at = grada; + + grad1 = 0.25*2*[grad_at(1) grad_an(1)]*newJ*[a_t;a_n]*delta_vee4^(-3/4); + grad2 = 0.25*2*[grad_at(2) grad_an(2)]*newJ*[a_t;a_n]*delta_vee4^(-3/4); + + grad = [grad1 grad2]; + +% end + + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator.m index ed0b7ab4..5db0a293 100644 --- a/ProgramFiles/GaitOptimization/optimalgaitgenerator.m +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator.m @@ -1,924 +1,233 @@ -function y=optimalgaitgenerator(s,dimension,npoints,a1,a2,lb,ub,hAx,hObject,eventdata,handles) -%%%%%%%%%%%%%% -% This function takes an input gait and runs fmincon to find the neareast locally -% optimal gait - -%Inputs: -% -%s: System file which contains the connection vector field, CCF's and -% metric data -%dimension: Indicates the number of shape variables of the system -%n: Number of points used to parametrize the gaits in a direct -% transcription method -% a1: Values of the points forming the gait along the first shape dimension -% a2: Values of the points forming the gait along the second shape dimension -% lb: Lower bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired -% ub: Upper bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired -% -% -% Outputs: -% -% y: Matrix whose values indicate coordinates of points which form the optimal gait -%%%%%%%%%%%% - -n=npoints; -P1(:,1)=a1(1,1:n)'; -P1(:,2)=a2(1,1:n)'; - -%% Finding fourier coeffecients. -% The first step is to go from a direct transcription of the initial gait -% to a fourier based parametrization. -% fa is a cell where the ith element contains the coefficients for the fourier parametrization along the ith direction - -t=1:1:npoints+1; -fa=cell(dimension); -% The bounds ensure the fourier series terms have the right period -options = fitoptions('fourier4'); -options.Lower = [-Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf 2*pi/n]; -options.Upper = -[-Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -2*pi/n]; - -for i=1:1:dimension - fa{i}=fit(t',[P1(:,i);P1(1,i)],'fourier4',options); -end - -%% The next step is to setup the fmincon call. -% y0 is the marix of all fourier series coefficients that describe the -% initial gait -% nonlcon is the function that imposes the constraints that all the points -% stay inside the grid -% outfun is the function that updates the gait on the GUI after every iteration - -A=[]; -b=[]; -Aeq=[]; -beq=[]; - -nu={'a0';'a1';'b1';'a2';'b2';'a3';'b3';'a4';'b4';'w'};% - lb1=[]; - ub1=[]; - -for i=1:dimension - for j=1:length(nu) - y0(j,i)=fa{i}.(nu{j}); - end -end - - options = optimoptions('fmincon','SpecifyObjectiveGradient',true,'Display','iter','Algorithm','sqp','SpecifyObjectiveGradient',true,'CheckGradients',false,'FiniteDifferenceType','central','MaxIter',4000,'MaxFunEvals',20000,'TolCon',10^-2,'OutputFcn', @outfun); - [yf fval exitflag output]=fmincon(@(y) solvedifffmincon(y,s,n,dimension,lb,ub),y0,A,b,Aeq,beq,lb1,ub1,@(y) nonlcon(y,s,n,dimension,lb,ub),options); - - -%% Getting point position values from the result of fmincon -% This section helps us go back to a direct transcription parametrization -% of the optimal gait from a fourier series parametrization. y is a column vector -% that contains coordinates of all points forming the optimized gait - -for i=1:1:n - for j=1:dimension - y1(i,j)=yf(1,j)+yf(2,j)*cos(i*yf(end,j))+yf(3,j)*sin(i*yf(end,j))+yf(4,j)*cos(2*i*yf(end,j))+... - +yf(5,j)*sin(2*i*yf(end,j))+yf(6,j)*cos(3*i*yf(end,j))+yf(7,j)*sin(3*i*yf(end,j))+... - +yf(8,j)*cos(4*i*yf(end,j))+yf(9,j)*sin(4*i*yf(end,j));%+yf(10,j)*cos(5*i*yf(end,j))+yf(11,j)*sin(5*i*yf(end,j));%+yf(12,j)*cos(6*i*yf(end,j))+yf(13,j)*sin(6*i*yf(end,j)); - end -end -y=y1(:); - -%% Uncomment for plotting the optimized gait. Potentially useful while debugging. -% for i=1:n -% xf(i)=y(i); -% yf(i)=y(n+i); -% end -% % -% % for i=1:n` -% % xf(i)=P1(i,1); -% % yf(i)=P1(i,2); -% % end -% % -% figure(11) -% hold on -% plot(xf,yf,'-o') - -end - -function [f,g]=solvedifffmincon(y,s,n,dimension,lb,ub) -%%%%%%%%%%%%% -% This function calculates efficiency (or displacement, if -% that is the objective function) and its gradient with respect to the coefficients obtained -% by the fourier series parametrization -% -% Inputs: -% -% y: Matrix containing the Fourier series coefficients -% s: System file which contains the connection vector field, CCF's and -% metric data -% dimension: Indicates the number of shape variables of the system -% n: The number of points desired in a direct transcription parametrization -% of the gaits -% lb: Lower bound of shape variables for each point which is obtained from the grid inside -% which an optimal gait is desired -% ub: Upper bound of shape variables for each point which is obtained from the grid inside -% which an optimal gait is desired -% -% Outputs: -% -% f: Objective function value (This is negative of efficiency by default, can be -% changed to displacement) -% g: Gradient of the objective function -%%%%%%%%%%%%% - -%% Obtaining points from fourier coefficients -% The first step is to obtain a direct transcription of the gait from the -% fourier series parametrization. y is the matrix of coordinate values of -% the points forming the gait. -afactor=0.001; -coeff=y; -for i=1:1:n - for j=1:dimension - y1(i,j)=y(1,j)+y(2,j)*cos(i*y(end,j))+y(3,j)*sin(i*y(end,j))+y(4,j)*cos(2*i*y(end,j))+... - +y(5,j)*sin(2*i*y(end,j))+y(6,j)*cos(3*i*y(end,j))+y(7,j)*sin(3*i*y(end,j))+... - +y(8,j)*cos(4*i*y(end,j))+y(9,j)*sin(4*i*y(end,j));%+y(10,j)*cos(5*i*y(end,j))+y(11,j)*sin(5*i*y(end,j));%+y(12,j)*cos(6*i*y(end,j))+y(13,j)*sin(6*i*y(end,j));% - end -end -clear y -y=y1; - -%% Calculating cost and displacement per gait -velocityvalues=zeros(n,dimension); -g=1; %assign a time period for executing the gait -p.phi_def = @(t) interp1( linspace(0,g,n+1), [y; y(1,:)], t); % function parametrizing the gait as a function of time - -for i=1:1:n-1 - velocityvalues(i,:)=n*(y(i+1,:)-y(i,:))/g; -end -velocityvalues(n,:)=n*(y(1,:)-y(n,:))/g; -p.dphi_def = @(t) interp1( linspace(0,g,n), [velocityvalues], t); % Shape space velocity as a function of time - - -[net_disp_orig, net_disp_opt, cost] = evaluate_displacement_and_cost1(s,p,[0, g],'interpolated','ODE'); % Call to the function that obtains displacement, cost and efficiency of a gait -lineint=net_disp_opt(1); % displacement produced in the x-direction produced on executing the gait measured in the optimal coordinates -totalstroke=cost; % Cost of executing the gait ones - -% If efficiency is negative, reversing the order of points so that -% efficiency is positive -if lineint<0 - lineint=-lineint; - ytemp=y; - for i=1:n - y(i,:)=ytemp(n+1-i,:); - end - invert=1; -else - lineint=lineint; - invert=0; -end - -%% Preliminaries for gradient calculation -% Preallocating memory for variables which we will need in further -% calculation -yvalues=cell(n,dimension); % Cell representation of the coordinates of all points forming the gait -interpstateccf=cell(1,dimension); % Variable which will store the ccf function grid used for interpolation -interpmetricgrid=cell(1,dimension); % Variable which will store the metric grid used for interpolation -ccf=zeros(n,dimension*(dimension-1)/2); % Variable which will store ccf function at each point -metric1=zeros(n,dimension,dimension);% Variable which will store metric at each point in the form of a matrix -metric = repmat({zeros(dimension)},[n 1]); % Variable which stores the metric at each point in the form of a 2x2 matrix -metricgrad1=zeros(n,dimension,dimension,dimension);% Variable which will store gradient of metric at each point in the form of a matrix -metricgrad = repmat({zeros(dimension)},[n,dimension]);% Variable which will store gradient of metric at each point in the form of a matrix - -% Interpolation to calculate all the variables needed for gradient -% calculation -for i=1:1:n - for j=1:1:dimension - yvalues{i,j}=y(i,j); - end - - for j=1:1:dimension - interpstateccf{j}=s.grid.eval{j,1}; - interpmetricgrid{j}=s.grid.metric_eval{j,1}; - end -end - - -for j=1:dimension*(dimension-1)/2 - ccf(:,j)=interpn(interpstateccf{:},s.DA_optimized{1,j},y(:,1),y(:,2),'spline'); -end - - -for j=1:1:dimension - for k=1:1:dimension - metric1(:,j,k)=interpn(interpmetricgrid{:},s.metricfield.metric_eval.content.metric{j,k},y(:,1),y(:,2),'spline'); - end -end -% metric{i}=eye(2); -for i=1:n - for j=1:1:dimension - for k=1:1:dimension - metric{i}(j,k)=metric1(i,j,k); - end - end -end - -for l=1:1:dimension - for m=1:1:dimension - if m==l - y2(:,m)=y(:,m)+afactor*ones(length(y),1); - y1(:,m)=y(:,m)-afactor*ones(length(y),1); - else - y2(:,m)=y(:,m); - y1(:,m)=y(:,m); - end - end - for j=1:1:dimension - for k=1:1:dimension - metricgrad1(:,l,j,k)=(interpn(interpmetricgrid{:},s.metricfield.metric_eval.content.metric{j,k},y2(:,1),y2(:,2),'spline')-interpn(interpmetricgrid{:},s.metricfield.metric_eval.content.metric{j,k},y1(:,1),y1(:,2),'spline'))/(2*afactor); - end - end - for i=1:n - for j=1:1:dimension - for k=1:1:dimension - metricgrad{i,l}(j,k)=metricgrad1(i,l,j,k); - end - end - end -end - -%l is the vector containing metric weighted distances between neighbouring -%points - -for i=1:1:n-1 - l(i)=sqrt((y(i+1,:)-y(i,:))*((metric{i}+metric{i+1})/2)*(y(i+1,:)-y(i,:))'); -end -l(n)=sqrt((y(1,:)-y(n,:))*((metric{n}+metric{1})/2)*(y(1,:)-y(n,:))'); - -%% Jacobianstroke is the gradient of cost. -%Contrigrad is the contribution to the gradient due to the metric changing - -jacobianstroke = zeros(n,dimension); -contrigrad=zeros(n,dimension); - -for i=1:n-1 - delp{i}=y(i+1,:)-y(i,:); % delp{i} is the vector joining the (i+1)th point to the ith point -end -delp{n}=y(1,:)-y(n,:); - -for i=2:n-1 - for j=1:dimension - %Contrigrad is the contribution to the gradient due to the metric changing - contrigrad(i,j)=0.5*delp{i}*metricgrad{i,j}*delp{i}'/(2*l(i))+0.5*delp{i-1}*metricgrad{i,j}*delp{i-1}'/(2*l(i-1)); - end - % Total gradient is the result of distance changing due to movement of point and the metric changing due to movement of the point - jacobianstroke(i,:)=(-(((metric{i}+metric{i+1})/2)*delp{i}')'-(delp{i}*((metric{i}+metric{i+1})/2)))/(2*l(i))+... - +((((metric{i-1}+metric{i})/2)*delp{i-1}')'+(delp{i-1}*((metric{i}+metric{i-1})/2)))/(2*l(i-1))+contrigrad(i,:); -end - -% Calculation for the 1st point and last point have to be done outside the -% loop as the (i+1)th point for the last point is the first point and -% (i-1)th point for the first point is the last point -for j=1:dimension - contrigrad(1,j)=0.5*delp{1}*metricgrad{1,j}*delp{1}'/(2*l(1))+0.5*delp{n}*metricgrad{1,j}*delp{n}'/(2*l(n)); -end -jacobianstroke(1,:)=(-(((metric{1}+metric{2})/2)*delp{1}')'-(delp{1}*((metric{1}+metric{2})/2)))/(2*l(1))+... - +((((metric{n}+metric{1})/2)*delp{n}')'+(delp{n}*((metric{n}+metric{1})/2)))/(2*l(n))+contrigrad(1,:); - -for j=1:dimension - contrigrad(n,j)=0.5*delp{n}*metricgrad{n,j}*delp{n}'/(2*l(n))+0.5*delp{n-1}*metricgrad{n,j}*delp{n-1}'/(2*l(n-1)); -end -jacobianstroke(n,:)=(-(((metric{n}+metric{1})/2)*delp{n}')'-(delp{n}*((metric{n}+metric{1})/2)))/(2*l(n))+... - +((((metric{n}+metric{n-1})/2)*delp{n-1}')'+(delp{n-1}*((metric{n}+metric{n-1})/2)))/(2*l(n-1))+contrigrad(n,:); - - -%% Jacobiandisp is the gradient of displacement. -% jacobiandispcalculator3 is the function that calculates the gradient of -% displacement for the ith point. It's input arguments are the coordinates of -% the (i-1)th, ith and (i+1)th point, CCF value at point i and the dimension of -% the shape space (dimension) - -jacobiandisp = zeros(n,dimension); -for i=2:1:n-1 - jacobiandisp(i,:)=jacobiandispcalculator3(y(i-1,:),y(i,:),y(i+1,:),ccf(i,:),dimension); -end -jacobiandisp(1,:)=jacobiandispcalculator3(y(n,:),y(1,:),y(2,:),ccf(1,:),dimension); -jacobiandisp(n,:)=jacobiandispcalculator3(y(n-1,:),y(n,:),y(1,:),ccf(n,:),dimension); - -%% Jacobianeqi is the concentration gradient. -%It is the term that keeps points eqi distant from each other and prevents crossover of gait. - -jacobianeqi = zeros(n,dimension); -for i=2:n-1; - len=sqrt((y(i+1,:)-y(i-1,:))*((metric{i-1}+metric{i+1})/2)*(y(i+1,:)-y(i-1,:))'); % metric weighted length between point (i-1) and (i+1) - midpoint=y(i-1,:)+((y(i+1,:)-y(i-1,:))*sqrtm((metric{i-1}+metric{i+1})/2))/2; % location of midpoint of the line segment joining point (i-1) and (i+1) - betacos=(y(i+1,:)-y(i-1,:))*sqrtm((metric{i-1}+metric{i+1})/2)*((y(i,:)-y(i-1,:))*sqrtm((metric{i-1}+metric{i})/2))'/(l(i-1)*len); - xhat=y(i-1,:)+(y(i+1,:)-y(i-1,:))*sqrtm((metric{i-1}+metric{i+1})/2)*l(i-1)*betacos/len; %projection of ith point onto the line joining the (i-1)th and (i+1)th points - jacobianeqi(i,:)=midpoint-xhat; % gradient of the ith point is equal to the difference between the midpoint and the projection of ith point -end - - len=sqrt((y(2,:)-y(n,:))*((metric{2}+metric{n})/2)*(y(2,:)-y(n,:))'); - midpoint=y(n,:)+((y(2,:)-y(n,:))*sqrtm((metric{n}+metric{2})/2))/2; - betacos=(y(2,:)-y(n,:))*sqrtm((metric{n}+metric{2})/2)*((y(1,:)-y(n,:))*sqrtm((metric{n}+metric{1})/2))'/(l(n)*len); - xhat=y(n,:)+(y(2,:)-y(n,:))*sqrtm((metric{n}+metric{2})/2)*l(n)*betacos/len; - jacobianeqi(1,:)=midpoint-xhat; - - len=sqrt((y(1,:)-y(n-1,:))*((metric{1}+metric{n-1})/2)*(y(1,:)-y(n-1,:))'); - midpoint=y(n-1,:)+((y(1,:)-y(n-1,:))*sqrtm((metric{1}+metric{n-1})/2))/2; - betacos=(y(1,:)-y(n-1,:))*sqrtm((metric{n-1}+metric{1})/2)*((y(n,:)-y(n-1,:))*sqrtm((metric{n-1}+metric{n})/2))'/(l(n-1)*len); - xhat=y(n-1,:)+(y(1,:)-y(n-1,:))*sqrtm((metric{n-1}+metric{1})/2)*l(n-1)*betacos/len; - jacobianeqi(n,:)=midpoint-xhat; - -%% changey/dcoeff tells us how much each point moves when a fourier series variable is changed -% chy is a cell with as many entries as the dimension of the shape space -% ith element of chy is a matrix where the (j,k)th entry tells us the change in the ith coordinate -% of the kth point of the gait resulting from a unit change in the jth -% fourier coefficient corresponding to the ith dimension of the shape space - -chy=cell(dimension,1); -for i=1:1:dimension - for j=1:1:n - chy{i}(:,j)=[1;cos(j*coeff(end,i));sin(j*coeff(end,i));cos(2*j*coeff(end,i));sin(2*j*coeff(end,i));cos(3*j*coeff(end,i));sin(3*j*coeff(end,i));cos(4*j*coeff(end,i));sin(4*j*coeff(end,i))];%cos(5*j*coeff(end,i));sin(5*j*coeff(end,i))];%;cos(6*j*coeff(end,i));sin(6*j*coeff(end,i))];% - end -end - - - -%% properly ordering gradients depending on wether lineint was negative or positive -if invert==0 - jacobiandisptemp=jacobiandisp; - jacobianstroketemp=jacobianstroke; - jacobianeqitemp=jacobianeqi; - jacobiandisp=jacobiandisp; - jacobianstroke=jacobianstroke; - jacobianeqi=jacobianeqi; -else - jacobiandisptemp=jacobiandisp; - jacobianstroketemp=jacobianstroke; - jacobianeqitemp=jacobianeqi; - for i=1:1:n - jacobiandisp(i,:)=jacobiandisptemp(n+1-i,:); - jacobianstroke(i,:)=jacobianstroketemp(n+1-i,:); - jacobianeqi(i,:)=jacobianeqitemp(n+1-i,:); - - end -end - -%% fourier series version of all gradients - -% The line below calculates the total gradient in a direct transcription -% parametrization -totaljacobian=jacobiandisp/totalstroke-lineint*jacobianstroke/totalstroke^2+jacobianeqi; - -% We then obtain gradients in a fourier series parametrization by -% projecting the gradients from the direct transcription space onto the -% fourier coefficient space -for i=1:1:dimension - for j=1:1:9 - jacobiandispfourier(j,i)=chy{i}(j,:)*jacobiandisp(:,i); - jacobianstrokefourier(j,i)=chy{i}(j,:)*jacobianstroke(:,i); - jacobianeqifourier(j,i)=chy{i}(j,:)*jacobianeqi(:,i); - totaljacobianfourier(j,i)=chy{i}(j,:)*totaljacobian(:,i); - end -end - - - - - - -%% Variables useful while debugging for flaws in gradient calculations - -% for i=1:n -% for j=1:1:dimension -% totaljacobianc(i,j)=chy{j}(:,i)'*totaljacobianfourier(:,j); -% jacobiandispc(i,j)=chy{j}(:,i)'*jacobiandispfourier(:,j); -% jacobianstrokec(i,j)=chy{j}(:,i)'*jacobianstrokefourier(:,j); -% end -% end -% totaljacobianctemp=totaljacobianc; -% jacobiandispctemp=jacobiandispc; -% jacobianstrokectemp=jacobianstrokec; -% for i=1:1:n -% jacobiandispc(i,:)=jacobiandispctemp(n+1-i,:); -% jacobianstrokec(i,:)=jacobianstrokectemp(n+1-i,:); -% totaljacobianc(i,:)=totaljacobianctemp(n+1-i,:); -% end - -%% minimizing negative of efficiency(or displacement) - f=-lineint/(totalstroke); -% f=-lineint; -if nargout>1 -% g=-totaljacobian(:); - g=[-totaljacobianfourier;zeros(1,dimension)]; -end - -%% Debugging and plotting -% This section was written up for the sole purpose of helping with -% debugging flaws in the optimizer. Appropriate sections can be uncommented -% to plot how the different gradients look during the optimization process. - -% for i=1:n -% G(i)=y(i,1); -% H(i)=y(i,2); -% P(i)=y(i,3); -% I(i)=1*jacobianstroketemp(i,1); -% J(i)=1*jacobianstroketemp(i,2); -% I(i)=1*jacobianstroke(i,1); -% J(i)=1*jacobianstroke(i,2); -% N(i)=1*jacobianstroke(i,3); -% K(i)=jacobiandisptemp(i,1); -% K(i)=jacobiandisp(i,1); -% K1(i) = jacobiandispc(i,1); -% K1(i)=jacobianforward(i,1); -% L(i)=jacobiandisptemp(i,2); -% L(i)=jacobiandisp(i,2); -% L1(i)=jacobiandispc(i,2); -% L1(i)=jacobianforward(i,2); -% Q(i)=jacobiandisp(i,3); -% B(i)=totaljacobianc(i,1); -% B1(i)=totaljacobian(i,1); -% C(i)=totaljacobianc(i,2); -% C1(i)=totaljacobian(i,2); -% D(i)=totaljacobian(i,3); -% O(i)=jacobianeqi(i,1); -% S(i)=jacobianeqi(i,2); -% R(i)=jacobianeqi(i,3); -% ccfx(i)=ccf(i,1); -% ccfy(i)=ccf(i,2); -% ccfz(i)=ccf(i,3); -% ccfcrosscheck(i)=ccf(i,:)*jacobiandisp(i,:)'; -% end -% % % % -% % % % -% clf(figure(6)) %%jacobiandisp -% figure(6) -% scale=0; -% scale1=1; -% quiver(G,H,K,L,scale1) -% hold on -% quiver(G,H,K1,L1,scale1) -% % quiver(G,H,ccfx,ccfy,scale1) -% plot(G,H) -% axis equal -% hold off -% -% % clf(figure(9)) %%jacobianforward -% % figure(9) -% % scale=0; -% % scale1=1; -% % quiver(G,H,K1,L1,scale1) -% % hold on -% % % quiver(G,H,P,ccfx,ccfy,ccfz,scale1) -% % plot(G,H) -% % axis equal -% % hold off -% % -% -% -% % % -% clf(figure(7)) %%jacobianstroke -% figure(7) -% scale=0; -% quiver(G,H,K,L,scale) -% quiver(G,H,I,J,scale1) -% hold on -% plot(G,H) -% axis equal -% hold off -% % % % -% clf(figure(8)) %%% totaljacobian -% figure(8) -% scale=0; -% quiver(G,H,B,C,scale1) -% hold on -% quiver(G,H,B1,C1,scale1) -% -% plot(G,H) -% axis equal -% hold off -% % -% % clf(figure(3)) %%%jacobianeqi -% % figure(3) -% % scale1=1; -% % quiver3(G,H,P,O,S,R,scale1) -% % hold on -% % plot3(G,H,P) -% % axis equal -% % hold off -% % -% % -% % % figure(5) -% % % scale=0; -% % % quiver(G,H,B1,C1,scale1) -% % % hold on -% % % plot(G,H) -% % % axis equal -% % % hold off -% % min(abs(totaljacobian)) -% % max(abs(totaljacobian)) -% % pause(0.1) -% -% % -% % for i=1:1:length(ccf) -% % ccfcheckper(i)=(ccfcheck(i)-ccf(i))/ccf(i); -% % end -% -% figure(5) -% plot(y(:,1),y(:,2)) -% axis equal -% -% pause(0.1) - - - -end - -function a=jacobiandispcalculator3(p1,p2,p3,ccf,dimension) -%%%%%%%%% -% -% jacobiandispcalculator3 is the function that calculates the gradient of -% displacement for the ith point. -% It's input arguments are the coordinates of the (i-1)th, ith and (i+1)th point, -% CCF value at point i(ccf) and the dimension of the shape space (dimension) -% -%%%%%%%%% - -l1=0; % variable for calculating length of the line segment joining the (i-1)th point with the (i+1)th point -for i=1:1:dimension - l1=l1+(p1(i)-p3(i))^2; - base(1,i)=p3(i)-p1(i); % vector connecting the (i-1)th point and (i+1)th point -end -l=sqrt(l1); % length of the line segment joining the (i-1)th point with the (i+1)th point - -for i=1:1:dimension - jacobian(1,i)=0; - perp1=zeros(1,dimension); - perp1(i)=1; - %parcomp=base*perp1'/norm(base); - %perp1-parcomp*base/norm(base); %%recheck again - perp=perp1;% Unit vector along the ith direction - % The for loop below calculates the gradient along the ith direction by - % treating the CCF as 2 forms. A specific (j,k) represents a component of the 2 form - for j=1:dimension-1 - for k=1:dimension-j - veca=zeros(1,dimension); - vecb=zeros(1,dimension); - veca(j)=1; - vecb(j+k)=1; - f=(j-1)*dimension-(j*(j-1))/2+k; - jacobian(1,i)=jacobian(1,i)+0.5*ccf(f)*((veca*perp')*(vecb*base')-(vecb*perp')*(veca*base')); - end - end -end - -a=jacobian; - -end - -function [A,Aeq]=nonlcon(y,s,n,dimension,lb,ub) -%%%%%%%%% -% -%This function imposes the nonlinear constraint that all the points forming the gait stay within bounds -% -%Inputs: -% -%y: Fourier series coefficients that describe the gait -%s: System file which contains the connection vector field, CCF's and -% metric data -%n: Number of points used to parametrize the gaits in a direct -% transcription method -%dimension: Indicates the number of shape variables of the system -%lb: Lower bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired -%ub: Upper bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired -% -%%%%%%%%% - -% The first step is to obtain a direct transciption parametrization of the gait from the -% fourier series parametrization -for i=1:1:n+1 - for j=1:dimension - y1(i,j)=y(1,j)+y(2,j)*cos(i*y(end,j))+y(3,j)*sin(i*y(end,j))+y(4,j)*cos(2*i*y(end,j))+... - +y(5,j)*sin(2*i*y(end,j))+y(6,j)*cos(3*i*y(end,j))+y(7,j)*sin(3*i*y(end,j))+... - +y(8,j)*cos(4*i*y(end,j))+y(9,j)*sin(4*i*y(end,j));%+y(10,j)*cos(5*i*y(end,j))+y(11,j)*sin(5*i*y(end,j));%+y(12,j)*cos(6*i*y(end,j))+y(13,j)*sin(6*i*y(end,j)); - end -end - -y2=y1(:); - -b=length(y2); - - -% A1 and A2 together impose the constraint that all the points forming the gait stay in bounds -A1=y2+lb; -A2=-y2-ub; - -A=[A1;A2]; - -% Aeq ensures that fmincon doesn't alter the period of fourier series -% components -Aeq=y(end,:)-2*pi/n*ones(size(y(end,:))); - -end - -function stop=outfun(y,optimValues,state) -%%%%%%%%% -% -%This function plots the current state of the gait on the sysplotter GUI -%after every iteration of the optimizer -% -%%%%%%%%% - -n=100; -dimension=length(y(1,:)); - -% The if else statement below deletes gaits 2 iterations after they have been plotted -if optimValues.iteration>2 - children=get(gca,'children'); - delete(children(6:10)); -else -end - -% The if else statement below fades the gait plotted during the previous iteration -if optimValues.iteration>1 - children=get(gca,'children'); - children(1).Color=[0.5 0.5 0.5]; - children(2).Color=[0.5 0.5 0.5]; - children(3).Color=[0.5 0.5 0.5]; - children(4).Color=[0.5 0.5 0.5]; - children(5).Color=[0.5 0.5 0.5]; - - children(1).LineWidth=4; - children(2).LineWidth=4; - children(3).LineWidth=4; - children(4).LineWidth=4; - children(5).LineWidth=4; -else -end - -% The if else statement below plots the gait after every iteration -if optimValues.iteration>0 - for i=1:1:n+1 - for j=1:dimension - y1(i,j)=y(1,j)+y(2,j)*cos(i*y(end,j))+y(3,j)*sin(i*y(end,j))+y(4,j)*cos(2*i*y(end,j))+... - +y(5,j)*sin(2*i*y(end,j))+y(6,j)*cos(3*i*y(end,j))+y(7,j)*sin(3*i*y(end,j))+... - +y(8,j)*cos(4*i*y(end,j))+y(9,j)*sin(4*i*y(end,j));%+y(10,j)*cos(5*i*y(end,j))+y(11,j)*sin(5*i*y(end,j));%+y(12,j)*cos(6*i*y(end,j))+y(13,j)*sin(6*i*y(end,j)); - end - end - hold on - handle1=plot(y1(:,1),y1(:,2),'k','linewidth',3); - plot_dir_arrows(y1(:,1),y1(:,2),2,'Color',[0 0 0],'LineWidth',3); -else -end -pause(0.1) -stop=false; -end - -function [net_disp_orig, net_disp_opt, cost] = evaluate_displacement_and_cost1(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_def" and "dphi_def", 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_def(0); - startshapelist = num2cell(startshape); - beta_theta = interpn(s.grid.eval{:},s.B_optimized.eval.Beta{3},startshapelist{:},'spline'); - 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_def(0); - startshapelist = num2cell(startshape); - beta_theta = interpn(s.grid.eval{:},s.B_optimized.eval.Beta{3},startshapelist{:},'spline'); - 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_def(t); - shapelist = num2cell(shape); - dshape = gait.dphi_def(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{:},'spline'),s.vecfield.eval.content.Avec); - - M = cellfun(@(C) interpn(s.grid.metric_eval{:},C,shapelist{:},'spline'),s.metricfield.metric_eval.content.metric); - - otherwise - error('Unknown method for evaluating local connection'); - end - - % Get the body velocity at the current time - t; - 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 - -function [g_end_orig,g_end_opt, cost_end] = extract_displacement_and_cost(datafile) -% Extract the displacement and cost data from a sysf_...shchf_....mat file - -% Load the target file -load(datafile,'p') - -% Prime arrays to hold the net displacement (in original and optimal -% coordinates) and cost from each shape change in the file. p.G_locus_full is -% single-level cell array of structures, each of which holds the -% information for one gait (with all segments concatenated) -g_end_orig = zeros(numel(p.G_locus_full),3); -g_end_opt = g_end_orig; -cost_end = zeros(numel(p.G_locus_full,1)); % If distance metric was not specified, euclidean metric in the parameters was assumed - -% Loop over each shape change -for i = 1:numel(p.G_locus_full) - - % Extract the final values for the relevant parameters - g_end_orig(i,:) = p.G_locus_full{i}.G(end,:); - g_end_opt(i,:) = p.G_locus_full{i}.G_opt(end,:); - cost_end(i) = p.G_locus_full{i}.S(end); -end -end \ No newline at end of file +function y=optimalgaitgenerator(s,dimension,npoints,a,lb,ub,stretch,direction,costfunction,handles) +%%%%%%%%%%%%%% +% This function takes an input gait and runs fmincon to find the neareast locally +% optimal gait + +%Inputs: +% +%s: System file which contains the connection vector field, CCF's and +% metric data +%dimension: Indicates the number of shape variables of the system +%n: Number of points used to parametrize the gaits in a direct +% transcription method +% a: n x m array, for n waypoints in m dimensions +% lb: Lower bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired +% ub: Upper bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired +% direction: Direction to optimize travel: 1=x,2=y,3=theta +% costfunction: costfunction type to optimize for +% Options: Path length (metric), torque squared (metric), +% covariant acceleration (metric), path length (coordinate), +% acceleration (coordinate) +% +% +% Outputs: +% +% y: Matrix whose values indicate coordinates of points which form the optimal gait +%%%%%%%%%%%% + +% n=npoints; +% for i=1:1:npoints +% a1(1,i)=1*cos(2*pi*(i-1)/n); +% a2(1,i)=1*cos(2*pi*(i-1)/n+pi/2); +% end + +% n=npoints; +% P1(:,1)=a1(1,1:n)'; +% P1(:,2)=a2(1,1:n)'; +% P1(end+1,:) = P1(1,:); % Close the loop on the gait + +% For minimal refactoring, mapping a -> P1 +P1 = a; + +% % Close the loop of the gait if necessary +% if P1(end,:) ~= P1(1,:) +% P1(end+1,:) = P1(1,:); +% end + + +%% Finding fourier coeffecients. +% The first step is to go from a direct transcription of the initial gait +% to a fourier based parametrization. +% fa is a cell where the ith element contains the coefficients for the fourier parametrization along the ith direction + +% Time period of gait is 1 second in order to handle calculations performed +% for inertial gait optimization +t = linspace(0,1,size(P1,1)); + +fa=cell(dimension); +% The bounds ensure the fourier series terms have the right period + % Bound only the frequency to be 2*pi, such that period = 1 +options = fitoptions('fourier4'); +options.Lower = [-Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf 2*pi]; +options.Upper = -[-Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -Inf -2*pi]; + +for i=1:1:dimension + fa{i}=fit(t',P1(:,i),'fourier4',options); +end + +%% The next step is to setup the fmincon call. +% y0 is the marix of all fourier series coefficients that describe the +% initial gait +% nonlcon is the function that imposes the constraints that all the points +% stay inside the grid +% outfun is the function that updates the gait on the GUI after every iteration + +A=[]; +b=[]; +Aeq=[]; +beq=[]; + +nu={'a0';'a1';'b1';'a2';'b2';'a3';'b3';'a4';'b4';'w'};% + lb1=[]; + ub1=[]; + + y0 = zeros(length(nu),dimension); +for i=1:dimension + for j=1:length(nu) + y0(j,i)=fa{i}.(nu{j}); + end +end + +writerObj = []; +% % Uncomment this section if you'd like to record a video of the +% % optimizer's steps in the shape space +% writerObj = VideoWriter('cost_as_time_period_circle_start.mp4','MPEG-4'); +% writerObj.FrameRate = 5; +% % set the seconds per image +% % open the video writer +% open(writerObj); +% figure(5); +% subplot(1,2,1) +% contour(s.grid.eval{1},s.grid.eval{2},s.DA_optimized{1},'LineWidth',1.5) +% axis square +% hold on + +s.costfunction = costfunction; +global bestCost bestDisp bestEff; +bestEff = 0; + +%Suppress warning for annoying thing in jacobianeqicalculator +warning('off','MATLAB:sqrtm:SingularMatrix'); + +try + options = optimoptions('fmincon','SpecifyObjectiveGradient',true,'Display','iter','Algorithm','sqp','CheckGradients',false,'FiniteDifferenceType','central','MaxIter',4000,'MaxFunEvals',20000,'TolCon',10^-3,'StepTolerance',1e-6,'OutputFcn', @(y,optimValues,state) outfun(y,optimValues,state,stretch,s,handles)); +catch + error('This code requires the global optimization toolbox to run') +end + + +objective_function_gradient = @(y) solvedifffmincon(y,s,npoints,dimension,direction,lb,ub,writerObj); +constraint_function = @(y) nonlcon(y,s,npoints,dimension,lb,ub,direction); + + [yf, ~,~,~]=fmincon(objective_function_gradient,y0,A,b,Aeq,beq,lb1,ub1,constraint_function,options); + +% % Uncomment this if you uncommented the section above so that the video +% % writer object is closed appropriately. +% close(writerObj); + +printstuff = 1; +if printstuff + disp(['Optimal Efficiency: ',num2str(bestEff)]); + disp(['Optimal Displacement: ',num2str(bestDisp)]); + disp(['Optimal Cost: ',num2str(bestCost)]); +end + +%% Getting point position values from the result of fmincon +% This section helps us go back to a direct transcription parametrization +% of the optimal gait from a fourier series parametrization. y is a column vector +% that contains coordinates of all points forming the optimized gait + +y1 = path_from_fourier(yf,npoints,dimension); +% path_from_fourier returns a self-connected gait, so remove the last point +% to give what optimalgaitgenerator expects to return +y1 = y1(1:end-1,:); +y=y1(:); + +%% Uncomment for plotting the optimized gait. Potentially useful while debugging. +% for i=1:n +% xf(i)=y(i); +% yf(i)=y(n+i); +% end +% % +% % for i=1:n` +% % xf(i)=P1(i,1); +% % yf(i)=P1(i,2); +% % end +% % +% figure(11) +% hold on +% plot(xf,yf,'-o') + +end + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +% function [g_end_orig,g_end_opt, cost_end] = extract_displacement_and_cost(datafile) +% % Extract the displacement and cost data from a sysf_...shchf_....mat file +% +% % Load the target file +% load(datafile,'p') +% +% % Prime arrays to hold the net displacement (in original and optimal +% % coordinates) and cost from each shape change in the file. p.G_locus_full is +% % single-level cell array of structures, each of which holds the +% % information for one gait (with all segments concatenated) +% g_end_orig = zeros(numel(p.G_locus_full),3); +% g_end_opt = g_end_orig; +% cost_end = zeros(numel(p.G_locus_full,1)); % If distance metric was not specified, euclidean metric in the parameters was assumed +% +% % Loop over each shape change +% for i = 1:numel(p.G_locus_full) +% +% % Extract the final values for the relevant parameters +% g_end_orig(i,:) = p.G_locus_full{i}.G(end,:); +% g_end_opt(i,:) = p.G_locus_full{i}.G_opt(end,:); +% cost_end(i) = p.G_locus_full{i}.S(end); +% end +% end + + + + + + + + + + + + + + + + + diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/acceleration_cost.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/acceleration_cost.m new file mode 100644 index 00000000..18843575 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/acceleration_cost.m @@ -0,0 +1,16 @@ +function dcost = acceleration_cost(M,dM,shape,dshape,ddshape,metric) +% Calculates the incremental cost for an inertial system where cost is covariant acceleration. +% Inputs: +% M: Mass matrix +% dM_alphadalpha: Partial of mass matrix with respect to shape variables; +% must be cell where the ith entry is the partial with respect to the +% ith shape variable +% shape: Current shape configuration of system, at which M and +% dM_alphadalpha were evaluated +% dshape: Current shape velocity of system +% ddshape: Current shape acceleration of system + C = calc_coriolis_matrix(dM,shape,dshape); + cov_acc = ddshape(:) + inv(M)*C; + dcost = cov_acc'*metric*cov_acc; + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/acceleration_gradient_helper.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/acceleration_gradient_helper.m new file mode 100644 index 00000000..45c99947 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/acceleration_gradient_helper.m @@ -0,0 +1,121 @@ +function del_cost = acceleration_gradient_helper(t,X,s,gait,grad_alpha,grad_alphadot,grad_alphaddot,metric,dM,ddM) +% Helper function to calculate the gradient of covariant acceleration cost +% Designed to work with ode45; solves for the gradient of cost at an +% instant of time t. +% Inputs: +% t: Time period at which gradient of cost is being evaluated. +% X: Unused, required by ode45 +% s: system struct used by sysplotter +% gait: Struct containing fields: +% phi_def: array function that returns shape at time value t +% dphi_def: array function that returns shape velocity at time t +% ddphi_def: array function that returns shape acceleration at time t +% grad_alphaddot: Gradient of shape acceleration with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time +% grad_alphadot: Gradient of shape velocity with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time +% grad_alpha: Gradient of shape position with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time + + % Evaluate gradient of shape variables + grad_alpha_eval = cellfun(@(C) C(t), grad_alpha, 'UniformOutput', false); + grad_alphadot_eval = cellfun(@(C) C(t), grad_alphadot, 'UniformOutput', false); + grad_alphaddot_eval = cellfun(@(C) C(t), grad_alphaddot, 'UniformOutput', false); + del_cost = zeros(size(grad_alpha_eval)); + % Get the shape and shape derivative at the current time + shape = zeros(size(s.grid.eval)); + dshape = zeros(size(s.grid.eval)); + ddshape = zeros(size(s.grid.eval)); + + shape_gait_def = readGait(gait.phi_def,t); + dshape_gait_def = readGait(gait.dphi_def,t); + ddshape_gait_def = readGait(gait.ddphi_def,t); + + actual_size = min(numel(shape),numel(shape_gait_def)); + shape(1:actual_size) = shape_gait_def(1:actual_size); + dshape(1:actual_size) = dshape_gait_def(1:actual_size); + ddshape(1:actual_size) = ddshape_gait_def(1:actual_size); + + shapelist = num2cell(shape); + + metricgrad = getMetricGrad(s,shape,dM,grad_alpha_eval); + + M = metric; + + % Using inverse enough that it's worth it to calc once upfront + M_inv = inv(M); + + % Regular torque calculation + C = calc_coriolis_matrix(dM,shape,dshape); + tau = M*ddshape(:) + C; + + %Covariant acceleration calculation + cov_acc = M_inv*tau; + + for i = 1:numel(grad_alpha_eval) + % Partial of shape variables with respect to fourier coefficient i + del_shape = grad_alpha_eval{i}; + del_dshape = grad_alphadot_eval{i}; + del_ddshape = grad_alphaddot_eval{i}; + + % Gradient of torque calculation + % Start with effect of gradient on M_alpha*alphaddot + M_temp = zeros(length(shapelist)); + for j = 1:length(shapelist) + M_temp = M_temp + dM{j}*del_shape(j); + end + + % Catching for debugging + try + M_grad = M_temp*ddshape(:) + M*del_ddshape(:); + catch + M_temp + end + + %Gradient of inverse of mass for covariant acc calculation + Minv_grad = zeros(length(shapelist)); + for j = 1:length(shapelist) + %Formula from matrix cookbook + %d(M^-1)=-M^-1*dM*M^-1 + dM_alphainv_dalpha = -M_inv*dM{j}*M_inv; + Minv_grad = Minv_grad + dM_alphainv_dalpha*del_shape(j); + end + + % Effect of gradient on dM_alphadalpha*alphadot*alphadot + C1_partialgrad = zeros(length(shapelist)); + C1_shapegrad = zeros(length(shapelist)); + C1_outergrad = zeros(length(shapelist)); + del_dM_alphadalpha = cell(size(shapelist)); + for j = 1:length(shapelist) + Cj_temp = zeros(length(shapelist)); + for k = 1:length(shapelist) + Cj_temp = Cj_temp + ddM{j,k}*del_shape(k); + end + del_dM_alphadalpha{j} = Cj_temp; + C1_partialgrad = C1_partialgrad + Cj_temp*dshape(j); + C1_shapegrad = C1_shapegrad + dM{j}*del_dshape(j); + C1_outergrad = C1_outergrad + dM{j}*dshape(j); + end + C1_grad = (C1_partialgrad + C1_shapegrad)*dshape(:) + ... + C1_outergrad*del_dshape(:); + + % Effect of gradient on -(1/2)*alphadot'*dM_alphadalpha*alphadot + C2_grad = zeros(size(shapelist(:))); + for j = 1:length(shapelist) + C2_grad(j) = del_dshape(:)'*dM{j}*dshape(:) + ... + dshape(:)'*del_dM_alphadalpha{j}*dshape(:) + ... + dshape(:)'*dM{j}*del_dshape(:); + end + % Gradient of torque + del_tau = M_grad + C1_grad - (1/2)*C2_grad; + % Gradient of covariant acc + del_cov_acc = Minv_grad*tau(:)+M_inv*del_tau(:); + del_cost(i) = del_cov_acc'*metric*cov_acc... + + cov_acc'*metricgrad{i}*cov_acc... + + cov_acc'*metric*del_cov_acc; + end + del_cost = del_cost(:); +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/accelerationcoord_gradient_helper.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/accelerationcoord_gradient_helper.m new file mode 100644 index 00000000..326f681d --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/accelerationcoord_gradient_helper.m @@ -0,0 +1,40 @@ +function del_cost = accelerationcoord_gradient_helper(t,X,s,gait,grad_alphaddot,metric,dM,ddM) +% Helper function to calculate the gradient of shape-space acceleration +% cost +% Designed to work with ode45; solves for the gradient of cost at an +% instant of time t. +% Inputs: +% t: Time period at which gradient of cost is being evaluated. +% X: Unused, required by ode45 +% s: system struct used by sysplotter +% gait: Struct containing fields: +% phi_def: array function that returns shape at time value t +% dphi_def: array function that returns shape velocity at time t +% ddphi_def: array function that returns shape acceleration at time t +% grad_alphaddot: Gradient of shape acceleration with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time + + % Evaluate gradient of shape variables + grad_alphaddot_eval = cellfun(@(C) C(t), grad_alphaddot, 'UniformOutput', false); + del_cost = zeros(size(grad_alphaddot_eval)); + % Get the shape derivative at the current time + + ddshape_def = readGait(gait.ddphi_def,t); + actual_size = min(numel(s.grid.eval),numel(ddshape_def)); + ddshape = zeros(size(s.grid.eval)); + + ddshape(1:actual_size) = ddshape_def(1:actual_size); + + % Regular cost calculation + cost = sqrt(ddshape(:)'*ddshape(:)); + + for i = 1:numel(grad_alphaddot_eval) + % Partial of shape acceleration with respect to fourier coefficient i + del_ddshape = grad_alphaddot_eval{i}; + + % Gradient of shape-space acceleration cost + del_cost(i) = del_ddshape(:)'*ddshape(:)+ddshape(:)'*del_ddshape(:); + end + del_cost = del_cost(:); +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/evaluate_displacement_and_cost1.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/evaluate_displacement_and_cost1.m new file mode 100644 index 00000000..7535a7a9 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/evaluate_displacement_and_cost1.m @@ -0,0 +1,91 @@ +function [net_disp_orig, net_disp_opt, cost] = evaluate_displacement_and_cost1(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_def" and "dphi_def", 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 +% fixed point, to reduce interpolation overhead computational times. +% +% 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 = 'fixed_step'; + 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); + + otherwise + error('Unknown method for integrating motion'); + end + + + % Convert the final motion into its representation in optimal + % coordinates + startshape = zeros(size(s.grid.eval)); + startshape_def = readGait(p.phi_def,0); + + actual_size = min(numel(startshape),numel(startshape_def)); + startshape(1:actual_size) = startshape_def(1:actual_size); + + startshapelist = num2cell(startshape); + beta_theta = interpn(s.grid.eval{:},s.B_optimized.eval.Beta{3},startshapelist{:},'spline'); + net_disp_opt = [cos(beta_theta) sin(beta_theta) 0;... + -sin(beta_theta) cos(beta_theta) 0;... + 0 0 1]*net_disp_orig; + + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/fixed_step_integrator.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/fixed_step_integrator.m new file mode 100644 index 00000000..e55efd59 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/fixed_step_integrator.m @@ -0,0 +1,121 @@ +% 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 ischar(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); + + %Prep interpolation inputs for velocities function + shape = zeros(size(s.grid.eval)); + shape_gait_def_0 = readGait(gait.phi_def,0); + actual_size = min(numel(shape),numel(shape_gait_def_0)); + + samplePoints = {}; + for dim = 1:actual_size + samplePoints{dim} = []; + end + + for time = tpoints + shape_gait_def = readGait(gait.phi_def,time); + shape(1:actual_size) = shape_gait_def(1:actual_size); + for dim = 1:numel(shape) + samplePoints{dim}(end+1) = shape(dim); + end + end + + indexList = 1:numel(tpoints); + id = eye(actual_size); + + As = cellfun(@(C) -interpn(s.grid.eval{:},C,... + samplePoints{:},'spline'),s.vecfield.eval.content.Avec,... + 'UniformOutput',false); + As = celltensorconvert(As); + + switch s.costfunction + case {'pathlength coord','acceleration coord'} + %In the case of these two cost functions, we only care about + %the connection field, and the metric is always identity. + %dM is passed a filler value + [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,As{i},id,1),... + tpoints,indexList,'UniformOutput',false); + case {'torque','covariant acceleration','power quality'} + %In the inertial cases, we need to calculate dM, and the metric + metrics = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... + samplePoints{:},'spline'),s.metricfield.metric_eval.content.metric,... + 'UniformOutput',false); + metrics = celltensorconvert(metrics); + + dM_set = {}; + for dim = 1:actual_size + dM_holder = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... + samplePoints{:},'spline'),s.coriolisfield.coriolis_eval.content.dM{dim},... + 'UniformOutput',false); + dM_holder = celltensorconvert(dM_holder); + dM_set{dim} = dM_holder; + end + dMs = {}; + for i = 1:numel(tpoints) + dMs{i} = {}; + for dim = 1:actual_size + dMs{i}{dim} = dM_set{dim}{i}; + end + end + + [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,... + As{i},metrics{i},dMs{i}),tpoints,indexList,'UniformOutput',false); + otherwise + %Otherwise, we're not doing inertial so don't need dM, but we + %do care about the metric and connection + metrics = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... + samplePoints{:},'spline'),s.metricfield.metric_eval.content.metric,... + 'UniformOutput',false); + metrics = celltensorconvert(metrics); + + [xi,dcost] = arrayfun(@(t,i) get_velocities(t,s,gait,ConnectionEval,... + As{i},metrics{i},1),tpoints,indexList,'UniformOutput',false); + end + + %%%%%%% + % 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 \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/getMetricGrad.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/getMetricGrad.m new file mode 100644 index 00000000..038f7dcb --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/getMetricGrad.m @@ -0,0 +1,21 @@ +function metricgrad = getMetricGrad(s,shape,dM,grad_alpha) +%Returns metric at a shape position, and gradient of metric w.r.t. fourier +%coefficients + +%s - structure containing metric function +%shape - array of shape values +%dM - derivative of matrix with respect to shape variables +%grad_alpha - gradient of shape values w.r.t. fourier coefficients + + n_dim = numel(s.grid.eval); + actual_size = min(size(shape,2),n_dim); + + metricgrad = repmat({zeros(actual_size)},size(grad_alpha)); + for j = 1:actual_size + + for k = 1:numel(grad_alpha) + metricgrad{k} = metricgrad{k}+ dM{j}*grad_alpha{k}(j); + end + end + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/get_velocities.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/get_velocities.m new file mode 100644 index 00000000..791953c5 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/get_velocities.m @@ -0,0 +1,57 @@ +% Evaluate the body velocity and cost velocity (according to system metric) +% at a given time +function [gcirc, dcost] = get_velocities(t,s,gait,ConnectionEval,A,metric,dM) + + % Get the shape and shape derivative at the current time + shape = zeros(size(s.grid.eval)); + dshape = zeros(size(s.grid.eval)); + ddshape = zeros(size(s.grid.eval)); + + shape_gait_def = readGait(gait.phi_def,t); + dshape_gait_def = readGait(gait.dphi_def,t); + ddshape_gait_def = readGait(gait.ddphi_def,t); + + actual_size = min(numel(shape),numel(shape_gait_def)); + shape(1:actual_size) = shape_gait_def(1:actual_size); + dshape(1:actual_size) = dshape_gait_def(1:actual_size); + ddshape(1:actual_size) = ddshape_gait_def(1:actual_size); + + M_a = metric; + + shapelist = num2cell(shape); + + % If doing functional eval of system (not recommended) + % Get the local connection and metric at the current time, in the new coordinates + if strcmpi(ConnectionEval,'functional') + + A = s.A_num(shapelist{:})./s.A_den(shapelist{:}); + + switch s.system_type + case 'drag' + metric = s.metric(shapelist{:}); + case 'inertia' + error('Functional ConnectionEval method not supported for inertia systems!') + end + + end + + % Get the body velocity at the current time + %t; + gcirc = - A * dshape(:); + + switch s.costfunction + case {'pathlength metric','pathlength coord'} + dcost = sqrt(dshape(:)'*metric*dshape(:)); + case 'pathlength metric2' + dcost = sqrt(dshape(:)'*metric*metric*dshape(:)); + case 'torque' + dcost = torque_cost(M_a,dM,shape,dshape,ddshape,metric); + case 'covariant acceleration' + dcost = acceleration_cost(M_a,dM,shape,dshape,ddshape,metric); + case 'acceleration coord' + dcost = ddshape(:)'*metric*ddshape(:); + case 'power quality' + dcost = power_quality_cost(M_a,dM,shape,dshape,ddshape); + end + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/helper_function.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/helper_function.m new file mode 100644 index 00000000..d8230f88 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/helper_function.m @@ -0,0 +1,17 @@ +% 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 \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/inertia_cost_gradient.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/inertia_cost_gradient.m new file mode 100644 index 00000000..cf3d568e --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/inertia_cost_gradient.m @@ -0,0 +1,130 @@ +function cost_grad = inertia_cost_gradient(s,n,y,g,gait,EvaluationMethod) +% Calculates the gradient of cost for inertial systems. +% Inputs: +% s: System struct used by sysplotter. +% n: Number of points at which gait should be evaluated in the shape +% space. +% y: Fourier coefficients that parametrize the gait. +% g: Time period over which gait is executed. +% gait: Struct containing fields: +% phi_def: array function that returns shape at time value t +% dphi_def: array function that returns shape velocity at time t +% ddphi_def: array function that returns shape acceleration at time t +% EvaluationMethod: String representing how the gradient of cost should +% be calculated; provide 'discrete' to evaluate at 100 discrete values or +% 'ode45' if you would like the gradient of cost to be integrated using +% ode45. Other values will result in error. + + % Contribution to gradient from the movement of each point due to + % change in fourier coefficients + [grad_alphaddot,grad_alphadot,grad_alpha] = shape_grad(n,y,g); + + cost_grad = zeros(size(grad_alpha)); + tspan = [0 g]; + if strcmpi(EvaluationMethod,'discrete') + num_pts = 100; + t_pts = linspace(0,g,num_pts); + del_t = t_pts(2) - t_pts(1); + + %Prep interpolation inputs for gradient calcs + shape = zeros(size(s.grid.eval)); + shape_gait_def_0 = readGait(gait.phi_def,0); + actual_size = min(numel(shape),numel(shape_gait_def_0)); + + samplePoints = {}; + for dim = 1:actual_size + samplePoints{dim} = []; + end + + for time = t_pts + shape_gait_def = readGait(gait.phi_def,time); + shape(1:actual_size) = shape_gait_def(1:actual_size); + for dim = 1:numel(shape) + samplePoints{dim}(end+1) = shape(dim); + end + end + + %Batch interpolate the metric at each point along the gait + metrics = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... + samplePoints{:},'spline'),s.metricfield.metric_eval.content.metric,... + 'UniformOutput',false); + metrics = celltensorconvert(metrics); + + dM_set = {}; + for dim = 1:actual_size + dM_holder = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... + samplePoints{:},'spline'),s.coriolisfield.coriolis_eval.content.dM{dim},... + 'UniformOutput',false); + dM_holder = celltensorconvert(dM_holder); + dM_set{dim} = dM_holder; + end + dMs = {}; + for i = 1:numel(t_pts) + dMs{i} = {}; + for dim = 1:actual_size + dMs{i}{dim} = dM_set{dim}{i}; + end + end + + empty_ddM = cell(size(s.coriolisfield.coriolis_eval.content.ddM)); + ddM_set = empty_ddM; + for dim = 1:numel(ddM_set) + ddM_holder = cellfun(@(C) interpn(s.grid.metric_eval{:},C,... + samplePoints{:},'spline'),s.coriolisfield.coriolis_eval.content.ddM{dim},... + 'UniformOutput',false); + ddM_holder = celltensorconvert(ddM_holder); + ddM_set{dim} = ddM_holder; + end + ddMs = {}; + for i = 1:numel(t_pts) + ddMs{i} = empty_ddM; + for dim = 1:numel(empty_ddM) + ddMs{i}{dim} = ddM_set{dim}{i}; + end + end + + switch s.costfunction + case 'torque' + for k = 1:length(t_pts) + del_cost = inertia_gradient_helper(t_pts(k),[],s,gait,grad_alpha,grad_alphadot,grad_alphaddot,metrics{k},dMs{k},ddMs{k}); + cost_grad = cost_grad + reshape(del_cost,size(cost_grad)).*del_t; + end + case 'covariant acceleration' + for k = 1:length(t_pts) + del_cost = acceleration_gradient_helper(t_pts(k),[],s,gait,grad_alpha,grad_alphadot,grad_alphaddot,metrics{k},dMs{k},ddMs{k}); + cost_grad = cost_grad + reshape(del_cost,size(cost_grad)).*del_t; + end + case 'acceleration coord' + for k = 1:length(t_pts) + del_cost = accelerationcoord_gradient_helper(t_pts(k),[],s,gait,grad_alphaddot,metrics{k},dMs{k},ddMs{k}); + cost_grad = cost_grad + reshape(del_cost,size(cost_grad)).*del_t; + end + case 'power quality' + for k = 1:length(t_pts) + del_cost = powerquality_gradient_helper(t_pts(k),[],s,gait,grad_alpha,grad_alphadot,grad_alphaddot,metrics{k},dMs{k},ddMs{k}); + cost_grad = cost_grad + reshape(del_cost,size(cost_grad)).*del_t; + end + end + % Reset gradient of fourier frequency to be zero to prevent changes + % to it + cost_grad(end,:) = 0; + elseif strcmpi(EvaluationMethod,'ode45') + switch s.costfunction + case 'torque' + sol = ode45(@(t,y) inertia_gradient_helper(t,y,s,gait,grad_alpha,grad_alphadot,grad_alphaddot),tspan,cost_grad); + case 'covariant acceleration' + sol = ode45(@(t,y) acceleration_gradient_helper(t,y,s,gait,grad_alpha,grad_alphadot,grad_alphaddot),tspan,cost_grad); + case 'acceleration coord' + sol = ode45(@(t,y) accelerationcoord_gradient_helper(t,y,s,gait,grad_alphaddot),tspan,cost_grad); + case 'power quality' + sol = ode45(@(t,y) powerquality_gradient_helper(t,y,s,gait,grad_alpha,grad_alphadot,grad_alphaddot),tspan,cost_grad); + end + % Extract the final motion + cost_grad = reshape(deval(sol,tspan(end)),size(cost_grad)); + % Reset gradient of fourier frequency to be zero to prevent changes + % to it + cost_grad(end,:) = 0; + else + error('Untenable option provided for EvaluationMethod!') + end +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/inertia_gradient_helper.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/inertia_gradient_helper.m new file mode 100644 index 00000000..2c68f585 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/inertia_gradient_helper.m @@ -0,0 +1,102 @@ +function del_cost = inertia_gradient_helper(t,X,s,gait,grad_alpha,grad_alphadot,grad_alphaddot,metric,dM,ddM) +% Helper function to calculate the gradient of cost for inertia systems. +% Designed to work with ode45; solves for the gradient of cost at an +% instant of time t. +% Inputs: +% t: Time period at which gradient of cost is being evaluated. +% X: Unused, required by ode45 +% s: system struct used by sysplotter +% gait: Struct containing fields: +% phi_def: array function that returns shape at time value t +% dphi_def: array function that returns shape velocity at time t +% ddphi_def: array function that returns shape acceleration at time t +% grad_alphaddot: Gradient of shape acceleration with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time +% grad_alphadot: Gradient of shape velocity with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time +% grad_alpha: Gradient of shape position with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time + + % Evaluate gradient of shape variables + grad_alpha_eval = cellfun(@(C) C(t), grad_alpha, 'UniformOutput', false); + grad_alphadot_eval = cellfun(@(C) C(t), grad_alphadot, 'UniformOutput', false); + grad_alphaddot_eval = cellfun(@(C) C(t), grad_alphaddot, 'UniformOutput', false); + del_cost = zeros(size(grad_alpha_eval)); + % Get the shape and shape derivative at the current time + shape = zeros(size(s.grid.eval)); + dshape = zeros(size(s.grid.eval)); + ddshape = zeros(size(s.grid.eval)); + + shape_gait_def = readGait(gait.phi_def,t); + dshape_gait_def = readGait(gait.dphi_def,t); + ddshape_gait_def = readGait(gait.ddphi_def,t); + + actual_size = min(numel(shape),numel(shape_gait_def)); + shape(1:actual_size) = shape_gait_def(1:actual_size); + dshape(1:actual_size) = dshape_gait_def(1:actual_size); + ddshape(1:actual_size) = ddshape_gait_def(1:actual_size); + + shapelist = num2cell(shape); + + metricgrad = getMetricGrad(s,shape,dM,grad_alpha_eval); + + M = metric; + + % Regular torque calculation + C = calc_coriolis_matrix(dM,shape,dshape); + tau = M*ddshape(:) + C; + + for i = 1:numel(grad_alpha_eval) + % Partial of shape variables with respect to fourier coefficient i + del_shape = grad_alpha_eval{i}; + del_dshape = grad_alphadot_eval{i}; + del_ddshape = grad_alphaddot_eval{i}; + + % Gradient of torque calculation + % Start with effect of gradient on M_alpha*alphaddot + M_temp = zeros(length(shapelist)); + for j = 1:length(shapelist) + M_temp = M_temp + dM{j}*del_shape(j); + end + % Catching for debugging + try + M_grad = M_temp*ddshape(:) + M*del_ddshape(:); + catch + M_temp + end + % Effect of gradient on dM_alphadalpha*alphadot*alphadot + C1_partialgrad = zeros(length(shapelist)); + C1_shapegrad = zeros(length(shapelist)); + C1_outergrad = zeros(length(shapelist)); + del_dM_alphadalpha = cell(size(shapelist)); + for j = 1:length(shapelist) + Cj_temp = zeros(length(shapelist)); + for k = 1:length(shapelist) + Cj_temp = Cj_temp + ddM{j,k}*del_shape(k); + end + del_dM_alphadalpha{j} = Cj_temp; + C1_partialgrad = C1_partialgrad + Cj_temp*dshape(j); + C1_shapegrad = C1_shapegrad + dM{j}*del_dshape(j); + C1_outergrad = C1_outergrad + dM{j}*dshape(j); + end + C1_grad = (C1_partialgrad + C1_shapegrad)*dshape(:) + ... + C1_outergrad*del_dshape(:); + + % Effect of gradient on -(1/2)*alphadot'*dM_alphadalpha*alphadot + C2_grad = zeros(size(shapelist(:))); + for j = 1:length(shapelist) + C2_grad(j) = del_dshape(:)'*dM{j}*dshape(:) + ... + dshape(:)'*del_dM_alphadalpha{j}*dshape(:) + ... + dshape(:)'*dM{j}*del_dshape(:); + end + + % Gradient of torque + del_tau = M_grad + C1_grad - (1/2)*C2_grad; + del_cost(i) = del_tau(:)'*tau(:)... + + tau(:)'*del_tau(:); + end + del_cost = del_cost(:); +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobiandispcalculator3.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobiandispcalculator3.m new file mode 100644 index 00000000..3ccd4c98 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobiandispcalculator3.m @@ -0,0 +1,43 @@ +function a=jacobiandispcalculator3(p1,p2,p3,ccf,dimension) +%%%%%%%%% +% +% jacobiandispcalculator3 is the function that calculates the gradient of +% displacement for the ith point. +% Its input arguments are the coordinates of the (i-1)th, ith and (i+1)th point, +% CCF value at point i(ccf) and the dimension of the shape space (dimension) +% +%%%%%%%%% + +l1=0; % variable for calculating length of the line segment joining the (i-1)th point with the (i+1)th point +base = zeros(1,dimension); +for i=1:numel(base) + l1=l1+(p1(i)-p3(i))^2; + base(1,i)=p3(i)-p1(i); % vector connecting the (i-1)th point and (i+1)th point +end +%l=sqrt(l1); % length of the line segment joining the (i-1)th point with the (i+1)th point + +jacobian = zeros(1,dimension); +for i=1:dimension +% jacobian(1,i)=0; + perp1=zeros(1,dimension); + perp1(i)=1; + %parcomp=base*perp1'/norm(base); + %perp1-parcomp*base/norm(base); %%recheck again + perp=perp1;% Unit vector along the ith direction + % The for loop below calculates the gradient along the ith direction by + % treating the CCF as 2 forms. A specific (j,k) represents a component of the 2 form + for j=1:dimension-1 + for k=1:dimension-j + veca=zeros(1,dimension); + vecb=zeros(1,dimension); + veca(j)=1; + vecb(j+k)=1; + f=(j-1)*dimension-(j*(j-1))/2+k; + jacobian(1,i)=jacobian(1,i)+0.5*ccf(f)*((veca*perp')*(vecb*base')-(vecb*perp')*(veca*base')); + end + end +end + +a=jacobian; + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobianeqicalculator.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobianeqicalculator.m new file mode 100644 index 00000000..4c83f1b2 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobianeqicalculator.m @@ -0,0 +1,39 @@ +function jacobianeqi = jacobianeqicalculator(y,n,dimension,metric) + % Calculates the gradient of the force driving the points along the + % gait to be equally spaced. + % Inputs: + % y: matrix containing points that transcribe the gait + % n: number of points in gait transcription + % dimension: number of shape variables + % metric: Riemannian metric + + jacobianeqi = zeros(n,dimension); + + %l is the vector containing metric weighted distances between neighbouring + %points + l = zeros(1,n); + for i=1:(numel(l)-1) + l(i)=sqrt((y(i+1,:)-y(i,:))*((metric{i}+metric{i+1})/2)*(y(i+1,:)-y(i,:))'); + end + l(end)=sqrt((y(1,:)-y(n,:))*((metric{n}+metric{1})/2)*(y(1,:)-y(n,:))'); + + for i=2:n-1 + len=sqrt((y(i+1,:)-y(i-1,:))*((metric{i-1}+metric{i+1})/2)*(y(i+1,:)-y(i-1,:))'); % metric weighted length between point (i-1) and (i+1) + midpoint=y(i-1,:)+((y(i+1,:)-y(i-1,:))*sqrtm((metric{i-1}+metric{i+1})/2))/2; % location of midpoint of the line segment joining point (i-1) and (i+1) + betacos=(y(i+1,:)-y(i-1,:))*sqrtm((metric{i-1}+metric{i+1})/2)*((y(i,:)-y(i-1,:))*sqrtm((metric{i-1}+metric{i})/2))'/(l(i-1)*len); + xhat=y(i-1,:)+(y(i+1,:)-y(i-1,:))*sqrtm((metric{i-1}+metric{i+1})/2)*l(i-1)*betacos/len; %projection of ith point onto the line joining the (i-1)th and (i+1)th points + jacobianeqi(i,:)=midpoint-xhat; % gradient of the ith point is equal to the difference between the midpoint and the projection of ith point + end + + len=sqrt((y(2,:)-y(n,:))*((metric{2}+metric{n})/2)*(y(2,:)-y(n,:))'); + midpoint=y(n,:)+((y(2,:)-y(n,:))*sqrtm((metric{n}+metric{2})/2))/2; + betacos=(y(2,:)-y(n,:))*sqrtm((metric{n}+metric{2})/2)*((y(1,:)-y(n,:))*sqrtm((metric{n}+metric{1})/2))'/(l(n)*len); + xhat=y(n,:)+(y(2,:)-y(n,:))*sqrtm((metric{n}+metric{2})/2)*l(n)*betacos/len; + jacobianeqi(1,:)=midpoint-xhat; + + len=sqrt((y(1,:)-y(n-1,:))*((metric{1}+metric{n-1})/2)*(y(1,:)-y(n-1,:))'); + midpoint=y(n-1,:)+((y(1,:)-y(n-1,:))*sqrtm((metric{1}+metric{n-1})/2))/2; + betacos=(y(1,:)-y(n-1,:))*sqrtm((metric{n-1}+metric{1})/2)*((y(n,:)-y(n-1,:))*sqrtm((metric{n-1}+metric{n})/2))'/(l(n-1)*len); + xhat=y(n-1,:)+(y(1,:)-y(n-1,:))*sqrtm((metric{n-1}+metric{1})/2)*l(n-1)*betacos/len; + jacobianeqi(n,:)=midpoint-xhat; +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobianstrokecalculator.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobianstrokecalculator.m new file mode 100644 index 00000000..118d8b75 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/jacobianstrokecalculator.m @@ -0,0 +1,50 @@ +function jacobianstroke = jacobianstrokecalculator(y,n,dimension,metric,metricgrad) + % Calculates the gradient of cost for drag dominated systems. + % Inputs: + % y: matrix containing points that transcribe the gait + % n: number of points in gait transcription + % dimension: number of shape variables + % metric: Riemannian metric + % metricgrad: Gradient of Riemannian metric + + %l is the vector containing metric weighted distances between neighbouring + %points + l = zeros(1,n); + for i=1:(numel(l)-1) + l(i)=sqrt((y(i+1,:)-y(i,:))*((metric{i}+metric{i+1})/2)*(y(i+1,:)-y(i,:))'); + end + l(end)=sqrt((y(1,:)-y(n,:))*((metric{n}+metric{1})/2)*(y(1,:)-y(n,:))'); + + delp = cell(1,n); + for i=1:(numel(delp)-1) + delp{i}=y(i+1,:)-y(i,:); % delp{i} is the vector joining the (i+1)th point to the ith point + end + delp{end}=y(1,:)-y(n,:); + + jacobianstroke = zeros(n,dimension); + contrigrad=zeros(n,dimension); + for i=2:n-1 + for j=1:dimension + %Contrigrad is the contribution to the gradient due to the metric changing + contrigrad(i,j)=0.5*delp{i}*metricgrad{i,j}*delp{i}'/(2*l(i))+0.5*delp{i-1}*metricgrad{i,j}*delp{i-1}'/(2*l(i-1)); + end + % Total gradient is the result of distance changing due to movement of point and the metric changing due to movement of the point + jacobianstroke(i,:)=(-(((metric{i}+metric{i+1})/2)*delp{i}')'-(delp{i}*((metric{i}+metric{i+1})/2)))/(2*l(i))+... + +((((metric{i-1}+metric{i})/2)*delp{i-1}')'+(delp{i-1}*((metric{i}+metric{i-1})/2)))/(2*l(i-1))+contrigrad(i,:); + end + + % Calculation for the 1st point and last point have to be done outside the + % loop as the (i+1)th point for the last point is the first point and + % (i-1)th point for the first point is the last point + for j=1:dimension + contrigrad(1,j)=0.5*delp{1}*metricgrad{1,j}*delp{1}'/(2*l(1))+0.5*delp{n}*metricgrad{1,j}*delp{n}'/(2*l(n)); + end + jacobianstroke(1,:)=(-(((metric{1}+metric{2})/2)*delp{1}')'-(delp{1}*((metric{1}+metric{2})/2)))/(2*l(1))+... + +((((metric{n}+metric{1})/2)*delp{n}')'+(delp{n}*((metric{n}+metric{1})/2)))/(2*l(n))+contrigrad(1,:); + + for j=1:dimension + contrigrad(n,j)=0.5*delp{n}*metricgrad{n,j}*delp{n}'/(2*l(n))+0.5*delp{n-1}*metricgrad{n,j}*delp{n-1}'/(2*l(n-1)); + end + jacobianstroke(n,:)=(-(((metric{n}+metric{1})/2)*delp{n}')'-(delp{n}*((metric{n}+metric{1})/2)))/(2*l(n))+... + +((((metric{n}+metric{n-1})/2)*delp{n-1}')'+(delp{n-1}*((metric{n}+metric{n-1})/2)))/(2*l(n-1))+contrigrad(n,:); +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/makeGait.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/makeGait.m new file mode 100644 index 00000000..81357e52 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/makeGait.m @@ -0,0 +1,26 @@ +%Makes gait definition structure of cell functions from +%fourier coefficients y for arbitrary dimension. +function gait = makeGait(y) + + ndim = size(y,2); + + gait.phi_def = cell(1,ndim); + gait.dphi_def = cell(1,ndim); + gait.ddphi_def = cell(1,ndim); + + for dim = 1:ndim + + w = y(end,dim); + gait.phi_def{dim} = @(t) y(1,dim)+y(2,dim)*cos(w*t)+y(3,dim)*sin(w*t)+y(4,dim)*cos(2*w*t)+... + +y(5,dim)*sin(2*w*t)+y(6,dim)*cos(3*w*t)+y(7,dim)*sin(3*w*t)+... + +y(8,dim)*cos(4*w*t)+y(9,dim)*sin(4*w*t); + gait.dphi_def{dim} = @(t) -w*y(2,dim)*sin(w*t)+w*y(3,dim)*cos(w*t)-2*w*y(4,dim)*sin(2*w*t)+... + +2*w*y(5,dim)*cos(2*w*t)-3*w*y(6,dim)*sin(3*w*t)+3*w*y(7,dim)*cos(3*w*t)+... + -4*w*y(8,dim)*sin(4*w*t)+4*w*y(9,dim)*cos(4*w*t); + gait.ddphi_def{dim} = @(t) -w^2*y(2,dim)*cos(w*t)-w^2*y(3,dim)*sin(w*t)-4*w^2*y(4,dim)*cos(2*w*t)+... + -4*w^2*y(5,dim)*sin(2*w*t)-9*w^2*y(6,dim)*cos(3*w*t)-9*w^2*y(7,dim)*sin(3*w*t)+... + -16*w^2*y(8,dim)*cos(4*w*t)-16*w^2*y(9,dim)*sin(4*w*t); + + end + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/nonlcon.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/nonlcon.m new file mode 100644 index 00000000..d3279497 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/nonlcon.m @@ -0,0 +1,70 @@ +function [A,Aeq]=nonlcon(y,s,n,dimension,lb,ub,direction) +%%%%%%%%% +% +%This function imposes the nonlinear constraint that all the points forming the gait stay within bounds +% +%Inputs: +% +%y: Fourier series coefficients that describe the gait +%s: System file which contains the connection vector field, CCF's and +% metric data +%n: Number of points used to parametrize the gaits in a direct +% transcription method +%dimension: Indicates the number of shape variables of the system +%lb: Lower bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired +%ub: Upper bound of shape variables for each point which is obtained from the grid inside which an optimal gait is desired +% +%%%%%%%%% + +% % The first step is to obtain a direct transciption parametrization of the gait from the +% % fourier series parametrization +y1 = path_from_fourier(y,n,dimension); +y2=y1(:); + +%b=length(y2); + +% A1 and A2 together impose the constraint that all the points forming the gait stay in bounds +A1=y2+lb; +A2=-y2-ub; + +A = [A1;A2]; + + +% Make sure the frequency doesn't get changed from 2*pi +Aeq = y(end,:)' - 2*pi; + + +% Make sure that the displacement in the other two directions is zero +w1 = y(end,1); % Frequency of Fourier transform +w2 = y(end,2); +% Assign a time period for executing the gait +T = 2*pi/w1; +p = makeGait(y); +[~, net_disp_opt] = evaluate_displacement_and_cost1(s,p,[0, T],'interpolated','fixed_step'); + +% % If optimizing for translation, restrict to zero rotation. +% if direction ~=3 +% Aeq(end+1) = net_disp_opt(3); +% end + +% Constrain solutions to only displace in the desired direction +for idx=1:3 + if idx ~= direction + Aeq(end+1) = net_disp_opt(idx); + end +end + +% +% gradA = []; +% +% +% gradAeq = ones(size(A,1),dimension); +% +% jacobiandisp = zeros(n,dimension); +% for i=2:1:n-1 +% jacobiandisp(i,:)=jacobiandispcalculator3(y(i-1,:),y(i,:),y(i+1,:),ccf(i,:),dimension); +% end +% jacobiandisp(1,:)=jacobiandispcalculator3(y(n,:),y(1,:),y(2,:),ccf(1,:),dimension); +% jacobiandisp(n,:)=jacobiandispcalculator3(y(n-1,:),y(n,:),y(1,:),ccf(n,:),dimension); + +end diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/outfun.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/outfun.m new file mode 100644 index 00000000..c9995304 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/outfun.m @@ -0,0 +1,83 @@ +function stop=outfun(y,optimValues,state,stretch,s,handles) +%%%%%%%%% +% +%This function plots the current state of the gait on the sysplotter GUI +%after every iteration of the optimizer +% +%%%%%%%%% + +n=100; +dimension=length(y(1,:)); + +% % The if else statement below deletes gaits 2 iterations after they have been plotted +% if optimValues.iteration>2 +% children=get(gca,'children'); +% delete(children(6:10)); +% else +% end + +for thisAxes = [1:numel(handles.plot_thumbnails.Children)] + + axes(handles.plot_thumbnails.Children(thisAxes)); + + % The if else statement below fades the gait plotted during the previous iteration + if optimValues.iteration>1 + children=get(gca,'children'); + for idx = 1:numel(children) + + if iscell(children(idx).UserData) && strcmp(children(idx).UserData{1},'OptimizeTracer') + children(idx).UserData = {'OptimizeTracer', children(idx).UserData{2}-1}; + + if children(idx).UserData{2} == 0 + + delete(children(idx)); + + else + + children(idx).Color=[0.5 0.5 0.5]; + children(idx).LineWidth=4; + end + end + + end + % children(1).Color=[0.5 0.5 0.5]; + % children(2).Color=[0.5 0.5 0.5]; + % children(3).Color=[0.5 0.5 0.5]; + % children(4).Color=[0.5 0.5 0.5]; + % children(5).Color=[0.5 0.5 0.5]; + % + % children(1).LineWidth=4; + % children(2).LineWidth=4; + % children(3).LineWidth=4; + % children(4).LineWidth=4; + % children(5).LineWidth=4; + else + end + + % The if else statement below plots the gait after every iteration + if optimValues.iteration>0 + y1 = path_from_fourier(y,n,dimension); + hold on + if stretch + stretchnames = {'stretch','surface'}; + stretchname = stretchnames{stretch}; + + [x_temp,y_temp,z_temp] = s.convert.(stretchname).old_to_new_points(y1(:,1),y1(:,2)); + else + x_temp = y1(:,1); + y_temp = y1(:,2); + z_temp = zeros(size(y1(:,1))); + if size(y1,2) > 2 + z_temp = y1(:,3); + end + end + handle1=line('XData',x_temp,'YData',y_temp,'ZData',z_temp,'color','k','linewidth',3,'UserData',{'OptimizeTracer',2}); %#ok + %plot_dir_arrows(y1(:,1),y1(:,2),2,'Color',[0 0 0],'LineWidth',3); + else + end + +end + +pause(0.05) +stop=false; +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/path_from_fourier.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/path_from_fourier.m new file mode 100644 index 00000000..6a3c6992 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/path_from_fourier.m @@ -0,0 +1,26 @@ +function y = path_from_fourier(f,n,dimension) +% Returns the shape space parametrization of the gait at n points when provided with +% the fourier coefficients f. The gait is returned as a self-closed gait +% (i.e. the first and last rows of the output y are the same point). +% Inputs: +% f: Fourier coefficients that parametrize the gait. +% n: Number of points that should compose the gait, less the final +% self-closed point +% dimension: Number of shape variables for system + + y = zeros(n+1,dimension); + % Determine time period based on value of fourier frequency + w = f(end,1); + T = 2*pi/w; + % Create time vector at which to evaluate points of gait + t = linspace(0,T,n+1); + % Evaluate the shape-space parametrization of the gait at every time + % value in t + for j=1:dimension + for i=1:1:n+1 + y(i,j)=f(1,j)+f(2,j)*cos(w*t(i))+f(3,j)*sin(w*t(i))+f(4,j)*cos(2*w*t(i))+... + +f(5,j)*sin(2*w*t(i))+f(6,j)*cos(3*w*t(i))+f(7,j)*sin(3*w*t(i))+... + +f(8,j)*cos(4*w*t(i))+f(9,j)*sin(4*w*t(i)); + end + end +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/power_quality_cost.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/power_quality_cost.m new file mode 100644 index 00000000..a8cf9afe --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/power_quality_cost.m @@ -0,0 +1,20 @@ +function dcost = power_quality_cost(M,dM,shape,dshape,ddshape) +% Calculates the incremental cost for an inertial system where cost is power quality. +% Inputs: +% M: Mass matrix +% dM_alphadalpha: Partial of mass matrix with respect to shape variables; +% must be cell where the ith entry is the partial with respect to the +% ith shape variable +% shape: Current shape configuration of system, at which M and +% dM_alphadalpha were evaluated +% dshape: Current shape velocity of system +% ddshape: Current shape acceleration of system + + % Start by calculating the coriolis matrix + C = calc_coriolis_matrix(dM,shape,dshape); + % Calculate the torque for this instant of time + dtau = M*ddshape(:) + C; + % Calculate power quality + dcost = (dshape(:)'*dtau)^2 - ((dshape(:)').^2*dtau.^2); + dcost = dcost + 100; +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/powerquality_gradient_helper.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/powerquality_gradient_helper.m new file mode 100644 index 00000000..45acbcce --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/powerquality_gradient_helper.m @@ -0,0 +1,100 @@ +function del_cost = powerquality_gradient_helper(t,X,s,gait,grad_alpha,grad_alphadot,grad_alphaddot,metric,dM,ddM) +% Helper function to calculate the gradient of cost for inertia systems. +% Designed to work with ode45; solves for the gradient of cost at an +% instant of time t. +% Inputs: +% t: Time period at which gradient of cost is being evaluated. +% X: Unused, required by ode45 +% s: system struct used by sysplotter +% gait: Struct containing fields: +% phi_def: array function that returns shape at time value t +% dphi_def: array function that returns shape velocity at time t +% ddphi_def: array function that returns shape acceleration at time t +% grad_alphaddot: Gradient of shape acceleration with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time +% grad_alphadot: Gradient of shape velocity with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time +% grad_alpha: Gradient of shape position with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time + + % Evaluate gradient of shape variables + grad_alpha_eval = cellfun(@(C) C(t), grad_alpha, 'UniformOutput', false); + grad_alphadot_eval = cellfun(@(C) C(t), grad_alphadot, 'UniformOutput', false); + grad_alphaddot_eval = cellfun(@(C) C(t), grad_alphaddot, 'UniformOutput', false); + del_cost = zeros(size(grad_alpha_eval)); + % Get the shape and shape derivative at the current time + shape = readGait(gait.phi_def,t); + shapelist = num2cell(shape); + dshape = readGait(gait.dphi_def,t); + ddshape = readGait(gait.ddphi_def,t); + + % Get mass and partial mass matrices + M = metric; + + % Regular torque calculation + C = calc_coriolis_matrix(dM,shape,dshape); + tau = M*ddshape(:) + C; + + for i = 1:numel(grad_alpha_eval) + % Partial of shape variables with respect to fourier coefficient i + del_shape = grad_alpha_eval{i}; + del_dshape = grad_alphadot_eval{i}; + del_ddshape = grad_alphaddot_eval{i}; + + % Gradient of torque calculation + % Start with effect of gradient on M_alpha*alphaddot + M_temp = zeros(length(shapelist)); + for j = 1:length(shapelist) + M_temp = M_temp + dM{j}*del_shape(j); + end + % Catching for debugging + try + M_grad = M_temp*ddshape(:) + M*del_ddshape(:); + catch + M_temp + end + % Effect of gradient on dM_alphadalpha*alphadot*alphadot + C1_partialgrad = zeros(length(shapelist)); + C1_shapegrad = zeros(length(shapelist)); + C1_outergrad = zeros(length(shapelist)); + del_dM_alphadalpha = cell(size(shapelist)); + for j = 1:length(shapelist) + Cj_temp = zeros(length(shapelist)); + for k = 1:length(shapelist) + Cj_temp = Cj_temp + ddM{j,k}*del_shape(k); + end + del_dM_alphadalpha{j} = Cj_temp; + C1_partialgrad = C1_partialgrad + Cj_temp*dshape(j); + C1_shapegrad = C1_shapegrad + dM{j}*del_dshape(j); + C1_outergrad = C1_outergrad + dM{j}*dshape(j); + end + C1_grad = (C1_partialgrad + C1_shapegrad)*dshape(:) + ... + C1_outergrad*del_dshape(:); + + % Effect of gradient on -(1/2)*alphadot'*dM_alphadalpha*alphadot + C2_grad = zeros(size(shapelist(:))); + for j = 1:length(shapelist) + C2_grad(j) = del_dshape(:)'*dM{j}*dshape(:) + ... + dshape(:)'*del_dM_alphadalpha{j}*dshape(:) + ... + dshape(:)'*dM{j}*del_dshape(:); + end + + % Gradient of torque + del_tau = M_grad + C1_grad - (1/2)*C2_grad; + % Gradient of (P1+P2)^2, aka gradient of v'*tau*v'*tau where v + % is the shape velocity + qual_1 = del_dshape(:)'*tau*dshape(:)'*tau + ... + dshape(:)'*del_tau*dshape(:)'*tau + ... + dshape(:)'*tau*del_dshape(:)'*tau + ... + dshape(:)'*tau*dshape(:)'*del_tau; + % Gradient of P1^2 + P2^2 + qual_2 = (2*del_dshape(:).*dshape(:))'*(tau.*tau) + ... + (dshape(:).*dshape(:))'*(2*del_tau.*tau); + % Gradient of power quality cost wrt this fourier coeff + del_cost(i) = qual_1 - qual_2; + end + del_cost = del_cost(:); +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/readGait.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/readGait.m new file mode 100644 index 00000000..61fe5e55 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/readGait.m @@ -0,0 +1,11 @@ +%Finds value of gait cell function at given time +function state = readGait(gaitFun,t) + + ndim = numel(gaitFun); + state = zeros(1,ndim); + + for i = 1:ndim + state(i) = gaitFun{i}(t); + end + +end diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/se2exp.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/se2exp.m new file mode 100644 index 00000000..2ff1984a --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/se2exp.m @@ -0,0 +1,23 @@ +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 \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/shape_grad.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/shape_grad.m new file mode 100644 index 00000000..8ecb9508 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/shape_grad.m @@ -0,0 +1,85 @@ +function [grad_alphaddot,grad_alphadot,grad_alpha] = shape_grad(n,y,g) +% Calculates the gradient of the shape position, velocity, and acceleration +% with respect to the fourier coefficients. +% Inputs: +% n: Number of points composing the gait when using shape-space +% parametrization +% y: Set of fourier coefficients parametrizing the gait +% g: Time period of the gait + +% Get the fourier frequency, number of shape variables, and number of +% fourier coefficientsinertia_cost_gradient +w = y(end,:); +dim = size(y,2); +num_coeffs = size(y,1); +% Initialize cell array of function handles to hold the partials of the +% shape variables with respect to the fourier coefficients +grad_alpha = cell(num_coeffs,dim); +grad_alphadot = cell(num_coeffs,dim); +grad_alphaddot = cell(num_coeffs,dim); + +for i = 1:num_coeffs + for j = 1:dim + % Coefficient a_0 is a lone scalar, so partials with respect to it + % are zero for the dotted terms and 1 for alpha + if i == 1 + grad_alpha{i,j} = @(t) [0*(1:j-1), 1, 0*(j+1:dim)]; + grad_alphadot{i,j} = @(t) zeros(1,dim); + grad_alphaddot{i,j} = @(t) zeros(1,dim); + continue + elseif i == num_coeffs % Partial w.r.t. frequency + grad_alpha{i,j} = @(t) [0*(1:j-1), t*(-y(2,j)*sin(w(j)*t) + y(3,j)*cos(w(j)*t) - ... + 2*y(4,j)*sin(2*w(j)*t) + 2*y(5,j)*cos(2*w(j)*t) - ... + 3*y(6,j)*sin(3*w(j)*t) + 3*y(7,j)*cos(3*w(j)*t) - ... + 4*y(8,j)*sin(4*w(j)*t) + 4*y(9,j)*cos(4*w(j)*t)), ... + 0*(j+1:dim)]; + + grad_alphadot{i,j} = @(t) [0*(1:j-1), -y(2,j)*(w(j)*t*cos(w(j)*t)+sin(w(j)*t)) + y(3,j)*(-w(j)*t*sin(w(j)*t)+cos(w(j)*t)) + ... + -2*y(4,j)*(2*w(j)*t*cos(2*w(j)*t)+sin(2*w(j)*t)) + 2*y(5,j)*(-2*w(j)*t*sin(2*w(j)*t)+cos(2*w(j)*t)) + ... + -3*y(6,j)*(3*w(j)*t*cos(3*w(j)*t)+sin(3*w(j)*t)) + 3*y(7,j)*(-3*w(j)*t*sin(3*w(j)*t)+cos(3*w(j)*t)) + ... + -4*y(8,j)*(4*w(j)*t*cos(4*w(j)*t)+sin(4*w(j)*t)) + 4*y(9,j)*(-4*w(j)*t*sin(4*w(j)*t)+cos(4*w(j)*t)), ... + 0*(j+1:dim)]; + + grad_alphaddot{i,j} = @(t) [0*(1:j-1), -y(2,j)*w(j)*(-t*w(j)*sin(w(j)*t)+2*cos(w(j)*t)) - y(3,j)*w(j)*(t*w(j)*cos(w(j)*t)+2*sin(w(j)*t)) + ... + -4*y(4,j)*w(j)*(-2*t*w(j)*sin(2*w(j)*t)+2*cos(2*w(j)*t)) - 4*y(5,j)*w(j)*(2*t*w(j)*cos(2*w(j)*t)+2*sin(2*w(j)*t)) + ... + -9*y(6,j)*w(j)*(-3*t*w(j)*sin(3*w(j)*t)+2*cos(3*w(j)*t)) - 9*y(7,j)*w(j)*(3*t*w(j)*cos(3*w(j)*t)+2*sin(3*w(j)*t)) + ... + -16*y(8,j)*w(j)*(-4*t*w(j)*sin(4*w(j)*t)+2*cos(4*w(j)*t)) - 16*y(9,j)*w(j)*(4*t*w(j)*cos(4*w(j)*t)+2*sin(4*w(j)*t)), ... + 0*(j+1:dim)]; + continue + end + % For partial alpha, a_n is associated with cosine and b_n is + % associated with sine; a_n terms are every second row entry in y + % with the b_n terms in between + if mod(i,2) == 0 + trig = @cos; + else + trig = @sin; + end + % mult comes from the multiplier of the natural frequency for + % increasing fourier coefficients + mult = floor(i/2); + + grad_alpha{i,j} = @(t) [0*(1:j-1), trig(mult*w(j)*t), 0*(j+1:dim)]; + % For partial alphadot, a_n is associated with sine and b_n is + % associated with cosine; the a_n terms are every second row entry + % in y with the b_n terms in between + if mod(i,2) == 0 + trig = @sin; + else + trig = @cos; + end + + grad_alphadot{i,j} = @(t) [0*(1:j-1), (-1)^(i-1)*mult*w(j)*trig(mult*w(j)*t), 0*(j+1:dim)]; + + % For partial alphaddot, a_n is associated with cosine and b_n is + % associated with sine + if mod(i,2) == 0 + trig = @cos; + else + trig = @sin; + end + + grad_alphaddot{i,j} = @(t) [0*(1:j-1), -mult^2*w(j)^2*trig(mult*w(j)*t), 0*(j+1:dim)]; + end +end +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/solvedifffmincon.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/solvedifffmincon.m new file mode 100644 index 00000000..f2d07d71 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/solvedifffmincon.m @@ -0,0 +1,319 @@ +function [f,g]=solvedifffmincon(y,s,n,dimension,direction,~,~,~)%,lb,ub,writerObj) +%%%%%%%%%%%%% +% This function calculates efficiency (or displacement, if +% that is the objective function) and its gradient with respect to the coefficients obtained +% by the fourier series parametrization +% +% Inputs: +% +% y: Matrix containing the Fourier series coefficients +% s: System file which contains the connection vector field, CCF's and +% metric data +% dimension: Indicates the number of shape variables of the system +% n: The number of points desired in a direct transcription parametrization +% of the gaits +% direction: direction in which to optimize motion: 1-x, 2-y, 3-theta +% costfunction: costfunction type to optimize for +% lb: Lower bound of shape variables for each point which is obtained from the grid inside +% which an optimal gait is desired +% ub: Upper bound of shape variables for each point which is obtained from the grid inside +% which an optimal gait is desired +% +% Outputs: +% +% f: Objective function value (This is negative of efficiency by default, can be +% changed to displacement) +% g: Gradient of the objective function +%%%%%%%%%%%%% + +%% Obtaining points from fourier coefficients +% The first step is to obtain a direct transcription of the gait from the +% fourier series parametrization. y1 is the matrix of coordinate values of +% the points forming the gait. +afactor=0.001; +coeff=y; +y1 = path_from_fourier(y,n,dimension); +y1 = y1(1:end-1,:); % Remove the last points because path_from_fourier returns self-connected gait + +%% Calculating cost and displacement per gait + +w1 = y(end,1); % Frequency of Fourier transform +w2 = y(end,2); +% Assign a time period for executing the gait +T = 2*pi/w1; + +% Define phi_def = [alpha1, alpha2] as a function of time t such that the +% array returns the shape variables given by the fourier coefficients at a +% time t +p = makeGait(y); + +% % Uncomment this section to verify that the shape variables and derivatives +% % have been derived appropriately +% valid = verify_shape_equations(p); +% valid_M = verify_mass_derivative(s); +% validate_cost_gradient(s,n,y,T,p); + +% Reassignment of point transcription to variable y for compatibility with +% old version +clear y +y=y1; +clear y1; + +global bestCost bestDisp bestEff; + +% Calculate displacement, cost and efficiency of a gait +% Note that, for inertial cost, cost is returned as the integral of torque +% squared, while for drag-based systems, cost is the path length of the +% gait +[~, net_disp_opt, cost] = evaluate_displacement_and_cost1(s,p,[0, T],'interpolated','fixed_step'); +lineint=net_disp_opt(direction); % displacement produced in the chosen direction produced on executing the gait measured in the optimal coordinates + +% Assign value for totalstroke, i.e. the cost metric used for efficiency +% calculation +if strcmpi(s.costfunction,'torque') || strcmpi(s.costfunction,'covariant acceleration') || strcmpi(s.costfunction,'acceleration coord') || strcmpi(s.costfunction,'power quality') + % With cost as time period, period of the gait is the cost to execute + % the gait at unit torque squared to the 1/4th power + totalstroke = cost^(1/4); +else + % Drag systems have cost as path length, so no modification is needed + totalstroke = cost; +end + +% If efficiency is negative, reversing the order of points so that +% efficiency is positive +if lineint<0 + lineint= -lineint; + ytemp=y; + for i=1:n + y(i,:)=ytemp(n+1-i,:); + end + invert=1; +else + invert=0; +end + +%% Preliminaries for gradient calculation +% Preallocating memory for variables which we will need in further +% calculation +yvalues=cell(n,dimension); % Cell representation of the coordinates of all points forming the gait +interpstateccf=cell(1,dimension); % Variable which will store the ccf function grid used for interpolation +interpmetricgrid=cell(1,dimension); % Variable which will store the metric grid used for interpolation +ccf=zeros(n,dimension*(dimension-1)/2); % Variable which will store ccf function at each point +metric1=zeros(n,dimension,dimension); % Variable which will store metric at each point in the form of a matrix +metric = repmat({zeros(dimension)},[n 1]); % Variable which stores the metric at each point in the form of a 2x2 matrix +metricgrad1=zeros(n,dimension,dimension,dimension); % Variable which will store gradient of metric at each point in the form of a matrix +metricgrad = repmat({zeros(dimension)},[n,dimension]); % Variable which will store gradient of metric at each point in the form of a matrix + +% Interpolation to calculate all the variables needed for gradient +% calculation +for i=1:1:n + for j=1:1:dimension + yvalues{i,j}=y(i,j); + end +end + +y_for_interp = mat2cell(y,size(y,1),ones(1,size(y,2))); + +for j=1:1:dimension + interpstateccf{j}=s.grid.eval{j,1}; + interpmetricgrid{j}=s.grid.metric_eval{j,1}; +end + +for j=1:dimension*(dimension-1)/2 + ccf(:,j)=interpn(interpstateccf{:},s.DA_optimized{direction,j},y_for_interp{:},'spline'); +end + +for j=1:1:dimension + for k=1:1:dimension + metric1(:,j,k)=interpn(interpmetricgrid{:},s.metricfield.metric_eval.content.metric{j,k},y_for_interp{:},'spline'); + end +end + +if strcmpi(s.costfunction,'pathlength coord') || strcmpi(s.costfunction,'acceleration coord') + for i=1:n + metric{i}=eye(dimension); + end +elseif strcmpi(s.costfunction,'pathlength metric') || strcmpi(s.costfunction,'pathlength metric2') + for i=1:n + for j=1:1:dimension + for k=1:1:dimension + metric{i}(j,k)=metric1(i,j,k); + end + end + end +end +if strcmpi(s.costfunction,'pathlength metric2') + for i = 1:n + metric{i} = metric{i}*metric{i}; + end +end + +if strcmpi(s.costfunction,'pathlength coord') || strcmpi(s.costfunction,'acceleration coord') + for i=1:dimension + for j=1:n + metricgrad{j,i} = zeros(dimension); + end + end +elseif strcmpi(s.costfunction,'pathlength metric') || strcmpi(s.costfunction,'pathlength metric2') + y2 = zeros(size(y)); + y1 = y2; + for l=1:1:dimension + for m=1:1:dimension + if m==l + y2(:,m)=y(:,m)+afactor*ones(length(y),1); + y1(:,m)=y(:,m)-afactor*ones(length(y),1); + else + y2(:,m)=y(:,m); + y1(:,m)=y(:,m); + end + end + y2_for_interp = mat2cell(y2,size(y,1),ones(1,size(y,2))); + y1_for_interp = mat2cell(y1,size(y,1),ones(1,size(y,2))); + for j=1:1:dimension + for k=1:1:dimension + metricgrad1(:,l,j,k)=(interpn(interpmetricgrid{:},s.metricfield.metric_eval.content.metric{j,k},y2_for_interp{:},'spline')... + -interpn(interpmetricgrid{:},s.metricfield.metric_eval.content.metric{j,k},y1_for_interp{:},'spline'))/(2*afactor); + end + end + for i=1:n + for j=1:1:dimension + for k=1:1:dimension + metricgrad{i,l}(j,k)=metricgrad1(i,l,j,k); + end + end + end + end +end +if strcmpi(s.costfunction,'pathlength metric2') + for l = 1:dimension + for i = 1:n + metricgrad{i,l} = metricgrad{i,l}*metric{i}+metric{i}*metricgrad{i,l}; + end + end +end + +%% changey/dcoeff tells us how much each point moves when a fourier series variable is changed +% chy is a cell with as many entries as the dimension of the shape space +% ith element of chy is a matrix where the (j,k)th entry tells us the change in the ith coordinate +% of the kth point of the gait resulting from a unit change in the jth +% fourier coefficient corresponding to the ith dimension of the shape space + +chy=cell(dimension,1); +% Create vector of time values at which to evaluate points of gait +t = linspace(0,T,n); +for i=1:1:dimension + for j=1:1:n + chy{i}(:,j)=[1;cos(t(j)*coeff(end,i));sin(t(j)*coeff(end,i));cos(2*t(j)*coeff(end,i));sin(2*t(j)*coeff(end,i));cos(3*t(j)*coeff(end,i));sin(3*t(j)*coeff(end,i));cos(4*t(j)*coeff(end,i));sin(4*t(j)*coeff(end,i))];%cos(5*t(j)*coeff(end,i));sin(5*t(j)*coeff(end,i))];%;cos(6*t(j)*coeff(end,i));sin(6*t(j)*coeff(end,i))];% + end +end + +%% Jacobianstroke is the gradient of cost. +%Contrigrad is the contribution to the gradient due to the metric changing +switch s.costfunction + case {'pathlength metric','pathlength coord','pathlength metric2'} + % Get the gradient of cost based on drag-dominated system + jacobianstroke = jacobianstrokecalculator(y,n,dimension,metric,metricgrad); + case {'torque','covariant acceleration','acceleration coord','power quality'} + % Get the gradient of cost based on inertia-dominated system + inertia_cost_grad = inertia_cost_gradient(s,n,coeff,T,p,'discrete'); + + % With cost as time period to execute the gait, the gradient of + % cost for inertial systems is the gradient of cost with period 1 + % divided by (4*T^3) + inertia_cost_grad = inertia_cost_grad./(4*totalstroke^3); + otherwise + error('Unexpected system type at cost gradient calculation!') +end + +%% Jacobiandisp is the gradient of displacement. +% jacobiandispcalculator3 is the function that calculates the gradient of +% displacement for the ith point. It's input arguments are the coordinates of +% the (i-1)th, ith and (i+1)th point, CCF value at point i and the dimension of +% the shape space (dimension) + +jacobiandisp = zeros(n,dimension); +for i=2:1:n-1 + jacobiandisp(i,:)=jacobiandispcalculator3(y(i-1,:),y(i,:),y(i+1,:),ccf(i,:),dimension); +end +jacobiandisp(1,:)=jacobiandispcalculator3(y(n,:),y(1,:),y(2,:),ccf(1,:),dimension); +jacobiandisp(n,:)=jacobiandispcalculator3(y(n-1,:),y(n,:),y(1,:),ccf(n,:),dimension); + +%% Jacobianeqi is the concentration gradient. +%It is the term that keeps points eqi distant from each other and prevents crossover of gait. + +jacobianeqi = jacobianeqicalculator(y,n,dimension,metric); + +%% properly ordering gradients depending on wether lineint was negative or positive +if invert + jacobiandisptemp=jacobiandisp; + if strcmpi(s.costfunction,'pathlength coord') || strcmpi(s.costfunction,'pathlength metric') || strcmpi(s.costfunction,'pathlength metric2') + jacobianstroketemp=jacobianstroke; + end + jacobianeqitemp=jacobianeqi; + for i=1:1:n + jacobiandisp(i,:)=jacobiandisptemp(n+1-i,:); + if strcmpi(s.costfunction,'pathlength coord') || strcmpi(s.costfunction,'pathlength metric') || strcmpi(s.costfunction,'pathlength metric2') + jacobianstroke(i,:)=jacobianstroketemp(n+1-i,:); + end + jacobianeqi(i,:)=jacobianeqitemp(n+1-i,:); + + end +end + +%% fourier series version of all gradients + +% We then obtain gradients in a fourier series parametrization by +% projecting the gradients from the direct transcription space onto the +% fourier coefficient space +for i=1:1:dimension + for j=1:1:9 + jacobiandispfourier(j,i)=chy{i}(j,:)*jacobiandisp(:,i); + if strcmpi(s.costfunction,'pathlength coord') || strcmpi(s.costfunction,'pathlength metric') || strcmpi(s.costfunction,'pathlength metric2') + jacobianstrokefourier(j,i)=chy{i}(j,:)*jacobianstroke(:,i); + end + jacobianeqifourier(j,i)=chy{i}(j,:)*jacobianeqi(:,i); + end +end +if strcmpi(s.costfunction,'torque') || strcmpi(s.costfunction,'covariant acceleration') || strcmpi(s.costfunction,'acceleration coord') || strcmpi(s.costfunction,'power quality') + % Inertia cost gradient is already in terms of the fourier coefficients + jacobianstrokefourier = inertia_cost_grad; + % Add zero terms to the gradient of displacement and jacobianeqi for + % frequency terms, since the inertia gradient includes those terms + jacobiandispfourier = [jacobiandispfourier;zeros(1,dimension)]; + jacobianeqifourier = [jacobianeqifourier;zeros(1,dimension)]; + % Calculate the total gradient of efficiency + % jacobianeqifourier commented out at Hossein's suggestion - don't + % necessarily want the points to all be equally spaced + totaljacobianfourier = jacobiandispfourier/totalstroke-lineint*jacobianstrokefourier/totalstroke^2;%+jacobianeqifourier; + % reset the gradient of the frequency terms to be zero so they aren't + % changed + totaljacobianfourier(end,:) = zeros(1,dimension); +else + totaljacobianfourier = jacobiandispfourier/totalstroke-lineint*jacobianstrokefourier/totalstroke^2+jacobianeqifourier; +end + +%% minimizing negative of efficiency(or displacement) +f=-lineint/(totalstroke); % Optimizing for displacement over cost +% f = -lineint; % Optimizing for displacement only + +if abs(f) > bestEff + bestEff = abs(f); + bestDisp = abs(lineint); + bestCost = abs(totalstroke); +end + +if nargout>1 + if strcmpi(s.costfunction,'torque') || strcmpi(s.costfunction,'covariant acceleration') || strcmpi(s.costfunction,'acceleration coord') || strcmpi(s.costfunction,'power quality') + % Return the gradient of efficiency as previously calculated for + % inertia systems + g = -totaljacobianfourier; % Optimizing for displacement over cost +% g = -jacobiandispfourier; % Optimizing for displacement only + else + % Return the gradient of efficiency plus row of zeros for frequency + % terms for drag systems + g=[-totaljacobianfourier;zeros(1,dimension)]; % Optimizing for displacement over cost +% g = [-jacobiandispfourier;zeros(1,dimension)]; % Optimizing for displacement only + end +end + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/torque_cost.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/torque_cost.m new file mode 100644 index 00000000..ac8840c1 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/torque_cost.m @@ -0,0 +1,19 @@ +function dcost = torque_cost(M,dM,shape,dshape,ddshape,metric) +% Calculates the incremental cost for an inertial system where cost is torque squared. +% Inputs: +% M: Mass matrix +% dM_alphadalpha: Partial of mass matrix with respect to shape variables; +% must be cell where the ith entry is the partial with respect to the +% ith shape variable +% shape: Current shape configuration of system, at which M and +% dM_alphadalpha were evaluated +% dshape: Current shape velocity of system +% ddshape: Current shape acceleration of system + + % Start by calculating the coriolis matrix + C = calc_coriolis_matrix(dM,shape,dshape); + % Calculate the torque for this instant of time and return the inner + % product of the torque with itself + dtau = M*ddshape(:) + C; + dcost = dtau'*dtau; +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/validate_cost_gradient.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/validate_cost_gradient.m new file mode 100644 index 00000000..79990319 --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/validate_cost_gradient.m @@ -0,0 +1,77 @@ +function validate_cost_gradient(s,n,y,g,p) +% Function to help verify that the cost gradient calculated by +% inertia_gradient_helper is valid. Values returned by function and +% individually calculated costs are printed to the terminal along with the +% difference between the two methods. Should be relatively small for full +% range of fourier coefficients. +% Inputs: +% s: System struct used by sysplotter. +% n: Number of points at which gait should be evaluated in the shape +% space. +% y: Fourier coefficients that parametrize the gait. +% g: Time period over which gait is executed. +% p: Struct containing fields: +% phi_def: array function that returns shape at time value t +% dphi_def: array function that returns shape velocity at time t +% ddphi_def: array function that returns shape acceleration at time t + +% Set the delta in the fourier coefficients between individual cost +% evaluations +fourier_delta = 0.001; +[grad_alphaddot,grad_alphadot,grad_alpha] = shape_grad(n,y,g); +% Go through each of the fourier coefficients and test how applying +% fourier_delta to each of them matches with the cost gradient calculated +% by inertia_gradient_helper +for fourier_test = 1:numel(y) + % Perturb the fourier coefficient to be tested by fourier_delta and + % create a parametrization for calculating the point in the gait at a + % time t + y2 = y; + y2(fourier_test) = y2(fourier_test) + fourier_delta; + w1 = y2(end,1); % Frequency of Fourier transform + w2 = y2(end,2); + p2 = makeGait(y2); + + % Perturb again in the opposite direction + y2 = y; + y2(fourier_test) = y2(fourier_test) - fourier_delta; + w1 = y2(end,1); % Frequency of Fourier transform + w2 = y2(end,2); + p3 = makeGait(y2); + + % Get the shape and shape derivative at a random time for each gait + t = g*rand(1); + shape = readGait(p3.phi_def,t); + shapelist = num2cell(shape); + dshape = readGait(p3.dphi_def,t); + ddshape = readGait(p3.ddphi_def,t); + shape_delta = readGait(p2.phi_def,t); + shapelist_delta = num2cell(shape_delta); + dshape_delta = readGait(p2.dphi_def,t); + ddshape_delta = readGait(p2.ddphi_def,t); + + % Get mass matrices at both locations + M = cellfun(@(C) interpn(s.grid.mass_eval{:},C,... + shapelist{:},'spline'),s.massfield.mass_eval.content.M_alpha); + M_delta = cellfun(@(C) interpn(s.grid.mass_eval{:},C,... + shapelist_delta{:},'spline'),s.massfield.mass_eval.content.M_alpha); + + % Get partial mass matrices at both locations + dM_alphadalpha = calc_partial_mass(s,shapelist); + dM_alphadalpha_delta = calc_partial_mass(s,shapelist_delta); + + % Calculate the cost using the two different gaits + cost = torque_cost(M,dM_alphadalpha,shape,dshape,ddshape); + cost_delta = torque_cost(M_delta,dM_alphadalpha_delta,shape_delta,dshape_delta,ddshape_delta); + % Calculate what the gradient of cost is for this particular point + cost_grad = inertia_gradient_helper(t,[],s,p,grad_alpha,grad_alphadot,grad_alphaddot); + cost_grad = reshape(cost_grad,size(y)); + cost_grad_rel = cost_grad(fourier_test) + cost_grad_calc = (cost_delta-cost)/(2*fourier_delta) + + % Find what the difference is between cost_grad and the costs evaluated + % at distance fourier_delta + err = cost_grad_rel - cost_grad_calc +end + +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/validate_shape_gradient.m b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/validate_shape_gradient.m new file mode 100644 index 00000000..b7b5febf --- /dev/null +++ b/ProgramFiles/GaitOptimization/optimalgaitgenerator_fcns/validate_shape_gradient.m @@ -0,0 +1,52 @@ +function validate_shape_gradient(n,y,g,grad_alphaddot,grad_alphadot,grad_alpha) %#ok +% Function that helps validate that the gradient of shape position, +% velocity, and acceleration are correctly calculated. Difference between +% the input gradients and calculation-verified gradients are printed to the +% terminal. Should be very close to zero. +% Inputs: +% n: Number of points at which gait should be evaluated in shape space +% y: Fourier coefficients that parametrize the gait +% g: Time period over which gait is executed +% grad_alphaddot: Gradient of shape acceleration with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time +% grad_alphadot: Gradient of shape velocity with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time +% grad_alpha: Gradient of shape position with respect to the +% fourier coefficients, cell array of same dimension as y where each +% entry is an array function of time + + % Select 10 random times at which to evaluate the gradient of shape + t = g*rand(1,10); + w1 = y(end,1); % Frequency of Fourier transform + w2 = y(end,2); + + % Evaluate the gradient at each random time against a hard-coded + % gradient calculation + for i = 1:length(t) + grad_alpha_eval = cellfun(@(C) C(t(i)), grad_alpha, 'UniformOutput', false); + grad_alpha1_eval = cell2mat(grad_alpha_eval(:,1)); + grad_alpha2_eval = cell2mat(grad_alpha_eval(:,2)); + grad_alpha1_calc = [1,0;cos(w1*t(i)),0;sin(w1*t(i)),0;cos(2*w1*t(i)),0;sin(2*w1*t(i)),0;cos(3*w1*t(i)),0;sin(3*w1*t(i)),0;cos(4*w1*t(i)),0;sin(4*w1*t(i)),0;0,0]; + grad_alpha2_calc = [0,1;0,cos(w2*t(i));0,sin(w2*t(i));0,cos(2*w2*t(i));0,sin(2*w2*t(i));0,cos(3*w2*t(i));0,sin(3*w2*t(i));0,cos(4*w2*t(i));0,sin(4*w2*t(i));0,0]; + grad_alpha1_err = grad_alpha1_eval - grad_alpha1_calc + grad_alpha2_err = grad_alpha2_eval - grad_alpha2_calc + + grad_alphadot_eval = cellfun(@(C) C(t(i)), grad_alphadot, 'UniformOutput', false); + grad_alphadot1_eval = cell2mat(grad_alphadot_eval(:,1)); + grad_alphadot2_eval = cell2mat(grad_alphadot_eval(:,2)); + grad_alphadot1_calc = [0,0;-w1*sin(w1*t(i)),0;w1*cos(w1*t(i)),0;-2*w1*sin(2*w1*t(i)),0;2*w1*cos(2*w1*t(i)),0;-3*w1*sin(3*w1*t(i)),0;3*w1*cos(3*w1*t(i)),0;-4*w1*sin(4*w1*t(i)),0;4*w1*cos(4*w1*t(i)),0;0,0]; + grad_alphadot2_calc = [0,0;0,-w2*sin(w2*t(i));0,w2*cos(w2*t(i));0,-2*w2*sin(2*w2*t(i));0,2*w2*cos(2*w2*t(i));0,-3*w2*sin(3*w2*t(i));0,3*w2*cos(3*w2*t(i));0,-4*w2*sin(4*w2*t(i));0,4*w2*cos(4*w2*t(i));0,0]; + grad_alphadot1_err = grad_alphadot1_eval - grad_alphadot1_calc; + grad_alphadot2_err = grad_alphadot2_eval - grad_alphadot2_calc; + + grad_alphaddot_eval = cellfun(@(C) C(t(i)), grad_alphaddot, 'UniformOutput', false); + grad_alphaddot1_eval = cell2mat(grad_alphaddot_eval(:,1)); + grad_alphaddot2_eval = cell2mat(grad_alphaddot_eval(:,2)); + grad_alphaddot1_calc = [0,0;-w1^2*cos(w1*t(i)),0;-w1^2*sin(w1*t(i)),0;-4*w1^2*cos(2*w1*t(i)),0;-4*w1^2*sin(2*w1*t(i)),0;-9*w1^2*cos(3*w1*t(i)),0;-9*w1^2*sin(3*w1*t(i)),0;-16*w1^2*cos(4*w1*t(i)),0;-16*w1^2*sin(4*w1*t(i)),0;0,0]; + grad_alphaddot2_calc = [0,0;0,-w2^2*cos(w2*t(i));0,-w2^2*sin(w2*t(i));0,-4*w2^2*cos(2*w2*t(i));0,-4*w2^2*sin(2*w2*t(i));0,-9*w2^2*cos(3*w2*t(i));0,-9*w2^2*sin(3*w2*t(i));0,-16*w2^2*cos(4*w2*t(i));0,-16*w2^2*sin(4*w2*t(i));0,0]; + grad_alphaddot1_err = grad_alphaddot1_eval - grad_alphaddot1_calc; + grad_alphaddot2_err = grad_alphaddot2_eval - grad_alphaddot2_calc; + end +end \ No newline at end of file diff --git a/ProgramFiles/GaitOptimization/pathlengthcost.m b/ProgramFiles/GaitOptimization/pathlengthcost.m new file mode 100644 index 00000000..486ab40b --- /dev/null +++ b/ProgramFiles/GaitOptimization/pathlengthcost.m @@ -0,0 +1,59 @@ +function [jacobianstroke,jacobianeqi] = pathlengthcost(y,M,dM,P) + +n = P.N; +dimension = P.shv; +jacobianstroke = zeros(n,dimension); +contrigrad=zeros(n,dimension); + +% Calculate the length of segments +for i=1:1:n-1 + L(i) = sqrt((y(i+1,:)-y(i,:))*((M{i}+M{i+1})/2)*(y(i+1,:)-y(i,:))'); +end +L(n)=sqrt((y(1,:)-y(n,:))*((M{n}+M{1})/2)*(y(1,:)-y(n,:))'); + + +%% Jacobiandisp-jacobian for displacement produced by gait +for i=1:n-1 + delp{i}=y(i+1,:)-y(i,:); +end +delp{n}=y(1,:)-y(n,:); + +for i=2:n-1 + for j=1:dimension + contrigrad(i,j)=0.5*delp{i}*dM{i,j}*delp{i}'/(2*L(i))+0.5*delp{i-1}*dM{i,j}*delp{i-1}'/(2*L(i-1)); + end + jacobianstroke(i,:)=(-(((M{i}+M{i+1})/2)*delp{i}')'-(delp{i}*((M{i}+M{i+1})/2)))/(2*L(i))+((((M{i-1}+M{i})/2)*delp{i-1}')'+(delp{i-1}*((M{i}+M{i-1})/2)))/(2*L(i-1))+contrigrad(i,:);%+0.5*[delp{i}*metricgradx{i}*delp{i}',delp{i}*metricgrady{i}*delp{i}',delp{i}*metricgradz{i}*delp{i}']/(2*l(i))+0.5*[delp{i-1}*metricgradx{i}*delp{i-1}',delp{i-1}*metricgrady{i}*delp{i-1}',delp{i-1}*metricgradz{i}*delp{i-1}']/(2*l(i-1)); +end + +for j=1:dimension + contrigrad(1,j)=0.5*delp{1}*dM{1,j}*delp{1}'/(2*L(1))+0.5*delp{n}*dM{1,j}*delp{n}'/(2*L(n)); +end +jacobianstroke(1,:)=(-(((M{1}+M{2})/2)*delp{1}')'-(delp{1}*((M{1}+M{2})/2)))/(2*L(1))+((((M{n}+M{1})/2)*delp{n}')'+(delp{n}*((M{n}+M{1})/2)))/(2*L(n))+contrigrad(1,:);%+0.5*[delp{1}*metricgradx{1}*delp{1}',delp{1}*metricgrady{1}*delp{1}',delp{1}*metricgradz{1}*delp{1}']/(2*l(1))+0.5*[delp{n}*metricgradx{1}*delp{n}',delp{n}*metricgrady{1}*delp{n}',delp{n}*metricgradz{1}*delp{n}']/(2*l(n)); + +for j=1:dimension + contrigrad(n,j)=0.5*delp{n}*dM{n,j}*delp{n}'/(2*L(n))+0.5*delp{n-1}*dM{n,j}*delp{n-1}'/(2*L(n-1)); +end +jacobianstroke(n,:)=(-(((M{n}+M{1})/2)*delp{n}')'-(delp{n}*((M{n}+M{1})/2)))/(2*L(n))+((((M{n}+M{n-1})/2)*delp{n-1}')'+(delp{n-1}*((M{n}+M{n-1})/2)))/(2*L(n-1))+contrigrad(n,:);%+0.5*[delp{n}*metricgradx{n}*delp{n}',delp{n}*metricgrady{n}*delp{n}',delp{n}*metricgradz{n}*delp{n}']/(2*l(n))+0.5*[delp{n-1}*metricgradx{n}*delp{n-1}',delp{n-1}*metricgrady{n}*delp{n-1}',delp{n-1}*metricgradz{n}*delp{n-1}']/(2*l(n-1)); + + +%% Equi-distance jacobian +for i=2:n-1 + len=sqrt((y(i+1,:)-y(i-1,:))*((M{i-1}+M{i+1})/2)*(y(i+1,:)-y(i-1,:))'); + midpoint=y(i-1,:)+((y(i+1,:)-y(i-1,:))*sqrtm((M{i-1}+M{i+1})/2))/2; + betacos=(y(i+1,:)-y(i-1,:))*sqrtm((M{i-1}+M{i+1})/2)*((y(i,:)-y(i-1,:))*sqrtm((M{i-1}+M{i})/2))'/(L(i-1)*len); + xhat=y(i-1,:)+(y(i+1,:)-y(i-1,:))*sqrtm((M{i-1}+M{i+1})/2)*L(i-1)*betacos/len; + jacobianeqi(i,:)=midpoint-xhat; +end + + len=sqrt((y(2,:)-y(n,:))*((M{2}+M{n})/2)*(y(2,:)-y(n,:))'); + midpoint=y(n,:)+((y(2,:)-y(n,:))*sqrtm((M{n}+M{2})/2))/2; + betacos=(y(2,:)-y(n,:))*sqrtm((M{n}+M{2})/2)*((y(1,:)-y(n,:))*sqrtm((M{n}+M{1})/2))'/(L(n)*len); + xhat=y(n,:)+(y(2,:)-y(n,:))*sqrtm((M{n}+M{2})/2)*L(n)*betacos/len; + jacobianeqi(1,:)=(midpoint-xhat); + + len=sqrt((y(1,:)-y(n-1,:))*((M{1}+M{n-1})/2)*(y(1,:)-y(n-1,:))'); + midpoint=y(n-1,:)+((y(1,:)-y(n-1,:))*sqrtm((M{1}+M{n-1})/2))/2; + betacos=(y(1,:)-y(n-1,:))*sqrtm((M{n-1}+M{1})/2)*((y(n,:)-y(n-1,:))*sqrtm((M{n-1}+M{n})/2))'/(L(n-1)*len); + xhat=y(n-1,:)+(y(1,:)-y(n-1,:))*sqrtm((M{n-1}+M{1})/2)*L(n-1)*betacos/len; + jacobianeqi(n,:)=(midpoint-xhat); + diff --git a/ProgramFiles/GaitOptimization/velocity_calc1.m b/ProgramFiles/GaitOptimization/velocity_calc1.m new file mode 100644 index 00000000..e5b3a9dc --- /dev/null +++ b/ProgramFiles/GaitOptimization/velocity_calc1.m @@ -0,0 +1,98 @@ +function [L_i,L_is1,L_is2,L_ip1, dL_i_x,dL_is1_x,dL_i_y,dL_is1_y] = velocity_calc1(i,r,M,dM,shv,cons) +% This code create the local normalization + + +% Find the angle of horizental line +SE2 = @(t) [cos(t) -sin(t);sin(t) cos(t)]; + + +% Fit the plane over the 3 points +xnew = [(r(i+1,:) - r(i-1,:))/(norm(r(i+1,:)-r(i-1,:))) 0]; +v_needed = [(r(i+1,:) - r(i,:))/(norm(r(i+1,:) - r(i,:))) 0]; + +znew_temp = cross(xnew,v_needed); +znew = znew_temp/norm(znew_temp); + +ynew_temp = cross(znew,xnew); +ynew = ynew_temp/norm(ynew_temp); + +x0 = [1 0 0]; +y0 = [0 1 0]; +z0 = [0 0 1]; +% Make the transformation matrix +Tr = [dot(x0,xnew) dot(x0,ynew) dot(x0,znew); + dot(y0,xnew) dot(y0,ynew) dot(y0,znew); + dot(z0,xnew) dot(z0,ynew) dot(z0,znew)]; + +% % Determine the angle from point 1 to point 3 for transforming the three +th = atan2(r(i+1,2)-r(i-1,2), r(i+1,1)-r(i-1,1)); +Tr = SE2(th); +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Obtain the length of Triangle sides +L_i = sqrt((r(i+1,:)-r(i,:))*((M{i}+M{i+1})/2)*(r(i+1,:)-r(i,:))'); +L_is1 = sqrt((r(i,:)-r(i-1,:))*((M{i-1}+M{i})/2)*(r(i,:)-r(i-1,:))'); +if i ~= 2 + L_is2 = sqrt((r(i-1,:)-r(i-2,:))*((M{i-1}+M{i-2})/2)*(r(i-1,:)-r(i-2,:))'); +else + L_is2 = 0; +end + + L_ip1 = sqrt((r(i+2,:)-r(i+1,:))*((M{i+2}+M{i+1})/2)*(r(i+2,:)-r(i+1,:))'); + + +for j = 1:shv + + dr = zeros(1,shv); + dr(j) = 1; + + dL_i{j} = 1*(-dr*((M{i}+M{i+1} )/2)*(r(i+1,:)-r(i,:))' + (r(i+1,:)-r(i,:))*((M{i}+M{i+1} )/2)*-dr' + (r(i+1,:)-r(i,:))*((dM{i,j} )/2)*(r(i+1,:)-r(i,:))')/(2*L_i); + dL_is1{j} = 1*(dr*((M{i-1}+M{i} )/2)*(r(i,:)-r(i-1,:))' + (r(i,:)-r(i-1,:))*((M{i-1}+M{i} )/2)*dr' + (r(i,:)-r(i-1,:))*(( dM{i,j})/2)*(r(i,:)-r(i-1,:))')/(2*L_is1); + +end + + +dL_i_x = cell2mat(dL_i)*Tr; +dL_is1_x = cell2mat(dL_is1)*Tr; +dL_i_x(2) = 0; +dL_is1_x(2) = 0; +dL_i_x = dL_i_x*inv(Tr); % Velocity in tangent direction is zero +dL_is1_x = dL_is1_x*inv(Tr); % Velocity only in tangent direction exist + +dL_i_y = cell2mat(dL_i)*Tr; +dL_is1_y = cell2mat(dL_is1)*Tr; +dL_i_y(1) = 0; +dL_is1_y(1) = 0; +dL_i_y = dL_i_y*inv(Tr); % Velocity in tangent direction is zero +dL_is1_y = dL_is1_y*inv(Tr); % Velocity only in tangent direction exist + + +% if ~isempty(cons) +% % First need to check the node +% for j = 1:length(cons.path_points) +% +% if ismember(i,cons.path_points{j}) +% +% T_rail_normalized = cons.T_rail{j}/norm(cons.T_rail{j}); +% dL1 = dot(dL1,T_rail_normalized)*T_rail_normalized; +% dL2 = dot(dL2,T_rail_normalized)*T_rail_normalized; +% +% % Find the gradiant to keep the path on the rail +% F = 10*cons.connecting_vector{j}; +% +% else +% +% F = 0; +% +% end +% +% end +% +% else +% +% F = 0; +% +% end + + + +end diff --git a/ProgramFiles/Geometry/ContinuousBackbone/backbone_conversion_factors.m b/ProgramFiles/Geometry/ContinuousBackbone/backbone_conversion_factors.m index d42c1b81..f49b1e3a 100644 --- a/ProgramFiles/Geometry/ContinuousBackbone/backbone_conversion_factors.m +++ b/ProgramFiles/Geometry/ContinuousBackbone/backbone_conversion_factors.m @@ -37,108 +37,45 @@ calc_J = 0; end +%%%%%%%%%%%%%%%%%%% +% If baseframe is specified as a non-cell object, wrap it into a cell array +if ~iscell(baseframe) + baseframe = {baseframe}; +end + +% Depending on which baseframe option is specified, use different methods +% to calculate transform and Jacobian to use in the conversion % Depending on which baseframe option is specified, use different methods % to calculate transform and Jacobian to use in the conversion -switch baseframe +for idx_baseframe = 1:numel(baseframe) - % Places the reference frame at the leftmost (lowest s) point on the - % backbone - case 'tail' + if ischar(baseframe{idx_baseframe}) || isscalar(baseframe{idx_baseframe}) - % Identify the leftmost portion of the body - s_zero = -0.5; - - % Extract the transform for this frame - frame_zero = vec_to_mat_SE2(h(s_zero)); - - % Jacobian for tail frame - if calc_J - J_zero = TgLginv(frame_zero)*J(s_zero); - end + switch baseframe{idx_baseframe} + + % Places the reference frame at the leftmost (lowest s) point on the + % backbone + case 'tail' - % Places the reference frame at the middle of the chain, splitting the - % difference between the two middle links if there is an even number of - % links - case {'centered','center','midpoint-tangent'} + % Identify the leftmost portion of the body + s_zero = -0.5; + % Extract the transform for this frame + frame_zero = vec_to_mat_SE2(h(s_zero)); - % Identify the middle portion of the body - s_zero = 0; - - % Extract the transform for this frame - frame_zero = vec_to_mat_SE2(h(s_zero)); - - % Jacobian for middle frame - if calc_J - J_zero = TgLginv(frame_zero)*J(s_zero); - end - - % Places the reference frame on the rightmost (highest s) point on the - % backbone - case {'head','head-tip'} - - % Identify the leftmost portion of the body - s_zero = 0.5; - - % Extract the transform for this frame - frame_zero = vec_to_mat_SE2(h(s_zero)); - - % Jacobian for tail frame - if calc_J - J_zero = TgLginv(frame_zero)*J(s_zero); - end + % Jacobian for tail frame + if calc_J + J_zero = TgLginv(frame_zero)*J(s_zero); + end + % Places the reference frame at the middle of the chain, splitting the + % difference between the two middle links if there is an even number of + % links + case {'centered','center','midpoint-tangent'} - % Places the reference frame at center of mass and average orientation - % of the backbone, assuming constant mass-per-s density - case 'com-mean' - - % Sample the backbone at a dense set of points - h_points = h(linspace(-.5,.5,100)); - % Average the x, y, and theta components. The theta value here is - % integrated local rotation, so we don't need to worry about - % unwrapping it - CoM = mean(h_points,2); - - % Place the new frame at this location - frame_zero = vec_to_mat_SE2(CoM); - - %%%%%%%%%%% - % The Jacobian of the weighted average of frames is the - % weighted average of their Jacobian (by the commutativity of - % sumation and derivation operations). - - if calc_J - - % Sample the Jacobian at a dense set of points - J_points = J(linspace(-.5,.5,100)); - - % Average the Jacobians - J_zero = mean(J_points,3); - - % Bring into local coordinates - J_zero = TgLginv(frame_zero)*J_zero; - - end - - %%%%%%%%%%% - % Several possible cases are handled within this "otherwise" case, - % because they need if/else logic - otherwise - - %%%%%%% - % If a number is provided as the input, place the baseframe on that - % link - % Check for numeric baseframe specification - if isnumeric(baseframe) - - % Make sure that base frame specification is actually in - % the range of valid link numbers - if baseframe >= -0.5 && baseframe <= 0.5 - - % Set the base frame as numerically specified in the input - s_zero = baseframe; + % Identify the middle portion of the body + s_zero = 0; % Extract the transform for this frame frame_zero = vec_to_mat_SE2(h(s_zero)); @@ -147,180 +84,285 @@ if calc_J J_zero = TgLginv(frame_zero)*J(s_zero); end - - else - - error (['Baseframe specification ' num2str(baseframe) ' is not in the range of -0.5 to 0.5']) - - end - - %%%%%%%% - % If the provided string is the name of a sysf_ system file, pull - % the transformation to minimum-perturbation coordinates from that file - else - - %%%%% - % Robustly check if the baseframe string is a sysf_ file that - % has had minimum-perturbation coordinates calculated - - %%% - % First, strip off sysf_ if it was included by the user - - if strncmp(baseframe,'sysf_',5) - baseframe1 = baseframe(6:end); - else - baseframe1 = baseframe; - end - - %%% - % Second, put sysf_ onto the front of the string - - baseframe1 = ['sysf_' baseframe1]; - - %%% - % Third, check if there is a sysf_ file with this name - - load('sysplotter_config.mat','inputpath') % Get the path to the current UserFiles folder - - sysf_filepath = fullfile(inputpath, 'Systems',... % Build a string to a sysf_ file with the baseframe name - [baseframe1 '.m']); - - sysf_file_present = exist(sysf_filepath, 'file'); % Test for this file being present - - %%% - % Fourth, if there is a sysf_ file of the appropriate name, see - % if there is an associated file with the results of the system - % calculations - if sysf_file_present - - - % Build a string to the filename where calculations from - % this system are stored - calcfilePath = fullfile(inputpath, 'sysplotter_data',... - [baseframe1 '_calc.mat']); - - % Check that the calculated system file exists - if exist(calcfilePath, 'file') - - % Import the datafile of the specified system - load(calcfilePath,'s') - - - %%%% - % Get the Jacobian from the baseframe used by the - % system to the minimum-perturbation baseframe + % Places the reference frame on the rightmost (highest s) point on the + % backbone + case {'head','head-tip'} + + % Identify the leftmost portion of the body + s_zero = 0.5; + + % Extract the transform for this frame + frame_zero = vec_to_mat_SE2(h(s_zero)); + + % Jacobian for tail frame + if calc_J + J_zero = TgLginv(frame_zero)*J(s_zero); + end + + + % Places the reference frame at center of mass and average orientation + % of the backbone, assuming constant mass-per-s density + case 'com-mean' + + % Sample the backbone at a dense set of points + h_points = h(linspace(-.5,.5,100)); + + % Average the x, y, and theta components. The theta value here is + % integrated local rotation, so we don't need to worry about + % unwrapping it + CoM = mean(h_points,2); + + % Place the new frame at this location + frame_zero = vec_to_mat_SE2(CoM); + + %%%%%%%%%%% + % The Jacobian of the weighted average of frames is the + % weighted average of their Jacobian (by the commutativity of + % sumation and derivation operations). + + if calc_J + + % Sample the Jacobian at a dense set of points + J_points = J(linspace(-.5,.5,100)); + + % Average the Jacobians + J_zero = mean(J_points,3); + + % Bring into local coordinates + J_zero = TgLginv(frame_zero)*J_zero; + + end + + %%%%%%%%%%% + % Several possible cases are handled within this "otherwise" case, + % because they need if/else logic + otherwise + + %%%%%%% + % If a number is provided as the input, place the baseframe on that + % link + % Check for numeric baseframe specification + if isnumeric(baseframe) && isscalar(baseframe{idx_baseframe}) + % Make sure that base frame specification is actually in + % the range of valid link numbers + if baseframe >= -0.5 && baseframe <= 0.5 + + % Set the base frame as numerically specified in the input + s_zero = baseframe; + + % Extract the transform for this frame + frame_zero = vec_to_mat_SE2(h(s_zero)); + + % Jacobian for middle frame + if calc_J + J_zero = TgLginv(frame_zero)*J(s_zero); + end + + else + + error (['Baseframe specification ' num2str(baseframe) ' is not in the range of -0.5 to 0.5']) + + end + + %%%%%%%% + % If the provided string is the name of a sysf_ system file, pull + % the transformation to minimum-perturbation coordinates from that file + else + + %%%%% + % Robustly check if the baseframe string is a sysf_ file that + % has had minimum-perturbation coordinates calculated %%% - % Interpolate the specified shape into the grids to - % extract the transform from the system's baseframe to - % its minimum-perturbation baseframe and the Jacobian - % of this transform - - frame_mp = zeros(3,1); % x y theta values of transformation - - if calc_J - J_mp = zeros(3,numel(shapeparams)); % x y theta rows and joint angle columns of Jacobian + % First, strip off sysf_ if it was included by the user + + if strncmp(baseframe{idx_baseframe},'sysf_',5) + baseframe1 = baseframe{idx_baseframe}(6:end); + else + baseframe1 = baseframe{idx_baseframe}; end - - shapeparams_cell = num2cell(shapeparams); % put joint angles into a cell array - - % Iterate over x y theta components - for idx = 1:numel(frame_mp) - - % Interpolate the shape angles into the grid and - % extract the corresponding component of the - % 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 - shapeparams_cell{:}); % Current shape - - % Scale the x and y components of the frame - % location - if any(idx==[1 2]) - frame_mp(idx) = L*frame_mp(idx)/s.geometry.length; - end - if calc_J - % Iterate over shape components for Jacobian - for idx2 = 1:numel(shapeparams) + %%% + % Second, put sysf_ onto the front of the string + + baseframe1 = ['sysf_' baseframe1]; %#ok + + %%% + % Third, check if there is a sysf_ file with this name + + load('sysplotter_config.mat','inputpath') % Get the path to the current UserFiles folder + + sysf_filepath = fullfile(inputpath, 'Systems',... % Build a string to a sysf_ file with the baseframe name + [baseframe1 '.m']); + + sysf_file_present = exist(sysf_filepath, 'file'); % Test for this file being present + + %%% + % Fourth, if there is a sysf_ file of the appropriate name, see + % if there is an associated file with the results of the system + % calculations + if sysf_file_present + + + % Build a string to the filename where calculations from + % this system are stored + calcfilePath = fullfile(inputpath, 'sysplotter_data',... + [baseframe1 '_calc.mat']); + + % Check that the calculated system file exists + if exist(calcfilePath, 'file') + + % Import the datafile of the specified system + load(calcfilePath,'s') + + + %%%% + % Get the Jacobian from the baseframe used by the + % system to the minimum-perturbation baseframe + - % 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 + %%% + % Interpolate the specified shape into the grids to + % extract the transform from the system's baseframe to + % its minimum-perturbation baseframe and the Jacobian + % of this transform + + frame_mp = zeros(3,1); % x y theta values of transformation + + if calc_J + J_mp = zeros(3,numel(shapeparams)); % x y theta rows and joint angle columns of Jacobian + end + + shapeparams_cell = num2cell(shapeparams); % put joint angles into a cell array + + % Iterate over x y theta components + for idx = 1:numel(frame_mp) + + % Interpolate the shape angles into the grid and + % extract the corresponding component of the + % 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 shapeparams_cell{:}); % Current shape % Scale the x and y components of the frame - % Jacobian + % location if any(idx==[1 2]) - J_mp(idx,idx2) = L*J_mp(idx,idx2)/s.geometry.length; + frame_mp(idx) = L*frame_mp(idx)/s.geometry.length; + end + + if calc_J + % Iterate over shape components for Jacobian + 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 + shapeparams_cell{:}); % Current shape + + % Scale the x and y components of the frame + % Jacobian + if any(idx==[1 2]) + J_mp(idx,idx2) = L*J_mp(idx,idx2)/s.geometry.length; + end + + + end end - end - end - end + % Convert the transforms that were just found into an + % SE(2) matrix and a body-velocity Jacobian + frame_mp = vec_to_mat_SE2(frame_mp); - % Convert the transforms that were just found into an - % SE(2) matrix and a body-velocity Jacobian - frame_mp = vec_to_mat_SE2(frame_mp); - - if calc_J - J_mp = TgLginv(frame_mp) * J_mp; - end - - %%%% - % Get the conversion factors from the tail link to the - % baseframe used by the system - - % Get the geometry specification in this file - if isfield(s,'geometry') - if isfield(s.geometry,'baseframe') - baseframe_original = s.geometry.baseframe; - else - error('Baseframe is not specified in system geometry structure') - end - else - error('System does not have a geometry structure') - end + if calc_J + J_mp = TgLginv(frame_mp) * J_mp; + end + + %%%% + % Get the conversion factors from the tail link to the + % baseframe used by the system + + % Get the geometry specification in this file + if isfield(s,'geometry') + if isfield(s.geometry,'baseframe') + baseframe_original = s.geometry.baseframe; + else + error('Baseframe is not specified in system geometry structure') + end + else + error('System does not have a geometry structure') + end - % Get conversion factors for the original baseframe - if calc_J - [frame_original,J_original] =... - backbone_conversion_factors(h,J,shapeparams,baseframe_original,L); + % Get conversion factors for the original baseframe + if calc_J + [frame_original,J_original] =... + backbone_conversion_factors(h,J,shapeparams,baseframe_original,L); + else + frame_original =... + backbone_conversion_factors(h,J,shapeparams,baseframe_original,L); + end + + % Combine original conversion with minimum-perturbation + % conversion + + % Frame construction is straightforward concatenation + % of SE(2) transforms + frame_zero = frame_original * frame_mp; + + % Combination of body-velocity Jacobians is by taking + % the adjoint-inverse of the proximal Jacobian by the + % distal transformation, then adding the distal + % Jacobian + if calc_J + J_zero = (Adjinv(frame_mp) * J_original) + J_mp; + end + + end + else - frame_original =... - backbone_conversion_factors(h,J,shapeparams,baseframe_original,L); - end - - % Combine original conversion with minimum-perturbation - % conversion - - % Frame construction is straightforward concatenation - % of SE(2) transforms - frame_zero = frame_original * frame_mp; - - % Combination of body-velocity Jacobians is by taking - % the adjoint-inverse of the proximal Jacobian by the - % distal transformation, then adding the distal - % Jacobian - if calc_J - J_zero = (Adjinv(frame_mp) * J_original) + J_mp; + error (['Baseframe specification ' baseframe ' is not a valid option, and there is no system file with that name']) end - end - - else - error (['Baseframe specification ' baseframe ' is not a valid option, and there is no system file with that name']) - end - + end + end + elseif isnumeric(baseframe{idx_baseframe}) + + + if ~exist('J_zero','var') + + % Extract the transform from the end to itself + frame_zero = eye(3); + + % Jacobian for base link is zero + J_zero = zeros(size(J_temp{1})); + + end + + % Step by the provided transformation + frame_step = baseframe{idx_baseframe}; + + % Apply the step relatve to the pre-existing frame zero + frame_zero = frame_zero * frame_step; + + % Use the adjoint-inverse of the step to modify the + % Jacobian + J_zero = Adjinv(frame_step) * J_zero; + + + end + +end + end \ No newline at end of file diff --git a/ProgramFiles/Geometry/NLinkChain/N_link_chain.m b/ProgramFiles/Geometry/NLinkChain/N_link_chain.m index 927e0eea..f1128f31 100644 --- a/ProgramFiles/Geometry/NLinkChain/N_link_chain.m +++ b/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,chain_description] = 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. % @@ -17,26 +17,44 @@ % % 'centered' : Default behavior % 'tail' : Lowest-numbered link is the base frame +% 'tail-tip' : Start of lowest-numbered link is the base frame % 'head' : Highest-numbered link is the base frame -% 'head-tip': End of the highest-numbered link is the base frame +% 'head-tip' : End of the highest-numbered link is the base frame % numeric : Specify a link number to use as a base frame % sysf_ Pull minimum-perturbation coordinates from a % sysf_ file. Argument should be the name of % a system in the current UserFiles folder +% 'start' : Modifier on a link specification (e.g., +% {2,'start'} to put the frame at the proximal end of the +% specified link +% 'end' : Modifier on a link specification (e.g., +% {head,'end'} to put the frame at the distal end of the +% specified link +% transform: A 3x3 SE(2) matrix giving the position +% of the base frame. This can be a standalone entry or a +% modifier on any other frame. % -% modes (optional): Option to map input "jointangles" across +% modes (optional): Option to map input "shapeparams" across % multiple links in a chain (which can have more joints than the -% provided number of "jointangles". Should be a matrix in which +% provided number of "shapeparams". 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. +% with a unit value of the corresponding "shapeparams" 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 -% will not be scaled. - -% jointangles: A vector of the angles between the links. Must be one +% 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 +% will not be scaled. +% +% prismatic (optional): Option to make some shapeparams prismatic. If +% specified, it should be of the same size as the shapeparams +% input, and composed of zeros and ones. All shape parameters +% marked with a one will be treated as prismatic joints parallel +% to the links before/after the joint. For systems with links marked +% prismatic, any calculations involving total system length will +% be with the prismatic joints at zero extension. +% +% shapeparams: A vector of the angles between the links. Must be one % element shorter than the linklengths vector % % @@ -71,11 +89,17 @@ % % J_zero : The Jacobian from shape velocity to body velocity of the % selected baseframe, with the first link held fixed. +% +% chain_description : The full set of chain features passed to +% N_link_conversion_factors (but modified so that they are in the +% specified baseframe, not the first-link frame. % %%%%%%%%%%%% % Input parsing + + % If no length is specified, do not scale the links if ~isfield(geometry,'length') || isempty(geometry.length) L = sum(geometry.linklengths); @@ -83,6 +107,12 @@ L = geometry.length; 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; +shapeparams = shapeparams(:); + + % If no baseframe is specified, use a centered chain if ~isfield(geometry,'baseframe') || isempty(geometry.baseframe) baseframe = 'centered'; @@ -92,28 +122,31 @@ % If no modes are specified, use an identity mapping for the modes if ~isfield(geometry,'modes') || isempty(geometry.modes) - modes = eye(numel(jointangles)); + modes = eye(numel(shapeparams)); else modes = geometry.modes; end -% Force linklength and jointangle vectors to be columns, and normalize link -% lengths for a total length of 1. -linklengths = geometry.linklengths(:)/sum(geometry.linklengths)*L; -jointangles = jointangles(:); +% If no prismatic joints are specified, all joints are rotational +if ~isfield(geometry,'prismatic') || isempty(geometry.prismatic) + prismatic = zeros(size(shapeparams)); +else + prismatic = geometry.prismatic; +end -% Expand jointangles from specified shape variables to actual joint angles + +% Expand jointvalues from specified shape variables to actual joint angles % by multiplying in the modal function -jointangles = modes*jointangles(:); +jointvalues = modes*shapeparams; %%%%%%%%%%%% % Prevent Matlab from playing tricks with imaginary numbers on symbolic % inputs and from complaining about assumptions on constants -if isa(jointangles,'sym') +if isa(jointvalues,'sym') - assume(symvar(jointangles),'real'); + assume(symvar(jointvalues),'real'); warning('off','symbolic:sym:sym:AssumptionsOnConstantsIgnored') end @@ -124,9 +157,9 @@ % Number of links and joints N_links = numel(linklengths); -M_joints = numel(jointangles); +M_joints = numel(jointvalues); if M_joints ~= N_links-1 - error(['There should be one more link than joint. There are ' num2str(N_links) ' links and ' num2str(M_joints) ' joint angles specified']); + error(['There should be one more link than joint. There are ' num2str(N_links) ' links and ' num2str(M_joints) ' joint values specified']); end % Decide if there is an even or odd number of joints N_odd = mod(N_links,2); @@ -148,9 +181,29 @@ % elements displaced along the x direction from the identity. links_v = [halflengths, zeros(N_links,2)]; -% Convert the joint angle values to group elements displaced along the -% theta direction from the identity -joints_v = [zeros(M_joints,2), jointangles]; +% Convert the joint values to group elements displaced along the +% theta direction from the identity (for rotational joints) or the x axis +% (for prismatic joints) + +% Create a placeholder set of joint displacement vectors +joints_v = zeros(numel(jointvalues),3); + +% Convert the set of joint displacement vectors to a symbolic array if any +% of the joint values are symbolic +if or( isa(jointvalues,'sym'), isa(linklengths,'sym') ) + joints_v = sym(joints_v); +end + +for idx = 1:numel(jointvalues) + switch prismatic(idx) + case 0 + joints_v(idx,3) = jointvalues(idx); + case 1 + joints_v(idx,1) = jointvalues(idx); + otherwise + error('Unrecognized marker for joint type') + end +end % Find the matrix representations of the link transformations links_m = vec_to_mat_SE2(links_v); @@ -166,11 +219,16 @@ % representing the corresponding link's configuration chain_m = repmat(eye(3),1,1,N_links); +% Also take a cumulative sum of the joint angles -- this is useful if we +% want to take a mean orientation of the link orientations while making a +% distinction between orientations separated by 2pi radians +jointvalues_c = [0; cumsum(jointvalues.*~prismatic)]; + % If we're working with symbolic variables, then we need to explicitly make % the array symbolic, because matlab tries to cast items being inserted % into an array into the array class, rather than converting the array to % accomodate the class of the items being inserted -if or( isa(jointangles,'sym'), isa(linklengths,'sym') ) +if or( isa(jointvalues,'sym'), isa(linklengths,'sym') ) chain_m = sym(chain_m); end @@ -204,13 +262,13 @@ % Build a 3-d array to hold the SE(2) matrices for each joint % (specifically for the frame at the end of the link proximal to the % joint, treating that as the stator for the joint). -jointchain_m = repmat(eye(3),1,1,numel(jointangles)); +jointchain_m = repmat(eye(3),1,1,numel(jointvalues)); % If we're working with symbolic variables, then we need to explicitly make % the array symbolic, because matlab tries to cast items being inserted % into an array into the array class, rather than converting the array to % accomodate the class of the items being inserted -if or( isa(jointangles,'sym'), isa(linklengths,'sym') ) +if or( isa(jointvalues,'sym'), isa(linklengths,'sym') ) jointchain_m = sym(jointchain_m); end @@ -218,7 +276,7 @@ jointchain_m(:,:,1) = links_m(:,:,1); % Iterate along the chain to find the configuration of the later joints -for idx = 2:numel(jointangles) +for idx = 2:numel(jointvalues) % The transformation from one joint to the next is the product % of the proximal joint's transformation with the link between the two joints @@ -256,7 +314,7 @@ % the array symbolic, because matlab tries to cast items being inserted % into an array into the array class, rather than converting the array to % accomodate the class of the items being inserted -if or( isa(jointangles,'sym'),isa(linklengths,'sym') ) +if or( isa(jointvalues,'sym'),isa(linklengths,'sym') ) J_pattern = sym(J_pattern); end @@ -291,7 +349,16 @@ % Multiply these the Adjoint-inverse transformation by the % joint axis to get the Jacobian from the current joint to % the current link - J_temp{idx}(:,idx2) = Adjointinverse_transform * [0;0;1]; + switch prismatic(idx2) + case 0 + joint_axis = [0;0;1]; + case 1 + joint_axis = [1;0;0]; + otherwise + error('Unrecognized marker for joint type') + end + + J_temp{idx}(:,idx2) = Adjointinverse_transform * joint_axis; else @@ -302,6 +369,9 @@ end + % Convert joint-angle Jacobian into shape-mode coordinates + J_temp{idx} = J_temp{idx} * modes; + end @@ -321,28 +391,40 @@ %%%%%% % Calculate the transformation from the original base frame to the new base % frame -[frame_zero,J_zero] = N_link_conversion_factors(chain_m,... - jointchain_m,... - links_m,... - joints_m,... - links_v,... - joints_v,... - jointangles,... - linklengths,... - J_temp,... - baseframe); + +chain_description = struct( ... +'chain_m',{chain_m},... +'jointchain_m',{jointchain_m},... +'links_m',{links_m},... +'joints_m',{joints_m},... +'links_v',{links_v},... % Future refactoring could dispense with passing both links_v and links_m and joints_v and joints_m +'joints_v',{joints_v},... +'jointvalues',{jointvalues},... +'jointvalues_c',{jointvalues_c}, ... +'linklengths',{linklengths},... +'shapeparams',{shapeparams},... +'modes',{modes},... +'J_temp',{J_temp},... +'baseframe',{baseframe} ... +); + +[frame_zero,J_zero] = N_link_conversion_factors(chain_description); %%%%%% % Use frame_zero and J_zero to convert the link transformations and % 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); +[h_m,J,J_full,chain_description] = N_link_conversion(chain_description,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,2),size(J{1},1)),modes]; -J_full = cellfun(@(j) j*full_mode_conversion,J_full,'UniformOutput',false); + +% %%%%%%% +% % 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 @@ -350,7 +432,10 @@ h.lengths = linklengths; - +if nargout > 5 + % Get the partial derivative of the Jacobian + dJdq = mobile_jacobian_derivative(J_full); +end end \ No newline at end of file diff --git a/ProgramFiles/Geometry/NLinkChain/N_link_conversion.m b/ProgramFiles/Geometry/NLinkChain/N_link_conversion.m index 65beb616..3cde4c25 100644 --- a/ProgramFiles/Geometry/NLinkChain/N_link_conversion.m +++ b/ProgramFiles/Geometry/NLinkChain/N_link_conversion.m @@ -1,4 +1,4 @@ -function [h_m,J,J_full] = N_link_conversion(chain_m,J_temp,frame_zero,J_zero) +function [h_m,J,J_full,C] = N_link_conversion(C,frame_zero,J_zero) %%%%%%% % This is a helper-function for N_link_chain. % @@ -21,8 +21,16 @@ % The code here uses the conversion transformation and Jacobian calculated % in N_link_conversion_factors. - % Preallocate a matrix for holding the transformed link matrices + + % Extract elements from structure array (some functions like size do not + % work properly if applied to struct contents without a breakout) + chain_m = C.chain_m; + jointchain_m = C.jointchain_m; + J_temp = C.J_temp; + + % Preallocate a matrix for holding the transformed link and joint location matrices h_m = zeros(size(chain_m)); + hj_m = zeros(size(jointchain_m)); % If we're working with symbolic variables, then we need to explicitly make % the array symbolic, because matlab tries to cast items being inserted @@ -32,7 +40,12 @@ h_m = sym(h_m); end - % For each link in the chain, transform its matrix by the inverse of the + if isa(jointchain_m,'sym') + hj_m = sym(hj_m); + end + + + % For each link and joint in the chain, transform its matrix by the inverse of the % central transformation for idx = 1:size(h_m,3) h_m(:,:,idx) = frame_zero \ chain_m(:,:,idx); @@ -42,6 +55,20 @@ end end + + for idx = 1:size(hj_m,3) + hj_m(:,:,idx) = frame_zero \ jointchain_m(:,:,idx); + + if isa(chain_m,'sym') + hj_m(:,:,idx) = simplify(hj_m(:,:,idx),'steps',10); + end + + end + + + % Save h_m to the chain description structure as the new chain_m + C.chain_m = h_m; + C.jointchain_m = hj_m; %%%%%%%% @@ -59,10 +86,13 @@ J_new = J_temp; for idx = 1:numel(J_new) J_new{idx} = ... - (J_new{idx} - ... % Jacobian from joint velocities to body velocity of link, with first link fixed + (J_temp{idx} - ... % Jacobian from joint velocities to body velocity of link, with first link fixed Adjinv(h_m(:,:,idx)) * ... % Adjoint-inverse transformation by position of this link in new frame J_zero); % Jacobian from joint velocities to body velocity of new frame, with first link fixed end + + % Save this Jacobian out to the chain description structure + C.J_temp = J_new; % Second operation is to transform the link Jacobians such that they @@ -74,6 +104,12 @@ J = J_new; for idx = 1:numel(J_new) J{idx} = TeLg(h_m(:,:,idx)) * J_new{idx}; % Left lifted action rotates into new base frame coordinates + + %Simplify J expression if necessary + if isa(J{idx},'sym') + J{idx} = simplify(J{idx},100); + end + end diff --git a/ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m b/ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m index e5817b4f..0406b646 100644 --- a/ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m +++ b/ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m @@ -1,13 +1,4 @@ -function [frame_zero,J_zero] = N_link_conversion_factors(chain_m,... - jointchain_m,... - links_m,... - joints_m,... - links_v,... - joints_v,... - jointangles,... - linklengths,... - J_temp,... - baseframe) +function [frame_zero,J_zero,link_zero] = N_link_conversion_factors(C) %%%%%%% % This is a helper-function for N_link_chain. % @@ -38,6 +29,23 @@ % Get some basic information about the chain whose kinematics we're % calculating +% Extract elements from structure array (some functions like size do not +% work properly if applied to struct contents without a breakout) +chain_m = C.chain_m; +jointchain_m = C.jointchain_m; +links_m = C.links_m; +%joints_m = C.joints_m; +links_v = C.links_v; +joints_v = C.joints_v; +%jointvalues = C.jointvalues; +jointvalues_c = C.jointvalues_c; +linklengths = C.linklengths; +shapeparams = C.shapeparams; +modes = C.modes; +J_temp = C.J_temp; +baseframe = C.baseframe; + + % Number of links and joints N_links = size(chain_m,3); M_joints = N_links-1; @@ -51,369 +59,494 @@ %%%%%%%%%%%%%%%%%%% - +%%%%%%%%%%%%%%%%%%% +% If baseframe is specified as a non-cell object, wrap it into a cell array +if ~iscell(baseframe) + baseframe = {baseframe}; +end + + % Depending on which baseframe option is specified, use different methods % to calculate transform and Jacobian to use in the conversion -switch baseframe +for idx_baseframe = 1:numel(baseframe) - % Places the reference frame at the midpoint of the lowest-index link on the chain - case 'tail' - - % Identify the first link - link_zero = 1; - - % Extract the transform from the end link to itself - frame_zero = eye(3); - - % Jacobian for base link is zero - J_zero = zeros(size(J_temp{1})); - - % Places the reference frame at the *end* of the lowest-index link on the chain - case 'tail-tip' - - % Identify the end link - link_zero = 1; - - % The transform from the midpoint of the first link to its end is - % the inverse of the half-link transform - frame_zero = inv(links_m(:,:,link_zero)); - - %%%%%%%% - % Jacobian to new frame is Jacobian of first link, but with an - % adjoint-inverse transform by the half-link to get to the end - - halfstep = Adjinv(frame_zero); - J_zero = halfstep * J_temp{1}; - - - % Places the reference frame at the middle of the chain, splitting the - % difference between the two middle links if there is an even number of - % links - case {'centered','center','midpoint-tangent'} - - - % If there is an odd number of links, the middle link is the center of the - % chain - if N_odd - - % Identify the middle link - link_zero = ceil(N_links/2); - - % Extract the transform from the end link to the middle link - frame_zero = chain_m(:,:,link_zero); - - %%%%%% - % Jacobian for conversion is Jacobian of link zero - J_zero = J_temp{link_zero}; - - % If there is an even number of links, the midpoint is at the middle joint, - % rotated by half of that joint's angle - else - - % Identify the middle joint - joint_zero = ceil(M_joints/2); - - % Extract the position of the middle joint, and multiply it by half of - % the transform associated with its joint angle - frame_zero = jointchain_m(:,:,joint_zero) * ... - vec_to_mat_SE2(joints_v(joint_zero,:)/2); - - %%%%%%%% - % Jacobian for conversion is Jacobian of link before it, with a - % half-step to get to the end of the link, and a half rotation - % to average orientation between the two links - halfstep = Adjinv(links_v(joint_zero,:)); - 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 - + if ischar(baseframe{idx_baseframe}) || isscalar(baseframe{idx_baseframe}) - end - - - - - - % Places the reference frame at the midpoint of the highest-numbered link in the chain - case 'head' - - % Identify the end link - link_zero = N_links; - - % Extract the transform from the end link to the end link - frame_zero = chain_m(:,:,link_zero); - - %%%%%%%% - % Jacobian to new frame is Jacobian of last link - J_zero = J_temp{end}; - - % Places the reference frame at the *end of* the highest-numbered link - % in the chain - case 'head-tip' - - % Identify the end link - link_zero = N_links; - - % Extract the transform from the end link to the end link, and - % multiply it by the half-link transformation on that link - frame_zero = chain_m(:,:,link_zero)*links_m(:,:,link_zero); - - %%%%%%%% - % Jacobian to new frame is Jacobian of last link, but with an - % adjoint-inverse transform by the half-link to get to the end - halfstep = Adjinv(links_v(end,:)); - J_zero = halfstep * J_temp{end}; - - % Places the reference frame at center of mass and average orientation - % of the links, using link-lengths as weighting terms - case 'com-mean' - - % Convert link positions to row form - chain = mat_to_vec_SE2(chain_m); - - % Substitute a cumulative sum of the joint angles for the - % orientations of the links: We're not looking for the mean of the - % SO(2)-modulated orientations, we're looking for the mean angular - % displacement from the base link. - chain(:,3) = [0;cumsum(jointangles)]; - - % Take a weighted average of the link positions - CoM = sum(diag(linklengths)*chain)/L; - - - % Place the new frame at this location - frame_zero = vec_to_mat_SE2(CoM); + switch baseframe{idx_baseframe} - %%%%%%%%%%% - % The Jacobian of the weighted average of frames is the - % weighted average of their Jacobian (by the commutativity of - % sumation and derivation operations). - - % Multiply each link's Jacobian by its link length - J_weighted = J_temp; - for idx = 1:numel(J_weighted) - J_weighted{idx} = TeLg(chain(idx,:)) * J_temp{idx} * linklengths(idx); - end + % Places the reference frame at the midpoint of the lowest-index link on the chain + case 'tail' - % Sum the weighted Jacobians - J_zero = J_weighted{1}; - for idx = 2:numel(J_weighted) - J_zero = J_zero + J_weighted{idx}; - end -% J_zero = sum(cat(3,J_weighted{:}),3); - - % Divide by the total length to g - J_zero = J_zero/L; - - % Bring into local coordinates - J_zero = TgLginv(frame_zero)*J_zero; - - - %%%%%%%%%%% - % Several possible cases are handled within this "otherwise" case, - % because they need if/else logic - otherwise - - %%%%%%% - % If a number is provided as the input, place the baseframe on that - % link - % Check for numeric baseframe specification - if isnumeric(baseframe) - - % Make sure that base frame specification is actually in - % the range of valid link numbers - if baseframe <= N_links && baseframe > 0 + % Identify the first link + link_zero = 1; - % Set the base frame as numerically specified in the input - link_zero = baseframe; + % Extract the transform from the end link to itself + frame_zero = eye(3); - % Extract the transform from the end link to the end link, and - % multiply it by the half-link transformation on that link + % Jacobian for base link is zero + J_zero = zeros(size(J_temp{1})); + + % Places the reference frame at the *end* of the lowest-index link on the chain + case {'tail-tip','tail-end'} + + % Identify the end link + link_zero = 1; + + % The transform from the midpoint of the first link to its end is + % the inverse of the half-link transform + frame_zero = inv(links_m(:,:,link_zero)); + + %%%%%%%% + % Jacobian to new frame is Jacobian of first link, but with an + % adjoint-inverse transform by the half-link to get to the end + + halfstep = Adjinv(frame_zero); + J_zero = halfstep * J_temp{1}; + + + % Places the reference frame at the middle of the chain, splitting the + % difference between the two middle links if there is an even number of + % links + case {'centered','center','midpoint-tangent'} + + + % If there is an odd number of links, the middle link is the center of the + % chain + if N_odd + + % Identify the middle link + link_zero = ceil(N_links/2); + + % Extract the transform from the end link to the middle link + frame_zero = chain_m(:,:,link_zero); + + %%%%%% + % Jacobian for conversion is Jacobian of link zero + J_zero = J_temp{link_zero}; + + % If there is an even number of links, the midpoint is at the middle joint, + % rotated by half of that joint's angle + else + + % Identify the middle joint + joint_zero = ceil(M_joints/2); + + % Extract the position of the middle joint, and multiply it by half of + % the transform associated with its joint angle + frame_zero = jointchain_m(:,:,joint_zero) * ... + vec_to_mat_SE2(joints_v(joint_zero,:)/2); + + %%%%%%%% + % Jacobian for conversion is Jacobian of link before it, with a + % half-step to get to the end of the link, and a half rotation + % to average orientation between the two links + halfstep = Adjinv(links_v(joint_zero,:)); + halfrotation = Adjinv(joints_v(joint_zero,:)/2); + J_zero = halfrotation * halfstep * J_temp{joint_zero}; + + % 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 + + + + + + % Places the reference frame at the midpoint of the highest-numbered link in the chain + case 'head' + + % Identify the end link + link_zero = N_links; + + % Extract the transform from the end link to the end link frame_zero = chain_m(:,:,link_zero); %%%%%%%% - % Jacobian to new frame is Jacobian of nth link - J_zero = J_temp{baseframe}; + % Jacobian to new frame is Jacobian of last link + J_zero = J_temp{end}; - else + % Places the reference frame at the *end of* the highest-numbered link + % in the chain + case 'head-tip' - error (['Baseframe specification ' num2str(baseframe) ' is not in the range of link numbers']) + % Identify the end link + link_zero = N_links; - end + % Extract the transform from the first link to the end link, and + % multiply it by the half-link transformation on that link + frame_zero = chain_m(:,:,link_zero)*links_m(:,:,link_zero); + + %%%%%%%% + % Jacobian to new frame is Jacobian of last link, but with an + % adjoint-inverse transform by the half-link to get to the end + halfstep = Adjinv(links_v(end,:)); + J_zero = halfstep * J_temp{end}; + + % Places the reference frame at center of mass and average orientation + % of the links, using link-lengths as weighting terms + case 'com-mean' + + % Convert link positions to row form + chain = mat_to_vec_SE2(chain_m); + + % Substitute a cumulative sum of the joint angles for the + % orientations of the links: We're not looking for the mean of the + % SO(2)-modulated orientations, we're looking for the mean angular + % displacement from the base link. + chain(:,3) = jointvalues_c; + + % Take a weighted average of the link positions + CoM = sum(diag(linklengths)*chain)/L; + + + % Place the new frame at this location + frame_zero = vec_to_mat_SE2(CoM); - %%%%%%%% - % If the provided string is the name of a sysf_ system file, pull - % the transformation to minimum-perturbation coordinates from that file - else + %%%%%%%%%%% + % The Jacobian of the weighted average of frames is the + % weighted average of their Jacobian (by the commutativity of + % sumation and derivation operations). + + % Multiply each link's Jacobian by its link length + J_weighted = J_temp; + for idx = 1:numel(J_weighted) + J_weighted{idx} = TeLg(chain(idx,:)) * J_temp{idx} * linklengths(idx); + end + + % Sum the weighted Jacobians + J_zero = J_weighted{1}; + for idx = 2:numel(J_weighted) + J_zero = J_zero + J_weighted{idx}; + end + % J_zero = sum(cat(3,J_weighted{:}),3); + + % Divide by the total length to g + J_zero = J_zero/L; + + % Bring into local coordinates + J_zero = TgLginv(frame_zero)*J_zero; + + + %%%%%%%%%%% + % These are cases that can be used as modifiers on other baseframes + + % Places the reference frame at the proximal end of the link to + % which it is attached + case 'start' + + % Make sure that a link was previously specified + if ~exist('link_zero','var') + error('Baseframe string ''start'' may only be used as a secondary baseframe specification, where the primary specification attaches the baseframe to a link.'); + end + + % The transform from the midpoint of the first link to its proximal end is + % the inverse of the half-link transform + frame_step = inv(links_m(:,:,link_zero)); + + % compose the frame step with the previously-computed + % frame_zero + frame_zero = frame_zero*frame_step; %#ok + + %%%%%%%% + % Jacobian to new frame is previously-calculated Jacobian, but with an + % adjoint-inverse transform by the half-link to get to the end + + halfstep = Adjinv(frame_step); + J_zero = halfstep * J_zero; + + % Places the reference frame at the distal end of the link to which + % it is attached + case 'end' + + % Make sure that a link was previously specified + if ~exist('link_zero','var') + error('Baseframe string ''end'' may only be used as a secondary baseframe specification, where the primary specification attaches the baseframe to a link.'); + end + + % Transform from the midpoint to the distal end of the link + frame_step = links_m(:,:,link_zero); + + % compose the frame step with the previously-computed + % frame_zero + frame_zero = frame_zero*frame_step; + + + %%%%%%%% + % Jacobian to new frame is previously-calculated Jacobian, but with an + % adjoint-inverse transform by the half-link to get to the end + + halfstep = Adjinv(frame_step); + J_zero = halfstep * J_zero; + - %%%%% - % Robustly check if the baseframe string is a sysf_ file that - % has had minimum-perturbation coordinates calculated - %%% - % First, strip off sysf_ if it was included by the user + %%%%%%%%%%% + % Several possible cases are handled within this "otherwise" case, + % because they need if/else logic + otherwise + + %%%%%%% + % If a number is provided as the input, place the baseframe on that + % link + % Check for numeric baseframe specification + if isnumeric(baseframe{idx_baseframe}) && isscalar(baseframe{idx_baseframe}) + + % Make sure that base frame specification is actually in + % the range of valid link numbers + if baseframe{idx_baseframe} <= N_links && baseframe{idx_baseframe} > 0 + + % Set the base frame as numerically specified in the input + link_zero = baseframe{idx_baseframe}; + + % Extract the transform from the end link to the end link, and + % multiply it by the half-link transformation on that link + frame_zero = chain_m(:,:,link_zero); + + %%%%%%%% + % Jacobian to new frame is Jacobian of nth link + J_zero = J_temp{baseframe{idx_baseframe}}; + + else + + error (['Baseframe specification ' num2str(baseframe{idx_baseframe}) ' is not in the range of link numbers']) + + end + - if strncmp(baseframe,'sysf_',5) - baseframe1 = baseframe(6:end); - else - baseframe1 = baseframe; - end - %%% - % Second, put sysf_ onto the front of the string - - baseframe1 = ['sysf_' baseframe1]; - %%% - % Third, check if there is a sysf_ file with this name + %%%%%%%% + % If the provided string is the name of a sysf_ system file, pull + % the transformation to minimum-perturbation coordinates from that file + else - load('sysplotter_config.mat','inputpath') % Get the path to the current UserFiles folder + %%%%% + % Robustly check if the baseframe string is a sysf_ file that + % has had minimum-perturbation coordinates calculated - sysf_filepath = fullfile(inputpath, 'Systems',... % Build a string to a sysf_ file with the baseframe name - [baseframe1 '.m']); - - sysf_file_present = exist(sysf_filepath, 'file'); % Test for this file being present - - %%% - % Fourth, if there is a sysf_ file of the appropriate name, see - % if there is an associated file with the results of the system - % calculations - if sysf_file_present - - % Pulling the minimum-perturbation coordinates does not - % work with symbolic input, because it requires an - % interpolation of an array at the input points. (Given - % that minimum-perturbation coordinates are solved for - % numerically, not much is lost by not being able to - % symbolically rebase to that frame - if isa(chain_m,'sym') - error('Rebasing the chain to minimum-perturbation coordinates is not supported for symbolic input'); + %%% + % First, strip off sysf_ if it was included by the user + + if strncmp(baseframe{idx_baseframe},'sysf_',5) + baseframe1 = baseframe{idx_baseframe}(6:end); + else + baseframe1 = baseframe{idx_baseframe}; end - - % Build a string to the filename where calculations from - % this system are stored - calcfilePath = fullfile(inputpath, 'sysplotter_data',... - [baseframe1 '_calc.mat']); - - % Check that the calculated system file exists - if exist(calcfilePath, 'file') - % Import the datafile of the specified system - load(calcfilePath,'s') + %%% + % Second, put sysf_ onto the front of the string + baseframe1 = ['sysf_' baseframe1]; %#ok - %%%% - % Get the Jacobian from the baseframe used by the - % system to the minimum-perturbation baseframe + %%% + % Third, check if there is a sysf_ file with this name + load('sysplotter_config.mat','inputpath') % Get the path to the current UserFiles folder + sysf_filepath = fullfile(inputpath, 'Systems',... % Build a string to a sysf_ file with the baseframe name + [baseframe1 '.m']); - %%% - % Interpolate the specified shape into the grids to - % extract the transform from the system's baseframe to - % its minimum-perturbation baseframe and the Jacobian - % of this transform + sysf_file_present = exist(sysf_filepath, 'file'); % Test for this file being present - 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 + %%% + % Fourth, if there is a sysf_ file of the appropriate name, see + % if there is an associated file with the results of the system + % calculations + if sysf_file_present - jointangles_cell = num2cell(jointangles); % put joint angles into a cell array + % Pulling the minimum-perturbation coordinates does not + % work with symbolic input, because it requires an + % interpolation of an array at the input points. (Given + % that minimum-perturbation coordinates are solved for + % numerically, not much is lost by not being able to + % symbolically rebase to that frame + if isa(chain_m,'sym') + error('Rebasing the chain to minimum-perturbation coordinates is not supported for symbolic input'); + end - % Iterate over x y theta components - for idx = 1:numel(frame_mp) + % Build a string to the filename where calculations from + % this system are stored + calcfilePath = fullfile(inputpath, 'sysplotter_data',... + [baseframe1 '_calc.mat']); - % Interpolate the shape angles into the grid and - % extract the corresponding component of the - % 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 - - % Scale the x and y components of the frame - % location - if any(idx==[1 2]) - frame_mp(idx) = L*frame_mp(idx)/s.geometry.length; - end - - % Iterate over shape components for Jacobian - for idx2 = 1:numel(jointangles) + % Check that the calculated system file exists + if exist(calcfilePath, 'file') + + % Import the datafile of the specified system + load(calcfilePath,'s') + + + %%%% + % Get the Jacobian from the baseframe used by the + % system to the minimum-perturbation baseframe + + + + %%% + % Interpolate the specified shape into the grids to + % extract the transform from the system's baseframe to + % its minimum-perturbation baseframe and the Jacobian + % of this transform + + frame_mp = zeros(3,1); % x y theta values of transformation + J_mp = zeros(3,numel(shapeparams)); % x y theta rows and joint angle columns of Jacobian + shapeparams_cell = num2cell(shapeparams); % put joint angles into a cell array - % 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 + % Iterate over x y theta components + for idx = 1:numel(frame_mp) + + % Interpolate the shape angles into the grid and + % extract the corresponding component of the + % 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 + shapeparams_cell{:}); % Current shape % Scale the x and y components of the frame - % Jacobian + % location if any(idx==[1 2]) - J_mp(idx,idx2) = L*J_mp(idx,idx2)/s.geometry.length; + frame_mp(idx) = L*frame_mp(idx)/s.geometry.length; end - end - + + % Iterate over shape components for Jacobian + 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 + shapeparams_cell{:}); % Current shape + + % Scale the x and y components of the frame + % Jacobian + if any(idx==[1 2]) + J_mp(idx,idx2) = L*J_mp(idx,idx2)/s.geometry.length; + end + end + - end - - - - % Convert the transforms that were just found into an - % SE(2) matrix and a body-velocity Jacobian - frame_mp = vec_to_mat_SE2(frame_mp); - J_mp = TgLginv(frame_mp) * J_mp; - - %%%% - % Get the conversion factors from the tail link to the - % baseframe used by the system - - % Get the geometry specification in this file - if isfield(s,'geometry') - if isfield(s.geometry,'baseframe') - baseframe_original = s.geometry.baseframe; - else - error('Baseframe is not specified in system geometry structure') end - else - error('System does not have a geometry structure') - end - % Get conversion factors for the original baseframe - [frame_original,J_original] =... - N_link_conversion_factors(chain_m,... - jointchain_m,... - links_m,... - joints_m,... - links_v,... - joints_v,... - jointangles,... - linklengths,... - J_temp,... - baseframe_original); - - % Combine original conversion with minimum-perturbation - % conversion - - % Frame construction is straightforward concatenation - % of SE(2) transforms - frame_zero = frame_original * frame_mp; - - % Combination of body-velocity Jacobians is by taking - % the adjoint-inverse of the proximal Jacobian by the - % distal transformation, then adding the distal - % Jacobian - J_zero = (Adjinv(frame_mp) * J_original) + J_mp; - - end - - else - error (['Baseframe specification ' baseframe ' is not a valid option, and there is no system file with that name']) + + % Convert the transforms that were just found into an + % SE(2) matrix and a Jacobian for the body velocity + % of the new frame relative to the frame in which it + % is defined + frame_mp = vec_to_mat_SE2(frame_mp); + J_mp = TgLginv(frame_mp) * J_mp; + + + % Concatenate this transform and Jacobian with any + % previously-calculated values + frame_zero = frame_zero * frame_mp; + J_zero = (Adjinv(frame_mp) * J_zero) + J_mp; + +% %%%% +% % Get the conversion factors from the tail link to the +% % baseframe used by the system +% +% % Get the geometry specification in this file +% if isfield(s,'geometry') +% if isfield(s.geometry,'baseframe') +% baseframe_original = s.geometry.baseframe; +% else +% error('Baseframe is not specified in system geometry structure') +% end +% else +% error('System does not have a geometry structure') +% end +% + +% % Get conversion factors for the original baseframe +% [frame_original,J_original] =... +% N_link_conversion_factors(C); +% % chain_m,... +% % jointchain_m,... +% % links_m,... +% % joints_m,... +% % links_v,... +% % joints_v,... +% % jointvalues,... +% % linklengths,... +% % shapeparams,... +% % modes,... +% % J_temp,... +% % baseframe_original); +% +% % Combine original conversion with minimum-perturbation +% % conversion +% +% % Frame construction is straightforward concatenation +% % of SE(2) transforms +% frame_zero = frame_original * frame_mp; +% +% % Combination of body-velocity Jacobians is by taking +% % the adjoint-inverse of the proximal Jacobian by the +% % distal transformation, then adding the distal +% % Jacobian +% J_zero = (Adjinv(frame_mp) * J_original) + J_mp; + + end + + else + error (['Baseframe specification ' baseframe{idx_baseframe} ' is not a valid option, and there is no system file with that name']) + end + end + end + %%%%%%% + % If a transformation matrix is provided as the input, apply it + % to the base frame (stacked onto any previous base frame) + + elseif isnumeric(baseframe{idx_baseframe}) + + + if ~exist('J_zero','var') + + % Identify the first link + link_zero = 1; + + % Extract the transform from the end link to itself + frame_zero = eye(3); + + % Jacobian for base link is zero + J_zero = zeros(size(J_temp{1})); + end - + + % Step by the provided transformation + frame_step = baseframe{idx_baseframe}; + + % Apply the step relatve to the pre-existing frame zero + frame_zero = frame_zero * frame_step; + + % Use the adjoint-inverse of the step to modify the + % Jacobian + J_zero = Adjinv(frame_step) * J_zero; + + end + + +end + + % Update J_temp to be the J_zero computed at this stage of the loop +% J_temp = J_zero; + +if ~exist('link_zero','var') + link_zero = 0; end + +end + + \ No newline at end of file diff --git a/ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m.orig b/ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m.orig new file mode 100644 index 00000000..dcc62c1f --- /dev/null +++ b/ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m.orig @@ -0,0 +1,645 @@ +function [frame_zero,J_zero,link_zero] = N_link_conversion_factors(C) +%%%%%%% +% This is a helper-function for N_link_chain. +% +% N_link_chain takes in an argument that specifies which link on the chain +% (or another frame, such as a center-of-mass frame or one loaded from a +% system file) should be used as the base frame. +% +% All of the N_link_chain calculations are made with the lowest-numbered +% (leftmost, or 'tail') link as the base frame. +% +% Transforming the positions of the links relative to the base frame is +% straightforward: We simply multiply each transformation by the inverse of +% the transformation from the old base frame to the new base frame +% +% To transform the link Jacobians into the new coordinates, we need the +% Jacobian from joint velocities to body velocity of new frame, with +% first link fixed. This calculation depends on how the kinematic map +% from the original base frame to the new base frame is calculated. +% +% The code in this file handles the special cases required for finding the +% transformation and Jacobian from the old base frame to the new base frame +% for different specifications of the new base frame. +% +% The actual transformation of the link matrices and Jacobians is handled +% by N_link_conversion. + +%%%%%%%%%%%%%% +% Get some basic information about the chain whose kinematics we're +% calculating + +% Extract elements from structure array (some functions like size do not +% work properly if applied to struct contents without a breakout) +chain_m = C.chain_m; +jointchain_m = C.jointchain_m; +links_m = C.links_m; +joints_m = C.joints_m; +links_v = C.links_v; +joints_v = C.joints_v; +jointangles = C.jointangles; +jointangles_c = C.jointangles_c; +linklengths = C.linklengths; +shapeparams = C.shapeparams; +modes = C.modes; +J_temp = C.J_temp; +baseframe = C.baseframe; + + +% Number of links and joints +N_links = size(chain_m,3); +M_joints = N_links-1; + +% Decide if there is an even or odd number of joints +N_odd = mod(N_links,2); + +% Total length for use in taking weighted averages +L = sum(linklengths); + +%%%%%%%%%%%%%%%%%%% + + +%%%%%%%%%%%%%%%%%%% +% If baseframe is specified as a non-cell object, wrap it into a cell array +if ~iscell(baseframe) + baseframe = {baseframe}; +end + + +% Depending on which baseframe option is specified, use different methods +% to calculate transform and Jacobian to use in the conversion +for idx_baseframe = 1:numel(baseframe) + +<<<<<<< HEAD + % Places the reference frame at the midpoint of the lowest-index link on the chain + case 'tail' + + % Identify the first link + link_zero = 1; + + % Extract the transform from the end link to itself + frame_zero = eye(3); + + % Jacobian for base link is zero + J_zero = zeros(size(J_temp{1})); + + % Places the reference frame at the *end* of the lowest-index link on the chain + case 'tail-tip' + + % Identify the end link + link_zero = 1; + + % The transform from the midpoint of the first link to its end is + % the inverse of the half-link transform + frame_zero = inv(links_m(:,:,link_zero)); + + %%%%%%%% + % Jacobian to new frame is Jacobian of first link, but with an + % adjoint-inverse transform by the half-link to get to the end + + halfstep = Adjinv(frame_zero); + J_zero = halfstep * J_temp{1}; + + case 'tail-end' + + % Identify the end link + link_zero = 1; + + % The transform from the midpoint of the first link to its end is + % the half-link transform + frame_zero = links_m(:,:,link_zero); + + %%%%%%%% + % Jacobian to new frame is Jacobian of last link, but with an + % adjoint-inverse transform by the half-link to get to the end + halfstep = Adjinv(frame_zero); + J_zero = halfstep * J_temp{end}; + + + % Places the reference frame at the middle of the chain, splitting the + % difference between the two middle links if there is an even number of + % links + case {'centered','center','midpoint-tangent'} + + + % If there is an odd number of links, the middle link is the center of the + % chain + if N_odd + + % Identify the middle link + link_zero = ceil(N_links/2); + + % Extract the transform from the end link to the middle link + frame_zero = chain_m(:,:,link_zero); + + %%%%%% + % Jacobian for conversion is Jacobian of link zero + J_zero = J_temp{link_zero}; + + % If there is an even number of links, the midpoint is at the middle joint, + % rotated by half of that joint's angle + else + + % Identify the middle joint + joint_zero = ceil(M_joints/2); + + % Extract the position of the middle joint, and multiply it by half of + % the transform associated with its joint angle + frame_zero = jointchain_m(:,:,joint_zero) * ... + vec_to_mat_SE2(joints_v(joint_zero,:)/2); + + %%%%%%%% + % Jacobian for conversion is Jacobian of link before it, with a + % half-step to get to the end of the link, and a half rotation + % to average orientation between the two links + halfstep = Adjinv(links_v(joint_zero,:)); + halfrotation = Adjinv(joints_v(joint_zero,:)/2); + J_zero = halfrotation * halfstep * J_temp{joint_zero}; + + % 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]; + +======= + if ischar(baseframe{idx_baseframe}) || isscalar(baseframe{idx_baseframe}) +>>>>>>> master + + switch baseframe{idx_baseframe} + + % Places the reference frame at the midpoint of the lowest-index link on the chain + case 'tail' + + % Identify the first link + link_zero = 1; + + % Extract the transform from the end link to itself + frame_zero = eye(3); + + % Jacobian for base link is zero + J_zero = zeros(size(J_temp{1})); + + % Places the reference frame at the *end* of the lowest-index link on the chain + case 'tail-tip' + + % Identify the end link + link_zero = 1; + + % The transform from the midpoint of the first link to its end is + % the inverse of the half-link transform + frame_zero = inv(links_m(:,:,link_zero)); + + %%%%%%%% + % Jacobian to new frame is Jacobian of first link, but with an + % adjoint-inverse transform by the half-link to get to the end + + halfstep = Adjinv(frame_zero); + J_zero = halfstep * J_temp{1}; + + + % Places the reference frame at the middle of the chain, splitting the + % difference between the two middle links if there is an even number of + % links + case {'centered','center','midpoint-tangent'} + + + % If there is an odd number of links, the middle link is the center of the + % chain + if N_odd + + % Identify the middle link + link_zero = ceil(N_links/2); + + % Extract the transform from the end link to the middle link + frame_zero = chain_m(:,:,link_zero); + + %%%%%% + % Jacobian for conversion is Jacobian of link zero + J_zero = J_temp{link_zero}; + + % If there is an even number of links, the midpoint is at the middle joint, + % rotated by half of that joint's angle + else + + % Identify the middle joint + joint_zero = ceil(M_joints/2); + + % Extract the position of the middle joint, and multiply it by half of + % the transform associated with its joint angle + frame_zero = jointchain_m(:,:,joint_zero) * ... + vec_to_mat_SE2(joints_v(joint_zero,:)/2); + + %%%%%%%% + % Jacobian for conversion is Jacobian of link before it, with a + % half-step to get to the end of the link, and a half rotation + % to average orientation between the two links + halfstep = Adjinv(links_v(joint_zero,:)); + halfrotation = Adjinv(joints_v(joint_zero,:)/2); + J_zero = halfrotation * halfstep * J_temp{joint_zero}; + + % 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 + + + + + + % Places the reference frame at the midpoint of the highest-numbered link in the chain + case 'head' + + % Identify the end link + link_zero = N_links; + + % Extract the transform from the end link to the end link + frame_zero = chain_m(:,:,link_zero); + + %%%%%%%% + % Jacobian to new frame is Jacobian of last link + J_zero = J_temp{end}; + + % Places the reference frame at the *end of* the highest-numbered link + % in the chain + case 'head-tip' + + % Identify the end link + link_zero = N_links; + + % Extract the transform from the first link to the end link, and + % multiply it by the half-link transformation on that link + frame_zero = chain_m(:,:,link_zero)*links_m(:,:,link_zero); + + %%%%%%%% + % Jacobian to new frame is Jacobian of last link, but with an + % adjoint-inverse transform by the half-link to get to the end + halfstep = Adjinv(links_v(end,:)); + J_zero = halfstep * J_temp{end}; + + % Places the reference frame at center of mass and average orientation + % of the links, using link-lengths as weighting terms + case 'com-mean' + + % Convert link positions to row form + chain = mat_to_vec_SE2(chain_m); + + % Substitute a cumulative sum of the joint angles for the + % orientations of the links: We're not looking for the mean of the + % SO(2)-modulated orientations, we're looking for the mean angular + % displacement from the base link. + chain(:,3) = jointangles_c; + + % Take a weighted average of the link positions + CoM = sum(diag(linklengths)*chain)/L; + + + % Place the new frame at this location + frame_zero = vec_to_mat_SE2(CoM); + + %%%%%%%%%%% + % The Jacobian of the weighted average of frames is the + % weighted average of their Jacobian (by the commutativity of + % sumation and derivation operations). + + % Multiply each link's Jacobian by its link length + J_weighted = J_temp; + for idx = 1:numel(J_weighted) + J_weighted{idx} = TeLg(chain(idx,:)) * J_temp{idx} * linklengths(idx); + end + + % Sum the weighted Jacobians + J_zero = J_weighted{1}; + for idx = 2:numel(J_weighted) + J_zero = J_zero + J_weighted{idx}; + end + % J_zero = sum(cat(3,J_weighted{:}),3); + + % Divide by the total length to g + J_zero = J_zero/L; + + % Bring into local coordinates + J_zero = TgLginv(frame_zero)*J_zero; + + + %%%%%%%%%%% + % These are cases that can be used as modifiers on other baseframes + + % Places the reference frame at the proximal end of the link to + % which it is attached + case 'start' + + % Make sure that a link was previously specified + if ~exist('link_zero','var') + error('Baseframe string ''start'' may only be used as a secondary baseframe specification, where the primary specification attaches the baseframe to a link.'); + end + + % The transform from the midpoint of the first link to its proximal end is + % the inverse of the half-link transform + frame_step = inv(links_m(:,:,link_zero)); + + % compose the frame step with the previously-computed + % frame_zero + frame_zero = frame_zero*frame_step; %#ok + + %%%%%%%% + % Jacobian to new frame is previously-calculated Jacobian, but with an + % adjoint-inverse transform by the half-link to get to the end + + halfstep = Adjinv(frame_step); + J_zero = halfstep * J_zero; + + % Places the reference frame at the distal end of the link to which + % it is attached + case 'end' + + % Make sure that a link was previously specified + if ~exist('link_zero','var') + error('Baseframe string ''end'' may only be used as a secondary baseframe specification, where the primary specification attaches the baseframe to a link.'); + end + + % Transform from the midpoint to the distal end of the link + frame_step = links_m(:,:,link_zero); + + % compose the frame step with the previously-computed + % frame_zero + frame_zero = frame_zero*frame_step; + + + %%%%%%%% + % Jacobian to new frame is previously-calculated Jacobian, but with an + % adjoint-inverse transform by the half-link to get to the end + + halfstep = Adjinv(frame_step); + J_zero = halfstep * J_zero; + + + + %%%%%%%%%%% + % Several possible cases are handled within this "otherwise" case, + % because they need if/else logic + otherwise + + %%%%%%% + % If a number is provided as the input, place the baseframe on that + % link + % Check for numeric baseframe specification + if isnumeric(baseframe{idx_baseframe}) && isscalar(baseframe{idx_baseframe}) + + % Make sure that base frame specification is actually in + % the range of valid link numbers + if baseframe{idx_baseframe} <= N_links && baseframe{idx_baseframe} > 0 + + % Set the base frame as numerically specified in the input + link_zero = baseframe{idx_baseframe}; + + % Extract the transform from the end link to the end link, and + % multiply it by the half-link transformation on that link + frame_zero = chain_m(:,:,link_zero); + + %%%%%%%% + % Jacobian to new frame is Jacobian of nth link + J_zero = J_temp{baseframe{idx_baseframe}}; + + else + + error (['Baseframe specification ' num2str(baseframe{idx_baseframe}) ' is not in the range of link numbers']) + + end + + + + + %%%%%%%% + % If the provided string is the name of a sysf_ system file, pull + % the transformation to minimum-perturbation coordinates from that file + else + + %%%%% + % Robustly check if the baseframe string is a sysf_ file that + % has had minimum-perturbation coordinates calculated + + %%% + % First, strip off sysf_ if it was included by the user + + if strncmp(baseframe{idx_baseframe},'sysf_',5) + baseframe1 = baseframe{idx_baseframe}(6:end); + else + baseframe1 = baseframe{idx_baseframe}; + end + + %%% + % Second, put sysf_ onto the front of the string + + baseframe1 = ['sysf_' baseframe1]; %#ok + + %%% + % Third, check if there is a sysf_ file with this name + + load('sysplotter_config.mat','inputpath') % Get the path to the current UserFiles folder + + sysf_filepath = fullfile(inputpath, 'Systems',... % Build a string to a sysf_ file with the baseframe name + [baseframe1 '.m']); + + sysf_file_present = exist(sysf_filepath, 'file'); % Test for this file being present + + %%% + % Fourth, if there is a sysf_ file of the appropriate name, see + % if there is an associated file with the results of the system + % calculations + if sysf_file_present + + % Pulling the minimum-perturbation coordinates does not + % work with symbolic input, because it requires an + % interpolation of an array at the input points. (Given + % that minimum-perturbation coordinates are solved for + % numerically, not much is lost by not being able to + % symbolically rebase to that frame + if isa(chain_m,'sym') + error('Rebasing the chain to minimum-perturbation coordinates is not supported for symbolic input'); + end + + % Build a string to the filename where calculations from + % this system are stored + calcfilePath = fullfile(inputpath, 'sysplotter_data',... + [baseframe1 '_calc.mat']); + + % Check that the calculated system file exists + if exist(calcfilePath, 'file') + + % Import the datafile of the specified system + load(calcfilePath,'s') + + + %%%% + % Get the Jacobian from the baseframe used by the + % system to the minimum-perturbation baseframe + + + + %%% + % Interpolate the specified shape into the grids to + % extract the transform from the system's baseframe to + % its minimum-perturbation baseframe and the Jacobian + % of this transform + + frame_mp = zeros(3,1); % x y theta values of transformation + J_mp = zeros(3,numel(shapeparams)); % x y theta rows and joint angle columns of Jacobian + + shapeparams_cell = num2cell(shapeparams); % put joint angles into a cell array + + % Iterate over x y theta components + for idx = 1:numel(frame_mp) + + % Interpolate the shape angles into the grid and + % extract the corresponding component of the + % 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 + shapeparams_cell{:}); % Current shape + + % Scale the x and y components of the frame + % location + if any(idx==[1 2]) + frame_mp(idx) = L*frame_mp(idx)/s.geometry.length; + end + + % Iterate over shape components for Jacobian + 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 + shapeparams_cell{:}); % Current shape + + % Scale the x and y components of the frame + % Jacobian + if any(idx==[1 2]) + J_mp(idx,idx2) = L*J_mp(idx,idx2)/s.geometry.length; + end + end + + + + end + + + + % Convert the transforms that were just found into an + % SE(2) matrix and a Jacobian for the body velocity + % of the new frame relative to the frame in which it + % is defined + frame_mp = vec_to_mat_SE2(frame_mp); + J_mp = TgLginv(frame_mp) * J_mp; + + + % Concatenate this transform and Jacobian with any + % previously-calculated values + frame_zero = frame_zero * frame_mp; + J_zero = (Adjinv(frame_mp) * J_zero) + J_mp; + +% %%%% +% % Get the conversion factors from the tail link to the +% % baseframe used by the system +% +% % Get the geometry specification in this file +% if isfield(s,'geometry') +% if isfield(s.geometry,'baseframe') +% baseframe_original = s.geometry.baseframe; +% else +% error('Baseframe is not specified in system geometry structure') +% end +% else +% error('System does not have a geometry structure') +% end +% + +% % Get conversion factors for the original baseframe +% [frame_original,J_original] =... +% N_link_conversion_factors(C); +% % chain_m,... +% % jointchain_m,... +% % links_m,... +% % joints_m,... +% % links_v,... +% % joints_v,... +% % jointangles,... +% % linklengths,... +% % shapeparams,... +% % modes,... +% % J_temp,... +% % baseframe_original); +% +% % Combine original conversion with minimum-perturbation +% % conversion +% +% % Frame construction is straightforward concatenation +% % of SE(2) transforms +% frame_zero = frame_original * frame_mp; +% +% % Combination of body-velocity Jacobians is by taking +% % the adjoint-inverse of the proximal Jacobian by the +% % distal transformation, then adding the distal +% % Jacobian +% J_zero = (Adjinv(frame_mp) * J_original) + J_mp; + + end + + else + error (['Baseframe specification ' baseframe{idx_baseframe} ' is not a valid option, and there is no system file with that name']) + end + + end + end + + %%%%%%% + % If a transformation matrix is provided as the input, apply it + % to the base frame (stacked onto any previous base frame) + + elseif isnumeric(baseframe{idx_baseframe}) + + + if ~exist('J_zero','var') + + % Identify the first link + link_zero = 1; + + % Extract the transform from the end link to itself + frame_zero = eye(3); + + % Jacobian for base link is zero + J_zero = zeros(size(J_temp{1})); + + end + + % Step by the provided transformation + frame_step = baseframe{idx_baseframe}; + + % Apply the step relatve to the pre-existing frame zero + frame_zero = frame_zero * frame_step; + + % Use the adjoint-inverse of the step to modify the + % Jacobian + J_zero = Adjinv(frame_step) * J_zero; + + end + + + end + + % Update J_temp to be the J_zero computed at this stage of the loop +% J_temp = J_zero; + + if ~exist('link_zero','var') + link_zero = 0; + end + +end + + + \ No newline at end of file diff --git a/ProgramFiles/Geometry/NLinkChain/N_link_conversion_move_chain.m b/ProgramFiles/Geometry/NLinkChain/N_link_conversion_move_chain.m new file mode 100644 index 00000000..f29b1930 --- /dev/null +++ b/ProgramFiles/Geometry/NLinkChain/N_link_conversion_move_chain.m @@ -0,0 +1,138 @@ +function [h_m,J,J_full,C] = N_link_conversion_move_chain(C,frame_zero,J_zero) +%%%%%%% +% This is a helper-function for N_link_chain. +% +% This function moves the existing baseframe of the chain described in C to +% a frame at frame_zero relative to the baseframe. (This is different from +% N_link_conversion, which leaves the chain fixed and moves the baseframe +% +% N_link_chain takes in an argument that specifies which link on the chain +% (or another frame, such as a center-of-mass frame or one loaded from a +% system file). +% +% All of the N_link_chain calculations are made with the lowest-numbered +% (leftmost, or 'tail') link as the base link. +% +% Transforming the positions of the link relative to the base frame is +% straightforward: We simply multiply each transformation by the inverse of +% the transformation from the old base frame to the new base frame +% +% To transform the link Jacobians into the new coordinates, we need the +% Jacobian from joint velocities to body velocity of new frame, with +% first link fixed. This calculation depends on how the kinematic map +% from the original base frame to the new base frame is calculated. +% +% The code here uses the conversion transformation and Jacobian calculated +% in N_link_conversion_factors. + + + % Extract elements from structure array (some functions like size do not + % work properly if applied to struct contents without a breakout) + chain_m = C.chain_m; + jointchain_m = C.jointchain_m; + J_temp = C.J_temp; + + % Preallocate a matrix for holding the transformed link and joint location matrices + h_m = zeros(size(chain_m)); + hj_m = zeros(size(jointchain_m)); + + % If we're working with symbolic variables, then we need to explicitly make + % the array symbolic, because matlab tries to cast items being inserted + % into an array into the array class, rather than converting the array to + % accomodate the class of the items being inserted + if isa(chain_m,'sym') + h_m = sym(h_m); + end + + if isa(jointchain_m,'sym') + hj_m = sym(hj_m); + end + + + % For each link and joint in the chain, transform its matrix by the inverse of the + % central transformation + for idx = 1:size(h_m,3) + h_m(:,:,idx) = frame_zero * chain_m(:,:,idx); + + if isa(chain_m,'sym') + h_m(:,:,idx) = simplify(h_m(:,:,idx),'steps',10); + end + + end + + for idx = 1:size(hj_m,3) + hj_m(:,:,idx) = frame_zero * jointchain_m(:,:,idx); + + if isa(chain_m,'sym') + hj_m(:,:,idx) = simplify(hj_m(:,:,idx),'steps',10); + end + + end + + + % Save h_m to the chain description structure as the new chain_m + C.chain_m = h_m; + C.jointchain_m = hj_m; + + + %%%%%%%% + % Use the Jacobian for the transformation from the old base frame to + % the new base frame to compute the link Jacobians with respect to that + % frame + + % First operation is to get the Jacobian that maps shape velocity to + % body velocity of the links, taking the frame that frame_zero is + % defined with respect to as fixed + % + % This is achieved by multiplying the Jacobian from the original base + % frame to the new base frame by the Adjoint-inverse of the + % transformation from the new base frame to the link, and then + % subtracting this value from the original + J_new = J_temp; + for idx = 1:numel(J_new) + J_new{idx} = ... + (J_temp{idx} + ... % Jacobian from joint velocities to body velocity of link, with first link of the chain fixed + Adjinv(chain_m(:,:,idx)) * ... % Adjoint-inverse transformation by position of this link relative to frame_zero + J_zero); % Jacobian from joint velocities to body velocity of frame_zero, with its reference frame fixed + end + + % Save this Jacobian out to the chain description structure + C.J_temp = J_new; + + + % Second operation is to transform the link Jacobians such that they + % return the velocities of the links in the coordinate directions of + % the new base frame (the reference frame for frame_zero), instead of in each link's local coordinates + % + % This is achieved by multiplying each link Jacobian by the left lifted + % action of the transformation from the new base frame to the link + J = J_new; + for idx = 1:numel(J_new) + J{idx} = TeLg(h_m(:,:,idx)) * J_new{idx}; % Left lifted action rotates into new base frame coordinates + + + %Simplify J expression if necessary + if isa(J{idx},'sym') + J{idx} = simplify(J{idx},100); + end + + end + + + % Third operation is to calculate the full Jacobian from body velocity + % of the base frame and shape velocity to body velocity of each link. + % + % This is achieved by augmenting each of the link's body-velocity + % Jacobians with a new block that contains the Adjoint-inverse of the + % transformation from the new base frame to the link + J_full = J_new; + for idx = 1:numel(J_new) + + Adjointinverse_body_transform = Adjinv(h_m(:,:,idx)); % Adjoint-inverse transformation by position of this link relative to frame zero + + J_full{idx} = [Adjointinverse_body_transform J_full{idx}]; % Place the Adjoint-inverse matrix as the first three columns of J_full + end + + + +end \ No newline at end of file diff --git a/ProgramFiles/Geometry/NLinkChain/branched_chain.m b/ProgramFiles/Geometry/NLinkChain/branched_chain.m new file mode 100644 index 00000000..8187a686 --- /dev/null +++ b/ProgramFiles/Geometry/NLinkChain/branched_chain.m @@ -0,0 +1,345 @@ +function [h, J, J_full,frame_zero,J_zero] = branched_chain(geometry,shapeparams_global) +% Build a backbone for a branching chain of links. The geometry input +% should be a cell array of structures, each of which has the same elements +% as required for an N_link_chain, and also (except for the first chain) +% has an "attachment" field that specifies the chain it should be attached +% to and a location at which to attach it, using the same format as +% baseframe. +% +% Inputs: +% +% geometry: Structure containing information about geometry of chain. +% Fields are: +% +% linklengths: Vectors of link lengths defining a kinematic chain. +% For a subchain (any chain other than the first), setting the +% first link length to zero will mean that it directly hinges off +% of the chain to which it is attached, rather than having a +% fixed link attached to the chain +% +% baseframe (optional): Specification of which link on the body should be +% treated as the base frame. Default behavior is to put the base +% frame at the center of the primary chain, either at the midpoint of the +% middle link, or at the joint between the two middle links and at +% the mean of their orientation. Options for this field are: +% +% 'centered' : Default behavior +% 'tail' : Lowest-numbered link is the base frame +% 'tail-tip' : Start of lowest-numbered link is the base frame +% 'head' : Highest-numbered link is the base frame +% 'head-tip' : End of the highest-numbered link is the base frame +% numeric : Specify a link number to use as a base frame +% sysf_ Pull minimum-perturbation coordinates from a +% sysf_ file. Argument should be the name of +% a system in the current UserFiles folder +% 'start' : Modifier on a link specification (e.g., +% {2,'start'} to put the frame at the proximal end of the +% specified link +% 'end' : Modifier on a link specification (e.g., +% {head,'end'} to put the frame at the distal end of the +% specified link +% transform: A 3x3 SE(2) matrix giving the position +% of the base frame. This can be a standalone entry or a +% modifier on any other frame. +% +% modes (optional): Option to map input "shapeparams" across +% multiple links in a chain (which can have more joints than the +% provided number of "shapeparams". 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 +% will not be scaled. +% +% shapeparams: A vector of the angles between the links. Must be one +% element shorter than the linklengths vector% +% +% +% +% Outputs: +% +% h : Locations of the chain links relative to the selected base frame, +% and lengths of the links +% +% h.pos: The link locations are stored in an Nx3 array, with +% each row a link and the columns corresponding to x,y,theta +% values for that link. These can be converted to stacks of SE(2) +% matrices via vec_to_mat_SE2, and back to vectors by +% mat_to_vec_SE2 +% +% h.lengths : Vector of linklengths. This is the original link length +% specification passed into the function, scaled by L, and stored +% as a column of values. +% +% J : Jacobians from joint angle velocities to velocity of each link +% relative to the base frame (the standard derivative of h). +% These Jacobians are stored in a cell array, with one matrix per +% cell. +% +% J_full : Full Jacobians from body velocity of the base frame and joint +% angle velocities to body velocity of each link relative to a +% non-moving frame. +% +% frame_zero : The transformation from the first link to the selected +% baseframe +% +% J_zero : The Jacobian from shape velocity to body velocity of the +% selected baseframe, with the first link held fixed. +% + +% Build a lookup table from joint numbers to joints on the subchains +joint_lookup_local_to_global = cell(size(geometry.subchains)); +joint_count = 0; + +joint_lookup_global_to_local = []; + +for idx = 1:numel(joint_lookup_local_to_global) + + % Handle any local modes applied to the system + % If no modes are specified, use an identity mapping for the modes + if ~isfield(geometry.subchains{idx},'modes') || isempty(geometry.subchains{idx}.modes) + geometry.subchains{idx}.modes = eye(numel(geometry.subchains{idx}.linklengths)-1); + end + + joint_lookup_local_to_global{idx} = joint_count + (1:size(geometry.subchains{idx}.modes,2))'; + + joint_count = joint_count + size(geometry.subchains{idx}.modes,2); + + joint_lookup_global_to_local = [joint_lookup_global_to_local; % Anything already in joint_lookup + [size(joint_lookup_global_to_local,1)+(1:size(geometry.subchains{idx}.modes,2))' ... % A sequence of numbers starting where existing numbers leave off + , idx*ones(size(geometry.subchains{idx}.modes,2),1) ... % The subchain number + , (1:size(geometry.subchains{idx}.modes,2))'] ]; % The joint number within the subchain + + +end + +% Handle any global modes applied to the system +% If no modes are specified, use an identity mapping for the modes +if ~isfield(geometry,'modes') || isempty(geometry.modes) + modes = eye(numel(shapeparams_global)); +else + modes = geometry.modes; +end + +% Expand jointangles from specified shape variables to actual joint angles +% by multiplying in the modal function +shapeparams_global = shapeparams_global(:); +shapeparams = modes*shapeparams_global; + + +% Get the kinematics for each individual chain +[h_set, J_set, J_full_set,frame_zero_set,J_zero_set,chain_description_set] ... + = cellfun(@(geom,shape) N_link_chain(geom,shapeparams(shape)),geometry.subchains,joint_lookup_local_to_global,'UniformOutput',false); + +%%%%%%%%% +% Internally pad the Jacobians with zeros so that they line up with their +% actual joint numbers + +J_set_padded = cell(size(J_set)); +J_full_set_padded = cell(size(J_full_set)); +J_temp_set_padded = cell(size(chain_description_set)); + +% Iterate over the individual chains +for idx = 1:numel(J_set) + + + % Extract the local-to-global joint lookup for joint on this subchain + jllg = joint_lookup_local_to_global{idx}; + + + % Create matrices of the right size to hold the J and J full matrices + J_set_padded{idx} = repmat({zeros(3,numel(shapeparams))},size(J_set{idx})); + J_full_set_padded{idx} = repmat({zeros(3,3+numel(shapeparams))},size(J_set{idx})); + J_temp_set_padded{idx} = repmat({zeros(3,numel(shapeparams))},size(chain_description_set{idx}.J_temp)); + + % Iterate over the Jacobans in the chain + for idx2 = 1:numel(J_set_padded{idx}) + + % Move the columns of J_set to their global joint number + J_set_padded{idx}{idx2}(:,jllg) = J_set{idx}{idx2}; + + % Move the shape columns of the J_full_set + J_full_set_padded{idx}{idx2}(:,1:3) = J_full_set{idx}{idx2}(:,1:3); + J_full_set_padded{idx}{idx2}(:,3+jllg) = J_full_set{idx}{idx2}(:,4:end); + + % Move the columns of J_full_set to their global joint number + J_temp_set_padded{idx}{idx2}(:,jllg) = chain_description_set{idx}.J_temp{idx2}; + + end + + % Overwrite the original J values with their padded versions + J_set{idx} = J_set_padded{idx}; + J_full_set{idx} = J_full_set_padded{idx}; + chain_description_set{idx}.J_temp = J_temp_set_padded{idx}; + +end + +% Loop over the subchains +for idx = 2:numel(h_set) + + % Extract the attachment parameters for ths subchain, and put it into + % the chain_description + attach = geometry.subchains{idx}.attach; + + chain_description_parent = chain_description_set{attach.parent}; + + chain_description_parent.baseframe = attach.location; + + % Put the padded J_temp into the chain_description + chain_description_set{idx}.J_temp = J_temp_set_padded{idx}; + + % Use the modified chain descriptions to get the location and Jacobian of the + % attachment point relative to the first chain's baseframe + [frame_zero,J_zero,link_zero] = N_link_conversion_factors(chain_description_parent); + + % Move the chain to frame_zero, J_zero + [h_m,J_set{idx},J_full_set{idx},chain_description_set{idx}] ... + = N_link_conversion_move_chain(chain_description_set{idx},frame_zero,J_zero); + + h_set{idx}.pos = mat_to_vec_SE2(h_m); + + % Update the cumulative sum of joint angles + if ~iscell(attach.location) + attach.location = {attach.location}; + end + for idx2 = 1:numel(attach.location) + + if isscalar(attach.location{idx2}) + orientation_offset = chain_description_parent.jointangles_c(attach.location{idx2}); + elseif ischar(attach.location{idx2}) && strncmp(attach.location{idx2},'tail',4) + orientation_offset = chain_description_parent.jointangles_c(1); + elseif ischar(attach.location{idx2}) && strncmp(attach.location{idx2},'head',4) + orientation_offset = chain_description_parent.jointangles_c(end); + elseif isnumeric(attach.location{idx2}) + t_v = mat_to_vec_SE2(attach.location{idx2}); + orientation_offset = t_v(3); + elseif ischar(attach.location{idx2}) && ( strncmp(attach.location{idx2},'start',4) || strncmp(attach.location{idx2},'end',4)) + orientation_offset = 0; + else + orientation_offset = 0; + warning('Chain attachment does not call out a link, errors may occur if the whole chain is specified to be in averaged coordinates') + end + chain_description_set{idx}.jointangles_c ... + = chain_description_set{idx}.jointangles_c ... + + orientation_offset; + end + + +% % Extract the attachment parameters for ths subchain +% attach = geometry{idx}.attach; +% +% % Get the position of the link to which this subchain is attached +% h_attach = vec_to_mat_SE2(h_set{attach.parent}.pos(attach.link,:)); +% +% % Multiply the link transformations by the attachment transformation +% for idx2 = 1:size(h_set{idx}.pos,2) +% h_set{idx}.pos(idx2,:) = mat_to_vec_SE2( h_attach * vec_to_mat_SE2(h_set{idx}.pos(idx2,:))); +% end +% +% % Transform the Jacobians +% for idx2 = 1:numel(J_set_padded{idx}) +% +% % Use left lifted action of the attachment point to modify the +% % in-frame Jacobian +% J_set_padded{idx}{idx2} = TeLg(h_attach) * J_set_padded{idx}{idx2}; +% +% % Convert the full Jacobian +% Adjointinverse_body_transform = Adjinv(h_attach); % Adjoint-inverse transformation by position of attachment point +% +% J_full_set_padded{idx}{idx2} = [Adjointinverse_body_transform * J_full_set_padded{idx}{idx2}(:,1:3), J_full_set_padded{idx}{idx2}(:,4:end)]; +% +% end + +end + + +% % Combine the h_set values into a single item +% h.pos = []; +% h.lengths = []; +% for idx = 1:numel(h_set) +% h.pos = [h.pos;h_set{idx}.pos]; +% h.lengths = [h.lengths;h_set{idx}.lengths]; +% end +% +% J = {}; +% for idx = 1:numel(J_set) +% +% J = [J,J_set{idx}]; %#ok +% +% end +% +% +% J_full = {}; +% for idx = 1:numel(J_set) +% +% J_full = [J_full,J_full_set{idx}]; %#ok +% +% end + + +%%%%%%%%%%% +% Apply any global baseframe and mode transformations + + + +% Baseframe modifications +chain_description.chain_m = []; +chain_description.jointchain_m = []; +chain_description.links_m = []; +chain_description.joints_m = []; +chain_description.links_v = []; +chain_description.joints_v = []; +chain_description.jointangles = []; +chain_description.jointangles_c = []; +chain_description.linklengths = []; +chain_description.shapeparams = []; +chain_description.modes = []; +chain_description.J_temp = []; +chain_description.baseframe = {}; + +for idx = 1:numel(chain_description_set) + + chain_description.chain_m = cat(3,chain_description.chain_m, chain_description_set{idx}.chain_m); + chain_description.jointchain_m = cat(3,chain_description.jointchain_m, chain_description_set{idx}.jointchain_m); + chain_description.links_m = cat(3,chain_description.links_m, chain_description_set{idx}.links_m); + chain_description.joints_m = cat(3,chain_description.joints_m, chain_description_set{idx}.joints_m); + chain_description.links_v = [chain_description.links_v; chain_description_set{idx}.links_v]; + chain_description.joints_v = [chain_description.joints_v; chain_description_set{idx}.joints_v]; + chain_description.jointangles = [chain_description.jointangles; chain_description_set{idx}.jointangles]; + chain_description.jointangles_c = [chain_description.jointangles_c; chain_description_set{idx}.jointangles_c]; + chain_description.linklengths = [chain_description.linklengths; chain_description_set{idx}.linklengths]; + chain_description.J_temp = [chain_description.J_temp, chain_description_set{idx}.J_temp]; + +end + +chain_description.shapeparams = shapeparams; +chain_description.modes = modes; +chain_description.baseframe = geometry.baseframe; + +% Make the final conversion by the overall baseframe specification +[frame_zero,J_zero] = N_link_conversion_factors(chain_description); + +[h_m,J,J_full,chain_description] = N_link_conversion(chain_description,frame_zero,J_zero); + + +% Apply modal restriction to shape components of Jacobians +for idx = 1:numel(J) + J{idx} = J{idx} * modes; + J_full{idx}(:,4:end) = J_full{idx}(:,4:end)*modes; +end + + +% For output, convert h into row form. Save this into a structure, with +% link lengths included +h.pos = mat_to_vec_SE2(h_m); + +h.lengths = []; +for idx = 1:numel(h_set) + h.lengths = [h.lengths;h_set{idx}.lengths]; +end + + +end \ No newline at end of file diff --git a/ProgramFiles/Geometry/NLinkChain/fat_branched_chain.m b/ProgramFiles/Geometry/NLinkChain/fat_branched_chain.m new file mode 100644 index 00000000..52d3c86e --- /dev/null +++ b/ProgramFiles/Geometry/NLinkChain/fat_branched_chain.m @@ -0,0 +1,170 @@ +function [B,h,J, J_full] = fat_branched_chain(geometry, jointangles, display) +% Generate a set of rounded-end links based on link lengths and joint angles in +% a kinematic chain +% +% Inputs: +% +% geometry: Structure containing information about geometry of chain. +% Fields are: +% +% linklengths: A vector of link lengths defining a kinematic chain +% +% baseframe (optional): Specification of which link on the body should be +% treated as the base frame. Default behavior is to put the base +% frame at the center of the chain, either at the midpoint of the +% middle link, or at the joint between the two middle links and at +% the mean of their orientation. Options for this field are +% specified in the documentation of N_link_chain. +% +% +% 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 +% will not be scaled. +% +% display: Structure containing information about how the system should +% be illustrated. Field required by this function is: +% +% aspect_ratio: ratio of width to length of the system +% +% sharpness: fraction of backbone taken up the rounding on one end +% of one link (specified this way so that the sharpness value on +% an N-link system matches that for a continuous backbone with +% the same appearance, and so that increasing link numbers does +% not blunt the ends of the links.) +% +% +% jointangles: A vector of the angles between the links. Must be one +% element shorter than the linklengths vector +% +% +% Outputs: +% +% B : Locus of points for the fat chain. First two rows are x and y +% values, third row is all ones (so that the columns can be +% transformed by SE(2) matrices. +% +% h : Locations of the chain links relative to the selected base frame, +% and lengths of the links +% +% h.pos: The link locations are stored in an Nx3 array, with +% each row a link and the columns corresponding to x,y,theta +% values for that link. These can be converted to stacks of SE(2) +% matrices via vec_to_mat_SE2, and back to vectors by +% mat_to_vec_SE(2) +% +% h.lengths : Vector of linklengths. This is the original link length +% specification passed into the function, scaled by L, and stored +% as a column of values. +% +% J : Jacobians from joint angle velocities to velocity of each link +% relative to the base frame (the standard derivative of h). +% These Jacobians are stored in a cell array, with one matrix per +% cell. +% +% J_full : Full Jacobians from body velocity of the base frame and joint +% angle velocities to body velocity of each link relative to a +% non-moving frame. +% +% frame_zero : The transformation from the first link to the selected +% baseframe +% +% J_zero : The Jacobian from shape velocity to body velocity of the +% selected baseframe, with the first link held fixed. + + + + + % Fill in default display parameters if not provided + if ~exist('display','var') || ~isfield(display,'aspect_ratio') + display.aspect_ratio = 0.03; + end + if ~isfield(display,'sharpness') + display.sharpness = 0.03; % This matches some shapes originally drawn in OmniGraffle + end + + % Get the positions and scaled lengths of the links + [h,J, J_full] = branched_chain(geometry,jointangles); + + % Convert the backbone locations into SE(2) transforms + g = vec_to_mat_SE2(h.pos); + + %%%%%%%% + % Create the links + + points = 50; % Number of points per link + N_links = size(g,3); % Number of links in the chain + B = []; % Array to hold points on chain + + % Generate the width of the links from the aspect ratio multiplied + % by the total length of the system + width = display.aspect_ratio * sum(h.lengths); + + % Generate the size of the tip of one link from the sharpness parameter + cap_width = display.sharpness * sum(h.lengths); + + + + % Iterate over links + for idx = 1:N_links + + if h.lengths(idx) ~= 0 + + %%% + % Create an ellipse-ended rectangle for the link + + % Make the shape slightly shorter than this link (so that there is + % a whitespace gap between them when plotted) + length = h.lengths(idx)/1.05; % + + % get the points on the ellipse-ended rectangle + C = squashed_rectangle(length,width,cap_width,points); + + % Transform rectangle to link position + C = g(:,:,idx) * C; + + % Append C onto the end of B, and add a NaN so that the links will + % plot discontinuously + B = [B, C, NaN(3,1)]; + + end + + end + +end + + +function XY = squashed_rectangle(length,width,cap_width,points) + + % Get the X,Y points on a "squashed rectangle" as defined in the OmniGraffle + % drawing program (a rectangle for which cap_fraction of the width is a split + % ellipse. To visually match OmniGraffle figures, cap_fraction should be .2 + % + % All the points returned will be on the elliptical end caps, with the + % final points on the caps the endpoints of the straight edges. the points + % parameter specifies how many points are included in each half-ellipse. + + %Create a parameterization for the points on the ellipse + t = linspace(0,pi,points)'; + + %Generate an ellipse whose 'a' axis is the width of the link, and whose + % 'b' axis is twice the cap width + a = width/2; + b = cap_width; + + X = -b*sin(t); + Y = a*cos(t); + + %Offset the cap + %X = X-(1-cap_width)*length/2; + X = X-(length/2-cap_width); + + %Mirror the cap + X = [X;-X;X(1)]; + Y = [Y;-Y;Y(1)]; + + % Augment these values with a column of ones to make them amenable to + % SE(2) transformations + XY = [X Y ones(2*points+1,1)]'; + +end \ No newline at end of file diff --git a/ProgramFiles/Geometry/generate_locomotor_locus.m b/ProgramFiles/Geometry/generate_locomotor_locus.m index 040636e1..92fda8ca 100644 --- a/ProgramFiles/Geometry/generate_locomotor_locus.m +++ b/ProgramFiles/Geometry/generate_locomotor_locus.m @@ -15,6 +15,9 @@ case {'n-link chain'} drawing_generator = @fat_chain; + + case {'branched chain'} + drawing_generator = @fat_branched_chain; end end diff --git a/ProgramFiles/Physics/Inertial/Discrete_links/Inertia_link.m b/ProgramFiles/Physics/Inertial/Discrete_links/Inertia_link.m new file mode 100644 index 00000000..32996bc1 --- /dev/null +++ b/ProgramFiles/Physics/Inertial/Discrete_links/Inertia_link.m @@ -0,0 +1,24 @@ +function [Inertia_link_system,Inertia_link_local] = Inertia_link(h,J_full,L,link_shape_parameters,fluid_density) +% Calculate the matrix that maps from system body and shape velocities to +% forces acting on the base frame of the system + + % These are for an elliptical link. Can be generalized to other shapes + + % length and width of an elliptical link + a = L/2; + b = link_shape_parameters.aspect_ratio * a; + + mass_link = pi*a*b; + + rotational_inertia_link = mass_link*(a^2+b^2)/4; + + added_mass = fluid_density * diag([pi*b^2, pi*a^2, (a^2-b^2)^2]); + + Inertia_link_local = diag([mass_link,mass_link,rotational_inertia_link]) + added_mass; + + + % Pullback of local inertia to body velocity/shape velocity coordinates + + Inertia_link_system = J_full' * Inertia_link_local * J_full; + +end \ No newline at end of file diff --git a/ProgramFiles/Physics/Inertial/Discrete_links/Inertial_connection_discrete.m b/ProgramFiles/Physics/Inertial/Discrete_links/Inertial_connection_discrete.m old mode 100644 new mode 100755 index df57ee5a..9cba5c12 --- a/ProgramFiles/Physics/Inertial/Discrete_links/Inertial_connection_discrete.m +++ b/ProgramFiles/Physics/Inertial/Discrete_links/Inertial_connection_discrete.m @@ -1,4 +1,4 @@ -function [A, h, J, J_full, omega] = Inertial_connection_discrete(geometry,physics,jointangles) +function [A, h, J, J_full, omega,M_full] = Inertial_connection_discrete(geometry,physics,jointangles) % Calculate the local connection for for an inertial system (floating in space or % ideal high-Re fluid) % @@ -57,12 +57,35 @@ % A is calculated. This matrix is a linear map from system body and % shape velocities to net external forces acting on the base frame, % which must be zero for all achievable motions of the system - +% %%%% % First, get the positions of the links in the chain and their % Jacobians with respect to the system parameters - [h, J, J_full] = N_link_chain(geometry,jointangles); + if isfield(geometry,'subchains') + [h, J, J_full] = branched_chain(geometry,jointangles); + else + [h, J, J_full] = N_link_chain(geometry,jointangles); + end + + %If examining inter-panel interaction + if isfield(physics,'interaction') + s.geometry = geometry; + s.physics = physics; + + %Then, get added mass metric using flat-plate panel method + [M_full,J,J_full,h,~] = getAddedMass_NLinkChain(jointangles',s); + + % Pfaffian is first three rows of M_full + omega = M_full(1:3,:); + + % Build the local connection + A = omega(:,1:3)\omega(:,4:end); + % T = A; + + return + end + %%%%%%%% % We are modeling low Reynolds number physics as being resistive @@ -88,24 +111,37 @@ % x,y,theta motion, and one column per joint), and there is one % contribution per link. This structure is of the same dimensions as % J_full, so we use it as a template. - link_inertias = J_full; + link_force_maps = J_full; % Now iterate over each link, calculating the map from system body and % shape velocities to forces acting on the body - for idx = 1:numel(link_inertias) + link_inertias = cell(size(link_force_maps)); + local_inertias = cell(size(link_force_maps)); + + for idx = 1:numel(link_force_maps) - link_inertias{idx} = Inertia_link(h.pos(idx,:),... % Position of this link relative to the base frame + [link_inertias{idx},local_inertias{idx}] = Inertia_link(h.pos(idx,:),... % Position of this link relative to the base frame J_full{idx},... % Jacobian from body velocity of base link and shape velocity to body velocity of this link h.lengths(idx),... % Length of this link - geometry.link_shape{idx},... % Shape type of this link geometry.link_shape_parameters{idx},... % Shape parametes for this link physics.fluid_density); % Fluid density relative to link end - % Sum the force-maps for each link to find the total map from system - % body and shape velocities to force actign on the body - M_full = sum(cat(3,link_inertias{:}),3); + if isa(link_inertias{1},'sym') + M_full = sym(zeros(size(link_inertias{1}))); + inertia_stack = cat(3,link_inertias{:}); + for i = 1:size(inertia_stack,1) + for j = 1:size(inertia_stack,2) + temp = inertia_stack(i,j,:); + M_full(i,j) = sum(temp(:)); + end + end + else + % Sum the force-maps for each link to find the total map from system + % body and shape velocities to force actign on the body + M_full = sum(cat(3,link_inertias{:}),3); + end % Pfaffian is first three rows of M_full @@ -113,35 +149,5 @@ % Build the local connection A = omega(:,1:3)\omega(:,4:end); - - + T = A; end - - -function Inertia_link_system = Inertia_link(h,J_full,L,link_shape,link_shape_parameters,fluid_density) -% Calculate the matrix that maps from system body and shape velocities to -% forces acting on the base frame of the system - - % These are for an elliptical link. Can be generalized to other shapes - - % length and width of an elliptical link - a = L/2; - b = link_shape_parameters.aspect_ratio * a; - - mass_link = pi*a*b; - - rotational_inertia_link = mass_link*(a^2+b^2)/4; - - added_mass = fluid_density * diag([pi*b^2, pi*a^2, (a^2-b^2)^2]); - - Inertia_link_local = diag([mass_link,mass_link,rotational_inertia_link]) + added_mass; - - - % Pullback of local inertia to body velocity/shape velocity coordinates - - Inertia_link_system = J_full' * Inertia_link_local * J_full; - - - - -end \ No newline at end of file diff --git a/ProgramFiles/Physics/Inertial/Discrete_links/Inertial_metric_discrete.m b/ProgramFiles/Physics/Inertial/Discrete_links/Inertial_metric_discrete.m new file mode 100644 index 00000000..9792377c --- /dev/null +++ b/ProgramFiles/Physics/Inertial/Discrete_links/Inertial_metric_discrete.m @@ -0,0 +1,135 @@ +function Mp = Inertial_metric_discrete(geometry,physics,jointangles) +% Calculate the power dissipation metric for an n-link chain under a +% viscous resitive-force model +% +% Inputs: +% +% geometry: Structure containing information about geometry of chain. +% Fields required for this function are: +% +% linklengths: A vector of link lengths defining a kinematic chain +% +% baseframe (optional): Specification of which link on the body should be +% treated as the base frame. Default behavior is to put the base +% frame at the center of the chain, either at the midpoint of the +% middle link, or at the joint between the two middle links and at +% the mean of their orientation. Options for this field are +% specified in the documentation of N_link_chain. +% +% 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 +% will not be scaled. +% +% physics: Structure containing information about the system's physics. +% Fields are: +% +% drag_ratio: Ratio of lateral to longitudinal drag +% +% drag_coefficient: Drag per unit length for longitudinal direction +% +% jointangles: Angles of the joints between the links, defining the +% chain's current shape. +% +% +% Output: +% +% Mp: A matrix (m x m matrix, where m is the number of elements in +% the joint angle input). This matrix encodes the resistance of the +% viscous medium to motion of the joints. It acts as a linear map +% from joint angular velocity to torques acting on the joints, and as +% a quadratic map from joint angular velocity to power being +% dissipated through the system. +% +% If Mp is taken as a metric tensor, pathlengths calculated from it +% have lengths in units of "sqrt(power)*time". This pathlength is a +% pacing-indepenent cost for the motion, and represents a "minimum +% cost" for following the path, which is achieved for a trajectory +% that follows the path at constant speed as measured by the metric, +% which itself corresponds to constant power. Given two paths, the path +% with a shorter length according to this metric can always be +% traversed more quickly at a given power, or with a lower power in a +% given time. + + + %%%%%%% + % To calculate this metric, first get the Local connection and full + % mass matrix for the system + + [A, ~,~, ~,~,M_full] = Inertial_local_connection(geometry,physics,jointangles); + + % Now fold down the mass matrix onto the shape space + + Mp = [-A.' eye(size(A,2))] * M_full * [-A; eye(size(A,2))]; + +% %%%%%%%% +% % Now calculate the metric contribution to each link +% +% % Pre-allocate storage for the metric contributions. Each contribution +% % is m x m (square matrix with as many rows/columns as there are shape +% % variables), and there is one contribution per link. +% link_metrics = repmat({zeros(numel(jointangles))},size(J_full)); +% +% % Get the Metric contribution from each link. This is the drag matrix +% % acting on an individual link, pulled back onto the space of joint +% % angles by the total Jacobian (including locomotion effects) from +% % joint angular velocity to the motion of the link. +% for idx = 1:numel(link_metrics) +% +% link_metrics{idx} = LowRE_link_metric(J_full{idx},... % Jacobian from body velocity of base link and shape velocity to body velocity of this link +% A,... % Local connection (i.e. Jacobian from shape velocity to body velocity of base link) +% h.lengths(idx),... % Length of this link +% physics.drag_ratio,... % Ratio of lateral to longitudinal drag +% physics.drag_coefficient); % Bulk drag coefficient +% end +% +% %%%%%%% +% % Finally, sum the metric contributions of the links to get the metric +% % for the whole system. +% Mp = sum(cat(3,link_metrics{:}),3); +% +% +% +% end +% +% function link_metric = LowRE_link_metric(J_full,A,L,drag_ratio,c) +% % Calculate the contribution to the power metric from a link. This is the +% % drag resistance on the link, pre- and post-multiplied by the Jacobian +% % from shape velocity to this link's body velocity +% +% %%%%%%% +% % Local drag on a link. +% % Longitudinal drag is proportional to longitudinal velocity. +% % Lateral drag is proportional to lateral velocity, with a different drag +% % coefficient, specified by its ratio to the longitudinal drag. +% % Rotational drag is proportional to rotational velocity, with a +% % coefficient based on integrating lateral drag along the length +% % of the link +% % c is the absolute drag coefficient +% +% % (Note that drag terms need to be positive here to make this a +% % positive-definite matrix that we can use as a metric) +% drag_matrix = [L 0 0; +% 0 drag_ratio*L 0; +% 0 0 drag_ratio/12*L^3]*c; +% +% +% %%%%%%%%%%% +% % Calculate the jacobian from shape variables to body velocity of this +% % point on the system: +% +% % -A maps shape velocity to body velocity of the system, so augmenting +% % -A with an identity matrix produces a map from shape velocity to body +% % and shape velocity of the system +% J_intermediate = [-A; eye(size(A,2))]; +% +% % J_full maps the system's body and shape velocity to the body velocity +% % of this link. +% J_total = J_full * J_intermediate; +% +% % Pre- and post-multiplying J_total by the drag matrix pulls the drag +% % matrix back from being a metric on the link's body velocities to +% % being a metric on the system's shape velocities +% link_metric = J_total' * drag_matrix * J_total; +% +% end \ No newline at end of file diff --git a/ProgramFiles/Physics/Inertial/Discrete_links/getAddedMass_NLinkChain.m b/ProgramFiles/Physics/Inertial/Discrete_links/getAddedMass_NLinkChain.m new file mode 100644 index 00000000..d122e3d0 --- /dev/null +++ b/ProgramFiles/Physics/Inertial/Discrete_links/getAddedMass_NLinkChain.m @@ -0,0 +1,627 @@ +function [FullMassMatrix,J,J_Full,h,local_inertias] = getAddedMass_NLinkChain(shape,s) +% Nathan Justus, 5/25/2020 +% Adaptation of work by Eva Kanso 2004 for sysplotter + +%-----------INPUTS------------ +% shape - column vector of shape variables +% s - structure containing geometry and physical properties of N-link chain + +%----------OUTPUT------------- +% AddedMassMatrix - locked inertia matrix (size: 3x3) + + +%Normalize swimmer to length 1 + +%Number of elliptic links in chain geometry +numLinks = numel(s.geometry.linklengths); +if isfield(s.geometry,'linkSeparation') + linkSep = s.geometry.linkSeparation; +else + linkSep = 0; + s.geometry.linkSeparation = 0; +end +totalLength = sum(s.geometry.linklengths)+linkSep*(numLinks-1); +s.geometry.length = s.geometry.length/totalLength; +s.geometry.linklengths = s.geometry.linklengths/totalLength; +s.geometry.linkSeparation = s.geometry.linkSeparation/totalLength; + +%If not specified, turn 'on' all links +if ~isfield(s.geometry,'activelinks') + s.geometry.activelinks = ones(1,numLinks); +end + +%If not specified, turn off inter-link interaction to save computation time +if ~isfield(s.physics,'interaction') + s.physics.interaction = 'off'; +end + + +%Build geometry of body shape +[zc,t,n,del,orientations,linkCenters,h] = buildNLinkBody(shape,s); + +%Get full mass matrix and jacobians +[FullMassMatrix,J,J_Full,local_inertias] = fullMassMatrix_NLinkChain_byLink(zc,t,n,del,s,shape,orientations,linkCenters); + +end + +function [allPanelPoints,t,n,del,orientations,zg,h] = buildNLinkBody(shape,s) + +% Nathan Justus 5/26/20 + +% Generates panel positions, normal/tangent vectors, and other relevant +% spatial information based on given geometry + +%-----------INPUTS------------- +% shape - column vector of shape variables +% s - structure containing geometry and physical properties of N-link chain + +%-----------OUTPUTS------------ +% allPanelPoints - position of ellipse points w.r.t base frame from s +% t - components of normal vectors tangent to ellipse panels +% n - components of outward normal vectors for ellipse panels +% del - ellipse panel lengths +% orientations - vector of ellipse orientations w.r.t. swimmer frame +% zg - matrix of ellipse cg's w.r.t. swimmer frame +% h - structure encoding swimmer configuration + +%Deal joint angles depending on if we're using mode shapes +if isfield(s.geometry,'modes') + jointAngles = s.geometry.modes*shape; +else + jointAngles = shape; +end + +% Number of elliptic links in chain geometry +numLinks = numel(s.geometry.linklengths); +% List of ellipse lengths +linklengths = s.geometry.linklengths; +% Define ratio parameters for ellipse: +aspectRatio = s.geometry.link_shape_parameters{1}.aspect_ratio; +% Number of panels comprising ellipse +npts = 100; +% Separation distance between ellipses at joints - avoids singularities +linkSep = s.geometry.linkSeparation/2; + +% Check to make sure that the given information makes sense with geometry +if numel(jointAngles)+1 ~= numLinks + error('Wrong number of joints for this system'); +end + +%XY locations of c.o.m. for each ellipse w.r.t. first link c.o.m. +zg = zeros(numLinks,2); +%Orientation of each link +orientation = 0; +%Get cg of each link w.r.t. first link cg +for i = [1:numLinks-1] + %Get semi-major axis of current ellipse and next ellipse + a1 = linklengths(i)/2; + a2 = linklengths(i+1)/2; + %Grab cg of current ellipse + lastCG = zg(i,:); + %Move to point at right edge of this ellipse (joint location) + rightEdge = lastCG + [(a1+linkSep)*cos(orientation),(a1+linkSep)*sin(orientation)]; + %If there are an even number of links and this joint is the middle, + if mod(numLinks,2) == 0 && i == numLinks/2 + %Log middle mosition as joint location + middlePosition = rightEdge; + end + %Rotate by the joint angle + orientation = orientation + jointAngles(i); + %Move to cg of new link + zg(i+1,:) = rightEdge + [(a2+linkSep)*cos(orientation),(a2+linkSep)*sin(orientation)]; +end + +%Deal out link orientations with respect to middle link +orientations = []; +%If odd number of links, frame alighned with middle link frame +if mod(numLinks,2) == 1 + for i = [1:numLinks] + if i < numLinks/2 + orientations(i) = -sum(jointAngles(i:floor(numLinks/2))); + elseif i == floor(numLinks/2)+1 + orientations(i) = 0; + elseif i > numLinks/2 + 1 + orientations(i) = sum(jointAngles(floor(numLinks/2)+1:i-1)); + end + end +%If even number of links, frame is oriented at average of the two +%center-most link orientations +else + orientations(1) = 0; + for i = [1:numel(jointAngles)] + orientations(i+1) = orientations(i) + jointAngles(i); + end + %Orientation of base frame with respect to leftmost link + middleOrientation = mean(orientations(numLinks/2:numLinks/2+1)); + orientations = orientations - middleOrientation; +end + +%Move/rotate center of masses for each link based off base frame +if s.geometry.baseframe == 'center' + %If odd number of links, get orientation and frame location from center + %ellipse + if mod(numLinks,2) == 1 + %Get orientation of middle link w.r.t. left link + middleOrientation = sum(jointAngles(1:numel(jointAngles)/2)); + %Get cg position of middle link w.r.t. left link cg + middlePosition = zg(floor(numLinks/2)+1,:); + end + %Shift cgs so frame center is at 0,0 + zg = zg - repmat(middlePosition,numLinks,1); + %Rotate cgs about center link so frame is aligned with working x,y + R = [cos(-middleOrientation),-sin(-middleOrientation);... + sin(-middleOrientation),cos(-middleOrientation)]; + zg = (R*zg')'; +end + +%Make structure with link cg positions and lengths for logging +h.pos = [zg,orientations']; +h.lengths = s.geometry.linklengths'; + +%Move ellipses to correct cgs/orientations +allPanelPoints = []; +t = []; +n = []; +del = []; +%For each link in the ellipse +for i = [1:numLinks] + %If that link is turned 'on' + if s.geometry.activelinks(i) + %Get ellipse data + a = linklengths(i)/2; + b = a*aspectRatio; + [zcg,ts,ns,dels] = ellipse(a,b,npts); + %Make rotation matrix + R = [cos(orientations(i)),-sin(orientations(i));... + sin(orientations(i)),cos(orientations(i))]; + %Rotate and translate ellipse to correct position/orientation + thisPanel = (R*zcg')' + zg(i,:); + allPanelPoints = [allPanelPoints;thisPanel]; + %Rotate tangent vectors + thisT = (R*ts')'; + t = [t;thisT]; + %Get normal vectors + thisN = (R*ns')'; + n = [n;thisN]; + %Get panel lengths + del = [del;dels]; + end +end + + +end + +function [An,Bt,Phi] = influence_NLinkChain_byLink(zc,t,n,del,N) +% ----------------- +% E Kanso, 14 april 2004 + +% -----------------INPUT +% +% zc position of collocation pts +% t components of vectors tangent to panels +% n components of outward normal vectors +% del panel length +% N number of panels +% +% zc, t and n are w.r.t base frame +% +% ---------------- + + +% -----------------INTERNAL VARIABLES +% +% Xrel(i,j) & Yrel(i,j) coordinates of control pt i (Ci) relative +% to control pt j (Cj) w.r.t inertial frame +% +% Cn(i,j) & Ct(i,j) normal and tangential coordinates of Ci relative +% to Cj w.r.t. a frame attached to the panel j +% +% Vn(i,j) & Vt(i,j) normal and tangential velocities induced +% at Ci due to a constant source distribution +% at panel j, w.r.t. a frame attached to panel j +% +% Vx(i,j) & Vy(i,j) velocities Vn(i,j) and Vt(i,j) +% expressed w.r.t. inertial frame +% +% ---------------- + + +% -----------------OUTPUT +% +% An(i,j) & Bt(i,j) normal and tangential velocities induced at Ci +% due to a constant source distribution at panel j +% Expressed w.r.t a frame attached to the panel i +% +% Phi potential function +% +% ---------------- + + +% initialize +Xc = zeros(N,N); Yc = zeros(N,N); +tx = zeros(N,N); ty = zeros(N,N); +nx = zeros(N,N); ny = zeros(N,N); +Ds = zeros(N,N); + + +% assign +Xc = zc(:,1)*ones(1,N); Yc = zc(:,2)*ones(1,N); +tx = t(:,1)*ones(1,N); ty = t(:,2)*ones(1,N); +nx = n(:,1)*ones(1,N); ny = n(:,2)*ones(1,N); + +Ds = 0.5.*ones(N,1)*del'; + + +% compute Xrel(i,j) and Yrel(i,j) +XREL = Xc-Xc'; +YREL = Yc-Yc'; + +% compute Cn(i,j) and Ct(i,j) +Cn = XREL.*nx' + YREL.*ny'; +Ct = XREL.*tx' + YREL.*ty'; + + +% compute Vn(i,j) and Vt(i,j) +temp1 = Cn.^2; +temp2 = (Ct + Ds).^2; +temp3 = (Ct - Ds).^2; +Vt_Num = temp2 + temp1; +Vt_Den = temp3 + temp1; +Vt = log(Vt_Num./Vt_Den); +Vn_Num = 2.*Cn.*Ds; +Vn_Den = Ct.^2 + temp1 - Ds.^2; +angle = 2.*atan(Vn_Num./Vn_Den); +Vn = angle + 2*pi*eye(N,N); + +% compute Vx(i,j) and Vy(i,j) +Vx = Vn.*nx' + Vt.*tx'; +Vy = Vn.*ny' + Vt.*ty'; + +% compute An(i,j) and Bt(i,j) +An = Vx.*nx + Vy.*ny; +Bt = Vx.*tx + Vy.*ty; + +% compute Phi +Phi = - Ct.*Vt - Cn.*Vn - Ds.*log((temp2 + temp1).*(temp3 + temp1)); + +end + +function linkAddedMass = getAddedMassByLink(s,zcg,t,n,del,linkNumber,inv_An,Phi) + +% Nathan Justus 5/26/20 + +% Generates added mass for a given link in swimmer frame + +%-----------INPUTS------------- +% s - structure containing geometry and physical properties of N-link chain +% zcg - N by 2 matrix of panel locations where N is # of panels +% t - N by 2 matrix of panel unit tangent vectors +% n - N by 2 matrix of panel unit normal vectors +% del - N-length vector of panel lengths +% linkNumber - scalar identifier for ellipse, relative to ~all~ links, l->r +% inv_An - inverse of induced normal velocities - see influence function +% Phi - potential function - see influence function for details + +%-----------OUTPUTS------------ +% linkAddedMass - 3 by 3 added mass matrix for chosen link in swimmer frame + +%Get system information +%Density of fluid swimmer is submerged in +density = s.physics.fluid_density; +%Number of links turned 'on' in system +numLinks = sum(s.geometry.activelinks); +%Redo linkNumber to be relative to only links turned 'on' +linkNumber = sum(s.geometry.activelinks(1:linkNumber)); +%number of panels in body +totalPoints = numel(del); +%number of panels per ellipse +npts = totalPoints/numLinks; + +%normal panel velocity due to angular rotation at unit speed +avel = zcg(:,1).*n(:,2) - zcg(:,2).*n(:,1); +%normal panel velocity due to unit velocity in x direction (right) +xvel = ones(totalPoints,1).*n(:,1); +%normal panel velocity due to unit velocity in y direction (up) +yvel = ones(totalPoints,1).*n(:,2); + +%For every active link +for i = [1:numLinks] + + %Body velocity components - zero everywhere but at corresponding links + vfn_x = zeros(totalPoints,1); + vfn_y = zeros(totalPoints,1); + vfn_a = zeros(totalPoints,1); + vfn_x(npts*(i-1)+1:npts*i) = xvel(npts*(i-1)+1:npts*i); + vfn_y(npts*(i-1)+1:npts*i) = yvel(npts*(i-1)+1:npts*i); + vfn_a(npts*(i-1)+1:npts*i) = avel(npts*(i-1)+1:npts*i); + vfn{i}.x = vfn_x; + vfn{i}.y = vfn_y; + vfn{i}.a = vfn_a; + + %Source density distributions + sigma{i}.x = inv_An*vfn_x; + sigma{i}.y = inv_An*vfn_y; + sigma{i}.a = inv_An*vfn_a; + + %Potential functions + phi{i}.x = Phi*sigma{i}.x; + phi{i}.y = Phi*sigma{i}.y; + phi{i}.a = Phi*sigma{i}.a; + +end + +%Storage for added mass contributions +mass = zeros(3); +%Compute nondimensionalized added masses for each active ellipse's effect +%on every other active ellipse +i = linkNumber; +for j = [1:numLinks] + %Added mass of ellipse just by itself + if i == j + xx = density*sum(phi{i}.x.*vfn{i}.x.*del); + yy = density*sum(phi{i}.y.*vfn{i}.y.*del); + aa = density*sum(phi{i}.a.*vfn{i}.a.*del); + xy = density*sum(phi{i}.x.*vfn{i}.y.*del); + xa = density*sum(phi{i}.x.*vfn{i}.a.*del); + ya = density*sum(phi{i}.y.*vfn{i}.a.*del); + thisMass = [xx,xy,xa;... + xy,yy,ya;... + xa,ya,aa]; + %Add contribution to total added mass matrix + mass = mass+thisMass; + %Added mass of ellipse from interaction with other ellipse + else + xx = density*sum(phi{i}.x.*vfn{j}.x.*del); + yy = density*sum(phi{i}.y.*vfn{j}.y.*del); + aa = density*sum(phi{i}.a.*vfn{j}.a.*del); + xy = .5*density*(sum(phi{i}.x.*vfn{j}.y.*del)+sum(phi{j}.x.*vfn{i}.y.*del)); + xa = .5*density*(sum(phi{i}.x.*vfn{j}.a.*del)+sum(phi{j}.x.*vfn{i}.a.*del)); + ya = .5*density*(sum(phi{i}.y.*vfn{j}.a.*del)+sum(phi{j}.y.*vfn{i}.a.*del)); + thisMass = [xx,xy,xa;... + xy,yy,ya;... + xa,ya,aa]; + %Add contribution to total added mass matrix + mass = mass+thisMass; + end +end + +%Return total added mass matrix +linkAddedMass = mass; + +end + +function [FullMassMatrix,jacobians,jacobians_full,local_inertias] = fullMassMatrix_NLinkChain_byLink(zcg,t,n,del,s,shape,orientations,linkCenters) +% Nathan Justus 5/26/20 + +% Generates full mass matrix for system in swimmer frame + +%-----------INPUTS------------- +% zcg - N by 2 matrix of panel locations where N is # of panels +% t - N by 2 matrix of panel unit tangent vectors +% n - N by 2 matrix of panel unit normal vectors +% del - N-length vector of panel lengths +% s - structure containing geometry and physical properties of N-link chain +% shape - shape variables for swimmer +% orientations - vector of ellipse orientations w.r.t. swimmer frame +% linkCenters - matrix of ellipse cg's w.r.t. swimmer frame + +%-----------OUTPUTS------------ +% FullMassMatrix - (3+S) by (3+S) mass matrix where S is # of shape vars +% jacobians - M-length cell vector of 3 by S matrices of link body +% jacobians where M is number of system ellipses +% jacobians_full - M-length cell vector of 3 by (3+S) matrices +% of link+spatial body jacobians + +%Storage for mass matrix +FullMassMatrix = zeros(3+numel(shape)); +%Number of links in system +numLinks = size(linkCenters,1); +%Density of surrounding fluid +density = s.physics.fluid_density; +%Number of panels in system +totalPoints = numel(del); +%Initialize cell storage for jacobians +jacobians = {}; +jacobians_full = {}; +%Initialize cell storage for local inertias +local_inertias = {}; + +%If inter-panel interaction is turned on +if strcmp(s.physics.interaction,'on') + %Get panel relative influences and potential function + [An,Bt,Phi] = influence_NLinkChain_byLink(zcg,t,n,del,totalPoints); + %Invert induced panel normal velocities + inv_An = inv(An); +end + +%Aspect ratio of ellipses in chain +aspectRatio = s.geometry.link_shape_parameters{1}.aspect_ratio; + +%For each ellipse in the chain +for link = [1:numLinks] + + %Initialize local inertia as zero mass matrix + local_inertias{link} = zeros(3); + + %Compute jacobian for corresponding link in chain + thisJacobian = getJacobian_NLinkChain(s,shape,link,orientations,linkCenters); + jacobians_full{link} = thisJacobian; + jacobians{link} = thisJacobian(:,4:end); + + %If the link is turned 'on' + if s.geometry.activelinks(link) + + %Get ellipse parameters for this link + linkLength = s.geometry.linklengths(link); + a = linkLength/2; + b = a*aspectRatio; + + %Calculate ellipse mass in link frame + mass_link = pi*a*b; + rotational_inertia_link = mass_link*(a^2+b^2)/4; + standardmass = diag([mass_link,mass_link,rotational_inertia_link]); + + %If interaction is turned on, use potential field to get added mass + if strcmp(s.physics.interaction,'on') + %Get link added mass in base swimmer frame + linkAddedMass = getAddedMassByLink(s,zcg,t,n,del,link,inv_An,Phi); + %Push forward calculated mass from base frame to link frame + pushforwardJ = thisJacobian(:,1:3); + iJ = inv(pushforwardJ); + iJt = inv(pushforwardJ'); + linkAddedMass = iJt*linkAddedMass*iJ; + %If interaction is turned off, just use known ellipse added mass + else + m_xx = density*pi*b^2; + m_yy = density*pi*a^2; + m_aa = density*pi/8*(a^2-b^2)^2; + linkAddedMass = diag([m_xx,m_yy,m_aa]); + end + + %Log local inertia in link frame + local_inertias{link} = linkAddedMass + standardmass; + + %Pull back added mass + standard mass to base frame through full joint jacobians + thisFullMass = thisJacobian'*(linkAddedMass+standardmass)*thisJacobian; + %Add this to the total mass matrix + FullMassMatrix = FullMassMatrix+thisFullMass; + end + +end + +end + +function bodyJacobian = getJacobian_NLinkChain(s,shape,linkNumber,orientations,linkCenters) +% Nathan Justus 5/26/20 + +% Generates body jacobian for link wrt swimmer frame + +%-----------INPUTS------------- +% s - structure containing geometry and physical properties of N-link chain +% shape - shape variables for swimmer +% linkNumber - identifier for link in chain wrt ~all~ other links +% orientations - vector of ellipse orientations w.r.t. swimmer frame +% linkCenters - matrix of ellipse cg's w.r.t. swimmer frame + +%-----------OUTPUTS------------ +% bodyJadobian - 3 by (3+S) body jacobian matrix for given link wrt +% swimmer base frame where S is # of shape variables + +%If using shape modes +if isfield(s.geometry,'modes') + %Import them to convert shape variables to joint angles + jointAngles = s.geometry.modes*shape; +%Otherwise +else + %Just use joint angles directly + jointAngles = shape; +end + +%Get distance between edge of ellipse and actual joint location +linkSeparation = s.geometry.linkSeparation/2; + +%If the base frame is in the middle of the center link +if s.geometry.baseframe == 'center' + %Figure out which link number corresponds to center link + midLink = numel(jointAngles)/2+1; + %Initialize local body jacobian + bodyJacobian = zeros(3,numel(jointAngles)); + %Initialize adjoint chain + adjoints = eye(3); + %If the chosen link is to the right of the center link, we will walk + %left along the chain back to the center + if linkNumber > midLink + for i = [linkNumber:-1:midLink+.5] + linkWidth1 = s.geometry.linklengths(i)/2; + linkStep1 = linkWidth1+linkSeparation; + linkWidth2 = s.geometry.linklengths(i-1)/2; + linkStep2 = linkWidth2+linkSeparation; + %Step halfway along a link + stepRight1 = invAdj2D([linkStep1,0,0]); + stepRight2 = invAdj2D([linkStep2,0,0]); + %If there are an even number of ellipses and almost at middle + if floor(midLink) ~= midLink && i-midLink < 1 + %Rotate frame by half joint angle + rotate = invAdj2D([0,0,jointAngles(i-1)/2]); + %Get the jacobian column for a half-angle at joint location + jacobianCol = adjoints*stepRight1*rotate*[0;0;.5]; + bodyJacobian(:,i-1) = jacobianCol; + %Move adjoint chain to base frame + adjoints = adjoints*stepRight1*rotate; + %If not to middle yet or for an odd number of ellipses + else + %Rotate frame by the joint angle + rotate = invAdj2D([0,0,jointAngles(i-1)]); + %Get the jacobian column from chain so far + jacobianCol = adjoints*stepRight1*rotate*[0;0;1]; + bodyJacobian(:,i-1) = jacobianCol; + %Move adjoint chain to center of proximal link for next step + adjoints = adjoints*stepRight1*rotate*stepRight2; + end + end + %If the chosen link is to the left of the center link, we will walk + %right along the chain back to the center + elseif linkNumber < midLink + for i = [linkNumber:midLink-.5] + linkWidth1 = s.geometry.linklengths(i)/2; + linkStep1 = linkWidth1+linkSeparation; + linkWidth2 = s.geometry.linklengths(i+1)/2; + linkStep2 = linkWidth2+linkSeparation; + %Step halfway along a link to the left + stepLeft1 = invAdj2D([-linkStep1,0,0]); + stepLeft2 = invAdj2D([-linkStep2,0,0]); + %If even number of links and almost to middle + if floor(midLink) ~= midLink && midLink-i < 1 + %Rotate frame opposite of half joint angle, since moving backwards + rotate = invAdj2D([0,0,-jointAngles(i)/2]); + %Get the jacobian column for a half-angle at joint location + jacobianCol = adjoints*stepLeft1*rotate*[0;0;-.5]; + bodyJacobian(:,i) = jacobianCol; + %Move adjoint chain to center of base frame + adjoints = adjoints*stepLeft1*rotate; + else + %Rotate frame opposite of joint angle, since moving backwards + rotate = invAdj2D([0,0,-jointAngles(i)]); + %Get jacobian column from chain so far + jacobianCol = adjoints*stepLeft1*rotate*[0;0;-1]; + bodyJacobian(:,i) = jacobianCol; + %Move adjoint chain to center of proximal link for next step + adjoints = adjoints*stepLeft1*rotate*stepLeft2; + end + end + end + + %If using shape modes, pull jacobian through them to get jacobian wrt + %actual shape variables instead of joint angles + if isfield(s.geometry,'modes') + bodyJacobian = bodyJacobian*s.geometry.modes; + end + + %Tack on 3x3 adjoints matrix to account for body movements + bodyJacobian = [adjoints,bodyJacobian]; +end + +end + +function iadj = invAdj2D(g) +% Nathan Justus 5/26/20 + +% Get inverse adjoint representation of a rotation/translation + +%-----------INPUTS------------- +% g - list holding x,y,alpha specifying translation/rotation amount + +%-----------OUTPUTS------------ +% iadj - inverse adjoint operator for body jacobian construction + +%Deal out variables +x = g(1); +y = g(2); +a = g(3); + +%Construct adjoint operator +adj = [cos(a),-sin(a),y;sin(a),cos(a),-x;0,0,1]; +%Take the inverse +iadj = inv(adj); + +end \ No newline at end of file diff --git a/ProgramFiles/Physics/Inertial/Inertial_energy_metric.m b/ProgramFiles/Physics/Inertial/Inertial_energy_metric.m new file mode 100644 index 00000000..97ee36ce --- /dev/null +++ b/ProgramFiles/Physics/Inertial/Inertial_energy_metric.m @@ -0,0 +1,21 @@ +function Mp = Inertial_energy_metric(geometry,physics,shapeparams) +% Calculate the dissipation power metric for a set of curvature bases + +% Identify what kind of system is being calculated, and use this to specify how +% the local connection should be generated +switch geometry.type + + case {'curvature basis','curvature bases','general curvature'} + physics_function = @Inertial_metric_continuous; + + case {'n-link chain','branched chain'} + physics_function = @Inertial_metric_discrete; + +end + +% Call the physics function identified for the system +Mp = physics_function(geometry,physics,shapeparams); + + + +end \ No newline at end of file diff --git a/ProgramFiles/Physics/Inertial/Inertial_local_connection.m b/ProgramFiles/Physics/Inertial/Inertial_local_connection.m old mode 100644 new mode 100755 index 58fc0c95..c93e5dc9 --- a/ProgramFiles/Physics/Inertial/Inertial_local_connection.m +++ b/ProgramFiles/Physics/Inertial/Inertial_local_connection.m @@ -1,5 +1,6 @@ -function [A, h, J,Omega] = Inertial_local_connection(geometry,physics,shapeparams) -% Calculate the local connection for an inertial system. +%function [A, M_a,J_full, local_inertias,M_full,h,J] = Inertial_local_connection(geometry,physics,shapeparams) +function [A, h, J,J_full,Omega,M_full] = Inertial_local_connection(geometry,physics,shapeparams) +% Calculate the local connection for a set of curvature bases % % Inputs: % geometry: structure defining system geometry @@ -9,8 +10,9 @@ % deformation (e.g., curvature or joint angles) % geometry.length: total length of swimmer % physics: structure defining system physics -% drag_ratio: ratio of lateral to longitudinal drag -% drag_coefficient: drag per unit length +% fluid_density: fluid density relative to body density +% interactions: boolean as to whether the added mass interactions +% between body panels should be considered % cparams: value of shape variables @@ -23,12 +25,12 @@ case {'curvature basis','curvature bases','general curvature'} physics_function = @Inertial_connection_continuous; - case {'n-link chain'} + case {'n-link chain','branched chain'} physics_function = @Inertial_connection_discrete; end % Call the physics function identified for the system -[A, h, J,Omega] = physics_function(geometry,physics,shapeparams); +[A, h, J,Omega,J_full,M_full] = physics_function(geometry,physics,shapeparams); end \ No newline at end of file diff --git a/ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_connection_continuous.m b/ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_connection_continuous.m index f5afcb7b..2f0dcec9 100644 --- a/ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_connection_continuous.m +++ b/ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_connection_continuous.m @@ -1,4 +1,4 @@ -function [A, h, J,Omega] = LowRE_connection_continuous(geometry,physics,shapeparams) +function [A, h, J,J_full,Omega]= LowRE_connection_continuous(geometry,physics,shapeparams) % Calculate the local connection for a set of curvature bases % % Inputs: @@ -16,13 +16,13 @@ %Generate backbone geometry and Jacobian from its local definition -[h,J] = backbone(geometry,shapeparams); +[h,J,J_full] = backbone(geometry,shapeparams); % Itegrate from one halflength before the midpoint to one halflength after it int_limit = [-0.5 0.5]; % Now integrate to get the pfaffian -Omega_sol = ode45( @(s,Omega) LowRE_Pfaffian_infinitesimal(s,h(s),J(s),geometry.length,physics.drag_coefficient,physics.drag_ratio),int_limit,zeros(3,3+length(shapeparams))); +Omega_sol = ode45( @(s,Omega) LowRE_Pfaffian_infinitesimal(s,h(s),J(s),J_full(s),geometry.length,physics.drag_coefficient,physics.drag_ratio),int_limit,zeros(3,3+length(shapeparams))); % Reshape the terms of the Pfaffian into a matrix of the correct dimension Omega = reshape(deval(Omega_sol,int_limit(end)),3,[]); @@ -35,7 +35,7 @@ end -function dOmega = LowRE_Pfaffian_infinitesimal(s,h,J,lambda,c,drag_ratio) %#ok +function dOmega = LowRE_Pfaffian_infinitesimal(s,h,J,J_full,lambda,c,drag_ratio) %#ok % Calculate the derivative of the local connection as it's built up along % the backbone diff --git a/ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_metric_continuous.m b/ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_metric_continuous.m index 8a27271c..d1098232 100644 --- a/ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_metric_continuous.m +++ b/ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_metric_continuous.m @@ -8,7 +8,7 @@ drag_matrix = [1 0; 0 physics.drag_ratio]*physics.drag_coefficient; % Get the backbone locus, Jacobian, and Local Connection functions - [A, h, J] = LowRE_local_connection(geometry,physics,shapeparams); + [A,h,J] = LowRE_local_connection(geometry,physics,shapeparams); % Integrate along the body for the power metric Mp_sol = ode45(@(s,Mp) dMetric(s,Mp,A,h(s/geometry.length),J(s/geometry.length),drag_matrix),int_limit,zeros(length(shapeparams)));%zeros(length(shapeparams)^2,1)); diff --git a/ProgramFiles/Physics/LowReynoldsRFT/Discrete_links/LowRE_connection_discrete.m b/ProgramFiles/Physics/LowReynoldsRFT/Discrete_links/LowRE_connection_discrete.m index bf10dada..6de50833 100644 --- a/ProgramFiles/Physics/LowReynoldsRFT/Discrete_links/LowRE_connection_discrete.m +++ b/ProgramFiles/Physics/LowReynoldsRFT/Discrete_links/LowRE_connection_discrete.m @@ -57,7 +57,11 @@ %%%% % First, get the positions of the links in the chain and their % Jacobians with respect to the system parameters - [h, J, J_full] = N_link_chain(geometry,jointangles); + if isfield(geometry,'subchains') + [h, J, J_full] = branched_chain(geometry,jointangles); + else + [h, J, J_full] = N_link_chain(geometry,jointangles); + end %%%%%%%% % We are modeling low Reynolds number physics as being resistive @@ -89,7 +93,7 @@ % shape velocities to forces acting on the body for idx = 1:numel(link_force_maps) - link_force_maps{idx} = LowRE_body_drag_link(h.pos(idx,:),... % Position of this link relative to the base frame + link_force_maps{idx}= LowRE_body_drag_link(h.pos(idx,:),... % Position of this link relative to the base frame J_full{idx},... % Jacobian from body velocity of base link and shape velocity to body velocity of this link h.lengths(idx),... % Length of this link physics.drag_ratio,... % Ratio of lateral to longitudinal drag @@ -192,8 +196,6 @@ [-L 0 0; 0 -drag_ratio*L 0; 0 0 -drag_ratio/12*L^3]*c; - - %%%%%%%%%% % Mapping from system body and shape velocity to body velocity of the @@ -203,6 +205,5 @@ % therefore % product of the two matrices calculated above and J_full omega = F_local_to_F_baseframe * gcirc_local_to_F_local * J_full; - end \ No newline at end of file diff --git a/ProgramFiles/Physics/LowReynoldsRFT/Discrete_links/LowRE_metric_discrete.m b/ProgramFiles/Physics/LowReynoldsRFT/Discrete_links/LowRE_metric_discrete.m index 666dd120..9fd627ac 100644 --- a/ProgramFiles/Physics/LowReynoldsRFT/Discrete_links/LowRE_metric_discrete.m +++ b/ProgramFiles/Physics/LowReynoldsRFT/Discrete_links/LowRE_metric_discrete.m @@ -55,6 +55,7 @@ %%%%%%% % To calculate this metric, first get the Local Connection, link % configurations, and full Jacobians for the links + [A,h,~,J_full] = LowRE_local_connection(geometry,physics,jointangles); %%%%%%%% diff --git a/ProgramFiles/Physics/LowReynoldsRFT/LowRE_dissipation_metric.m b/ProgramFiles/Physics/LowReynoldsRFT/LowRE_dissipation_metric.m index bf8c4d99..a11b1dac 100644 --- a/ProgramFiles/Physics/LowReynoldsRFT/LowRE_dissipation_metric.m +++ b/ProgramFiles/Physics/LowReynoldsRFT/LowRE_dissipation_metric.m @@ -8,7 +8,7 @@ case {'curvature basis','curvature bases','general curvature'} physics_function = @LowRE_metric_continuous; - case {'n-link chain'} + case {'n-link chain','branched chain'} physics_function = @LowRE_metric_discrete; end diff --git a/ProgramFiles/Physics/LowReynoldsRFT/LowRE_local_connection.asv b/ProgramFiles/Physics/LowReynoldsRFT/LowRE_local_connection.asv new file mode 100644 index 00000000..85787eb9 --- /dev/null +++ b/ProgramFiles/Physics/LowReynoldsRFT/LowRE_local_connection.asv @@ -0,0 +1,98 @@ +function [A, M_a,J_full, local_inertias,M_full,h] = LowRE_local_connection(geometry,physics,shapeparams) +% Calculate the local connection for a set of curvature bases +% +% Inputs: +% geometry: structure defining system geometry +% geometry.type: how the system geometry is defined +% (e.g., links, curvature basis, general curvature) +% geometry.function: map from shape variables to local backbone +% deformation (e.g., curvature or joint angles) +% geometry.length: total length of swimmer +% physics: structure defining system physics +% drag_ratio: ratio of lateral to longitudinal drag +% drag_coefficient: drag per unit length +% cparams: value of shape variables + + + + +% Identify what kind of system is being calculated, and use this to specify how +% the local connection should be generated +switch geometry.type + + case {'curvature basis','curvature bases','general curvature'} + physics_function = @LowRE_connection_continuous; + %ForDebugging + M_ + + case {'n-link chain','branched chain'} + physics_function = @LowRE_connection_discrete; + %Get inertial properties for calculating inertial metric + [A, M_a,J_full, local_inertias,M_full] = Inertial_tensors_discrete(geometry,physics,shapeparams); + +end + +% Call the physics function identified for the system +[A, h, J,J_full,Omega] = physics_function(geometry,physics,shapeparams); + +end + + +% %Generate backbone geometry and Jacobian from its local definition +% [h,J] = backbone(geometry,shapeparams); +% +% % Itegrate from one halflength before the midpoint to one halflength after it +% int_limit = geometry.length*[-0.5 0.5]; +% +% % Now integrate to get the pfaffian +% Omega_sol = ode45( @(s,Omega) LowRE_Pfaffian_infinitesimal(s,h(s),J(s),physics.drag_coefficient,physics.drag_ratio),int_limit,zeros(3,3+length(shapeparams))); +% +% % Reshape the terms of the Pfaffian into a matrix of the correct dimension +% Omega = reshape(deval(Omega_sol,int_limit(end)),3,[]); +% +% % Calculate the local connection by multiplying the inverse of the first +% % block in the Pfaffian by the second block +% A = Omega(:,1:3)\Omega(:,4:end); +% +% +% end +% +% +% function dOmega = LowRE_Pfaffian_infinitesimal(s,h,J,c,drag_ratio) %#ok +% % Calculate the derivative of the local connection as it's built up along +% % the backbone +% +% % Convert velocity to local velocity +% gdot_to_gcirc_local = TgLginv(h); +% +% % Local drag, based on unit longitudinal drag, lateral according to the ratio, no local +% % torsional drag, multiplied by drag coefficient +% gcirc_local_to_F_local = ... +% [-1 0 0; +% 0 -drag_ratio 0; +% 0 0 0]*c; +% +% % Transfer force to midpoint-tangent frame by transpose of the +% % adjoint-inverse action +% F_local_to_F_midpoint = transpose(Adjinv(h)); +% +% % shape component of pfaffian +% % (map from shape velocities to midpoint-tangent forces) +% omega2 = F_local_to_F_midpoint ... +% * gcirc_local_to_F_local ... +% * gdot_to_gcirc_local ... +% * J; % J is rdot to gdot mapping +% +% % system body velocity component of pfaffian +% omega1 = F_local_to_F_midpoint ... +% * gcirc_local_to_F_local ... +% * gdot_to_gcirc_local ... +% * TeRg(h); % Mapping from gdot of system to gdot of outboard point +% +% % Combine contributions to Pfaffian from shape and position motion +% dOmega = [omega1 omega2]; +% +% % Turn Pfaffian into column vector for ODE45 +% dOmega = dOmega(:); +% +% end \ No newline at end of file diff --git a/ProgramFiles/Physics/LowReynoldsRFT/LowRE_local_connection.m b/ProgramFiles/Physics/LowReynoldsRFT/LowRE_local_connection.m index d172793d..070dbf24 100644 --- a/ProgramFiles/Physics/LowReynoldsRFT/LowRE_local_connection.m +++ b/ProgramFiles/Physics/LowReynoldsRFT/LowRE_local_connection.m @@ -1,4 +1,4 @@ -function [A, h, J,Omega] = LowRE_local_connection(geometry,physics,shapeparams) +function [A, h, J,J_full,Omega] = LowRE_local_connection(geometry,physics,shapeparams) % Calculate the local connection for a set of curvature bases % % Inputs: @@ -23,72 +23,12 @@ case {'curvature basis','curvature bases','general curvature'} physics_function = @LowRE_connection_continuous; - case {'n-link chain'} + case {'n-link chain','branched chain'} physics_function = @LowRE_connection_discrete; end % Call the physics function identified for the system -[A, h, J,Omega] = physics_function(geometry,physics,shapeparams); +[A, h, J,J_full,Omega] = physics_function(geometry,physics,shapeparams); -end - - -% %Generate backbone geometry and Jacobian from its local definition -% [h,J] = backbone(geometry,shapeparams); -% -% % Itegrate from one halflength before the midpoint to one halflength after it -% int_limit = geometry.length*[-0.5 0.5]; -% -% % Now integrate to get the pfaffian -% Omega_sol = ode45( @(s,Omega) LowRE_Pfaffian_infinitesimal(s,h(s),J(s),physics.drag_coefficient,physics.drag_ratio),int_limit,zeros(3,3+length(shapeparams))); -% -% % Reshape the terms of the Pfaffian into a matrix of the correct dimension -% Omega = reshape(deval(Omega_sol,int_limit(end)),3,[]); -% -% % Calculate the local connection by multiplying the inverse of the first -% % block in the Pfaffian by the second block -% A = Omega(:,1:3)\Omega(:,4:end); -% -% -% end -% -% -% function dOmega = LowRE_Pfaffian_infinitesimal(s,h,J,c,drag_ratio) %#ok -% % Calculate the derivative of the local connection as it's built up along -% % the backbone -% -% % Convert velocity to local velocity -% gdot_to_gcirc_local = TgLginv(h); -% -% % Local drag, based on unit longitudinal drag, lateral according to the ratio, no local -% % torsional drag, multiplied by drag coefficient -% gcirc_local_to_F_local = ... -% [-1 0 0; -% 0 -drag_ratio 0; -% 0 0 0]*c; -% -% % Transfer force to midpoint-tangent frame by transpose of the -% % adjoint-inverse action -% F_local_to_F_midpoint = transpose(Adjinv(h)); -% -% % shape component of pfaffian -% % (map from shape velocities to midpoint-tangent forces) -% omega2 = F_local_to_F_midpoint ... -% * gcirc_local_to_F_local ... -% * gdot_to_gcirc_local ... -% * J; % J is rdot to gdot mapping -% -% % system body velocity component of pfaffian -% omega1 = F_local_to_F_midpoint ... -% * gcirc_local_to_F_local ... -% * gdot_to_gcirc_local ... -% * TeRg(h); % Mapping from gdot of system to gdot of outboard point -% -% % Combine contributions to Pfaffian from shape and position motion -% dOmega = [omega1 omega2]; -% -% % Turn Pfaffian into column vector for ODE45 -% dOmega = dOmega(:); -% -% end \ No newline at end of file +end \ No newline at end of file diff --git a/ProgramFiles/Physics/Nonholonomic/Nonholonomic_connection_discrete.m b/ProgramFiles/Physics/Nonholonomic/Nonholonomic_connection_discrete.m index 4da6ed1a..2630b7db 100644 --- a/ProgramFiles/Physics/Nonholonomic/Nonholonomic_connection_discrete.m +++ b/ProgramFiles/Physics/Nonholonomic/Nonholonomic_connection_discrete.m @@ -81,7 +81,11 @@ %%%% % First, get the positions of the links in the chain and their % Jacobians with respect to the system parameters - [h, J, J_full] = N_link_chain(geometry,jointangles); + if isfield(geometry,'subchains') + [h, J, J_full] = branched_chain(geometry,jointangles); + else + [h, J, J_full] = N_link_chain(geometry,jointangles); + end %%%% diff --git a/ProgramFiles/Test/Process_robot_test_datafile.m b/ProgramFiles/Test/Process_robot_test_datafile.m new file mode 100644 index 00000000..cfbc77b0 --- /dev/null +++ b/ProgramFiles/Test/Process_robot_test_datafile.m @@ -0,0 +1,22 @@ +function Process_robot_test_datafile + +load SerpenoidGranularLocomotion2016 + +FAx1 = scatteredInterpolant(data_log(:,1),data_log(:,2),data_log(:,3)); +FAy1 = scatteredInterpolant(data_log(:,1),data_log(:,2),data_log(:,4)); +FAtheta1 = scatteredInterpolant(data_log(:,1),data_log(:,2),data_log(:,5)); +FAx2 = scatteredInterpolant(data_log(:,1),data_log(:,2),data_log(:,6)); +FAy2 = scatteredInterpolant(data_log(:,1),data_log(:,2),data_log(:,7)); +FAtheta2 = scatteredInterpolant(data_log(:,1),data_log(:,2),data_log(:,8)); + +[alpha1,alpha2] = ndgrid(linspace(-3.1,3.1,51)); + +Ax1 = FAx1(alpha1,alpha2); +Ax2 = FAx2(alpha1,alpha2); +Ay1 = FAy1(alpha1,alpha2); +Ay2 = FAy2(alpha1,alpha2); +Atheta1 = FAtheta1(alpha1,alpha2); +Atheta2 = FAtheta2(alpha1,alpha2); + + +save('SerpenoidGranularConnection2016','alpha1','alpha2','Ax1','Ax2','Ay1','Ay2','Atheta1','Atheta2') \ No newline at end of file diff --git a/ProgramFiles/sys_calcpath_fcns/deltritest.m b/ProgramFiles/Test/deltritest.m similarity index 100% rename from ProgramFiles/sys_calcpath_fcns/deltritest.m rename to ProgramFiles/Test/deltritest.m diff --git a/ProgramFiles/Utilities/fseriesdemo.m b/ProgramFiles/Test/fseriesdemo.m similarity index 100% rename from ProgramFiles/Utilities/fseriesdemo.m rename to ProgramFiles/Test/fseriesdemo.m diff --git a/ProgramFiles/Test/inertia_unit_test.m b/ProgramFiles/Test/inertia_unit_test.m new file mode 100644 index 00000000..a649a904 --- /dev/null +++ b/ProgramFiles/Test/inertia_unit_test.m @@ -0,0 +1,34 @@ +addpath('Physics/Inertial/Discrete_links'); +addpath('Physics/Inertial'); +addpath('Utilities/Kinematics'); +syms a1 a2; + +link_shape_params = struct('aspect_ratio',0.125); +geometry.link_shape_parameters = repmat({link_shape_params},3,1); +link_shape = struct('a',0,'b',0); +geometry.link_shape = repmat({link_shape},3,1); +geometry.type = 'n-link chain'; +geometry.linklengths = [1,1,1]; +geometry.function = ''; +physics = struct('drag_ratio','','drag_coefficient','','fluid_density',1); +shapeparams = [a1,a2]; +[A, h, J,J_full,Omega] = Inertial_local_connection(geometry,physics,shapeparams); + +% Get the partial derivative of the Jacobian +dJdq = mobile_jacobian_derivative(J_full); + +% Now iterate over each link, calculating the map from system body and +% shape velocities to forces acting on the body +for idx = 1:numel(J_full) + + [link_inertias{idx},local_inertias{idx}] = Inertia_link(h.pos(idx,:),... % Position of this link relative to the base frame + J_full{idx},... % Jacobian from body velocity of base link and shape velocity to body velocity of this link + h.lengths(idx),... % Length of this link + geometry.link_shape{idx},... % Shape type of this link + geometry.link_shape_parameters{idx},... % Shape parametes for this link + physics.fluid_density); % Fluid density relative to link + +end + +% Get the partial derivative of the Mass matrix +dMdq = partial_mass_matrix(J_full,dJdq,local_inertias,'mobile'); \ No newline at end of file diff --git a/ProgramFiles/Test/purcell_test.m b/ProgramFiles/Test/purcell_test.m new file mode 100644 index 00000000..f5679c23 --- /dev/null +++ b/ProgramFiles/Test/purcell_test.m @@ -0,0 +1,61 @@ +%% optimize_so3 test using sysplotter and purcell swimmer +% requirements: + % sysplotter 'ProgramFiles' directory on matlab path + % config 'sysplotter_config.mat' file at same level as run directory + +%% load sysf +system = 'sysf_three_link_lowRe'; +%system = 'sysf_twin_rotor'; +sys_calcsystem('calculate', system); +load([system '_calc.mat'], 's'); + +%% pull out grid, original LC, optimal LC (use 'eval' coordinates) +grid_true = s.grid.eval; +grid_points = cellfun(@(c) unique(c), grid_true, 'UniformOutput', false); +A_orig_full = s.vecfield.eval.content.Avec; +A_opt_full = s.vecfield.eval.content.Avec_optimized; + +%% get \theta LC fields for both orig, opt (use as X rotation dim) +% augment with zero fields for Y, Z rotation dim +z = {zeros(length(grid_points{1}))}; %zero field as cell +A_orig = [A_orig_full(3,:); z z; z z]; +A_opt_true = [A_opt_full(3,:); z z; z z]; + +%% use optimize_so3.m on A_orig, grid to produce A_opt_test +% optimize +[grid_test, X, Y, Z, A_opt_test] = optimize_so3(grid_points, A_orig); +% get CCF +DA_orig = calc_ccf(grid_points, A_orig, @rot_mat, @rot_vec); +DA_opt = calc_ccf(grid_points, A_opt_test, @rot_mat, @rot_vec); + +%% plot X fields for A_opt (use sysplotter fn for coloring?) +figure(1); clf; +subplot(1,3,1); +quiver(grid_true{1}, grid_true{2}, A_orig{1,1}, A_orig{1,2}, 'Color', [0 0 0]); +axis('square'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_\theta, original coordinates'); +subplot(1,3,2); +quiver(grid_true{1}, grid_true{2}, A_opt_true{1,1}, A_opt_true{1,2}, 'Color', [0 0 0]); +axis('square'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_\theta, true minimum perturbation coordinates'); +subplot(1,3,3); +quiver(grid_test{1}, grid_test{2}, A_opt_test{1,1}, A_opt_test{1,2}, 'Color', [0 0 0]); +axis('square'); +xlim([-3 3]); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_\theta, test minimum perturbation coordinates'); + +%% plot CCF in orig, opt +figure(2);clf; +subplot(1,2,1); +contour(grid_test{1}, grid_test{2}, DA_orig{1}); +axis('square'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('Original CCF'); +subplot(1,2,2); +contour(grid_test{1}, grid_test{2}, DA_opt{1}); +axis('square'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('Optimal CCF'); \ No newline at end of file diff --git a/ProgramFiles/Test/space_test.m b/ProgramFiles/Test/space_test.m new file mode 100644 index 00000000..9d2f5a08 --- /dev/null +++ b/ProgramFiles/Test/space_test.m @@ -0,0 +1,158 @@ +%% optimize_so3 test on a reaction wheel satellite +% create samples in ea. shape dimension +num_samples=31; bound=pi/4; +dim = linspace(-bound,bound,num_samples); +samples = {dim,dim}; +% create local connection +z = zeros(num_samples); +o = ones(num_samples); +% the following are valid positive configurations +A_orig = {z, z; -o, z; z, -o}; +%A_orig = {-o z; z z; z -o}; +%A_orig = {-o z; z -o; z z}; +% optimize +[grid, X, Y, Z, A_opt] = optimize_so3(samples, A_orig); + +%% compare lengths +X_orig = [A_orig{1,1}(:)';A_orig{1,2}(:)']; +Y_orig = [A_orig{2,1}(:)';A_orig{2,2}(:)']; +Z_orig = [A_orig{3,1}(:)';A_orig{3,2}(:)']; +n_orig = norm([mean(vecnorm(X_orig)) mean(vecnorm(Y_orig)) mean(vecnorm(Z_orig))]); +disp(['Original norm-average metric: ', num2str(n_orig)]); + +X_opt = [A_opt{1,1}(:)';A_opt{1,2}(:)']; +Y_opt = [A_opt{2,1}(:)';A_opt{2,2}(:)']; +Z_opt = [A_opt{3,1}(:)';A_opt{3,2}(:)']; +n_opt = norm([mean(vecnorm(X_opt)) mean(vecnorm(Y_opt)) mean(vecnorm(Z_opt))]); +disp(['Optimal norm-average metric: ', num2str(n_opt)]); + +%% test gaits +% create gait +t_vec = linspace(0, 2*pi); +alpha = @(t) 0.5*bound*[cos(t); sin(t)]; +d_alpha = @(t) 0.5*bound*[-sin(t); cos(t)]; +% vector for plotting +alpha_vec = alpha(t_vec); +% integrate over connection +[g_circ_orig, g_orig] = so3_integrator(t_vec, alpha, d_alpha, grid, A_orig); +[g_circ_opt, g_opt] = so3_integrator(t_vec, alpha, d_alpha, grid, A_opt); + +%% constraint curvature +DA_orig = calc_ccf(samples, A_orig, @rot_mat, @rot_vec); +DA_opt = calc_ccf(samples, A_opt, @rot_mat, @rot_vec); + +%% plot connections +figure(1);clf; + +subplot(2,3,1); +hold on; +quiver(grid{1},grid{2},A_orig{1,1},A_orig{1,2}, 'Color', [0 0 0]); +plot(alpha_vec(1,:), alpha_vec(2,:), 'Color', [234 14 30]/255); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_x, original coordinates'); +subplot(2,3,2); +hold on; +quiver(grid{1},grid{2},A_orig{2,1},A_orig{2,2}, 'Color', [0 0 0]); +plot(alpha_vec(1,:), alpha_vec(2,:), 'Color', [234 14 30]/255); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_y, original coordinates'); +subplot(2,3,3); +hold on; +quiver(grid{1},grid{2},A_orig{3,1},A_orig{3,2}, 'Color', [0 0 0]); +plot(alpha_vec(1,:), alpha_vec(2,:), 'Color', [234 14 30]/255); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_z, original coordinates'); + +subplot(2,3,4); +hold on; +quiver(grid{1},grid{2},A_opt{1,1},A_opt{1,2}, 'Color', [0 0 0]); +plot(alpha_vec(1,:), alpha_vec(2,:), 'Color', [234 14 30]/255); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_x, optimal coordinates'); +subplot(2,3,5); +hold on; +quiver(grid{1},grid{2},A_opt{2,1},A_opt{2,2}, 'Color', [0 0 0]); +plot(alpha_vec(1,:), alpha_vec(2,:), 'Color', [234 14 30]/255); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_y, optimal coordinates'); +subplot(2,3,6); +hold on; +quiver(grid{1},grid{2},A_opt{3,1},A_opt{3,2}, 'Color', [0 0 0]); +plot(alpha_vec(1,:), alpha_vec(2,:), 'Color', [234 14 30]/255); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('A_z, optimal coordinates'); + +%% plot log coords +figure(2); clf; + +subplot(2,3,1); +plot(t_vec, g_circ_orig(1,:), 'Color', [234 14 30]/255); +xlabel('Time'); +ylabel('Displacement'); +title('X Diplacement, Original Coordinates'); +subplot(2,3,2); +plot(t_vec, g_circ_orig(2,:), 'Color', [234 14 30]/255); +xlabel('Time'); +ylabel('Displacement'); +title('Y Diplacement, Original Coordinates'); +subplot(2,3,3); +plot(t_vec, g_circ_orig(3,:), 'Color', [234 14 30]/255); +xlabel('Time'); +ylabel('Displacement'); +title('Z Diplacement, Original Coordinates'); + +subplot(2,3,4); +plot(t_vec, g_circ_opt(1,:), 'Color', [234 14 30]/255); +xlabel('Time'); +ylabel('Displacement'); +title('X Diplacement, Optimal Coordinates'); +subplot(2,3,5); +plot(t_vec, g_circ_opt(2,:), 'Color', [234 14 30]/255); +xlabel('Time'); +ylabel('Displacement'); +title('Y Diplacement, Optimal Coordinates'); +subplot(2,3,6); +plot(t_vec, g_circ_opt(3,:), 'Color', [234 14 30]/255); +xlabel('Time'); +ylabel('Displacement'); +title('Z Diplacement, Optimal Coordinates'); + +%% plot CCF +figure(3); clf; +subplot(2,3,1); +contour(grid{1}, grid{2}, DA_orig{1}); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('X CCF, Original Coordinates'); +subplot(2,3,2); +contour(grid{1}, grid{2}, DA_orig{2}); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('Y CCF, Original Coordinates'); +subplot(2,3,3); +contour(grid{1}, grid{2}, DA_orig{3}); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('Z CCF, Original Coordinates'); + +subplot(2,3,4); +contour(grid{1}, grid{2}, DA_opt{1}); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('X CCF, Optimal Coordinates'); +subplot(2,3,5); +contour(grid{1}, grid{2}, DA_opt{2}); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('Y CCF, Optimal Coordinates'); +subplot(2,3,6); +contour(grid{1}, grid{2}, DA_opt{3}); +axis('equal'); +xlabel('\alpha_1'); ylabel('\alpha_2'); +title('Z CCF, Optimal Coordinates'); diff --git a/ProgramFiles/sys_calcsystem_fcns/test_connection_and_metric.m b/ProgramFiles/Test/test_connection_and_metric.m similarity index 100% rename from ProgramFiles/sys_calcsystem_fcns/test_connection_and_metric.m rename to ProgramFiles/Test/test_connection_and_metric.m diff --git a/ProgramFiles/Utilities/LieGroups.m b/ProgramFiles/Utilities/LieGroups.m new file mode 100644 index 00000000..7422dc00 --- /dev/null +++ b/ProgramFiles/Utilities/LieGroups.m @@ -0,0 +1,45 @@ +% LieGroups.m +% defines supported Lie Groups acting as configuration spaces +% this is a half-measure. groups should be classes; one day, update +% sysplotter to reflect the below capability. + +classdef LieGroups + % supported groups + enumeration + SE2, SO3 + end + % group-specific helpers + methods + % dimensionality + function dim = n_dim(group) + % extend here for other groups + if group == LieGroups.SE2 || group==LieGroups.SO3 + dim = 3; + end + end + % matrix mapping + function fn = mat_fn(group) + if group == LieGroups.SE2 + fn = @vec_to_mat_SE2_lie; + elseif group == LieGroups.SO3 + fn = @vec_to_mat_SO3; + end + end + % vector mapping + function fn = vec_fn(group) + if group == LieGroups.SE2 + fn = @mat_to_vec_SE2_lie; + elseif group == LieGroups.SO3 + fn = @mat_to_vec_SO3; + end + end + % optimizer + function fn = optimizer_fn(group) + if group == LieGroups.SE2 + fn = @optimize_coordinate_choice; + elseif group == LieGroups.SO3 + fn = @optimize_so3_sys; + end + end + end +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/extract_displacement_and_cost.m b/ProgramFiles/Utilities/extract_displacement_and_cost.m index e6adbd82..b1ddf8e9 100644 --- a/ProgramFiles/Utilities/extract_displacement_and_cost.m +++ b/ProgramFiles/Utilities/extract_displacement_and_cost.m @@ -1,4 +1,4 @@ -function [g_end_orig,g_end_opt, cost_end] = extract_displacement_and_cost(datafile) +function [g_end_orig,g_end_opt, cost_end,BVI_orig, cBVI_orig, BVI_opt,cBVI_opt] = extract_displacement_and_cost(datafile) % Extract the displacement and cost data from a sysf_...shchf_....mat file % Load the target file @@ -11,6 +11,10 @@ g_end_orig = zeros(numel(p.G_locus_full),3); g_end_opt = g_end_orig; cost_end = zeros(numel(p.G_locus_full,1)); % If distance metric was not specified, euclidean metric in the parameters was assumed +BVI_orig = g_end_orig; +cBVI_orig = g_end_orig; +BVI_opt = g_end_orig; +cBVI_opt = g_end_orig; % Loop over each shape change for i = 1:numel(p.G_locus_full) @@ -19,4 +23,10 @@ g_end_orig(i,:) = p.G_locus_full{i}.G(end,:); g_end_opt(i,:) = p.G_locus_full{i}.G_opt(end,:); cost_end(i) = p.G_locus_full{i}.S(end); + + BVI_orig(i,:) = p.G_locus_full{i}.bvi(end,:); + BVI_opt(i,:) = p.G_locus_full{i}.bvi_opt(end,:); + cBVI_orig(i,:) = p.cBVI{i}; + cBVI_opt(i,:) = p.cBVI_opt{i}; + end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/four_way_symmetry.m b/ProgramFiles/Utilities/four_way_symmetry.m similarity index 100% rename from ProgramFiles/sys_calcsystem_fcns/four_way_symmetry.m rename to ProgramFiles/Utilities/four_way_symmetry.m diff --git a/ProgramFiles/sys_calcpath_fcns/fourier_eval.m b/ProgramFiles/Utilities/fourier_eval.m similarity index 100% rename from ProgramFiles/sys_calcpath_fcns/fourier_eval.m rename to ProgramFiles/Utilities/fourier_eval.m diff --git a/ProgramFiles/Utilities/generate_exp_deriv.m b/ProgramFiles/Utilities/generate_exp_deriv.m new file mode 100644 index 00000000..c2f167f3 --- /dev/null +++ b/ProgramFiles/Utilities/generate_exp_deriv.m @@ -0,0 +1,20 @@ +% computes the time-derivative of the exponential map +% ouputs: + % f: anonymous function, accepting (g, d/dt g) as vectors +function f = generate_exp_deriv + % define symvars for beta, derivatives + beta_vec = sym('beta', [1 3]); + d_beta_vec = sym('d_beta', [1 3]); + assume(beta_vec, 'real'); + assume(d_beta_vec, 'real'); + % generate matrices + beta_mat = vec_to_mat_SO3(beta_vec); + d_beta_mat = vec_to_mat_SO3(d_beta_vec); + % compute integral + t = sym('t'); + grand = expm(-t*beta_mat) * d_beta_mat * expm(t*beta_mat); + p_exp_mat = real(int(grand, t, 0, 1)); %ignore zero-valued complex part + f = matlabFunction(p_exp_mat,... + 'file', 'exp_deriv',... + 'vars', {beta_vec, d_beta_vec}); +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/grid_to_baseline.m b/ProgramFiles/Utilities/grid_to_baseline.m new file mode 100644 index 00000000..23607348 --- /dev/null +++ b/ProgramFiles/Utilities/grid_to_baseline.m @@ -0,0 +1,11 @@ +function grid_baseline = grid_to_baseline(grid) + + grid_baseline = cell(size(grid)); + callout_template = num2cell(ones(size(grid_baseline))); + for idx = 1:numel(grid_baseline) + callout = callout_template; + callout{idx} = ':'; + grid_baseline{idx} = squeeze(grid{idx}(callout{:})); + end + +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/hypercubemesh/hypercube_element_shape_functions.m b/ProgramFiles/Utilities/hypercubemesh/hypercube_element_shape_functions.m index 7b67981e..e0fa1599 100644 --- a/ProgramFiles/Utilities/hypercubemesh/hypercube_element_shape_functions.m +++ b/ProgramFiles/Utilities/hypercubemesh/hypercube_element_shape_functions.m @@ -1,26 +1,22 @@ +% generate shape functions for an N-dimensional hypercubic element + function [shape_functions, shape_dfunctions] = hypercube_element_shape_functions(N) -% Make the shape functions for an N-dimensional hypercubic element - % Prime the arrays of shape functions and dfunctions + % prime arrays of shape functions and dfunctions shape_functions = cell(2^N,1); shape_dfunctions = cell(2^N,N); - % Set up the node orderings for the shape functions, up to 3d elements - ordering(:,1) = [-1, 1, 1,-1,-1, 1, 1,-1,-1, 1, 1,-1,-1, 1, 1,-1]'; - ordering(:,2) = [-1,-1, 1, 1,-1,-1, 1, 1,-1,-1, 1, 1,-1,-1, 1, 1]'; - ordering(:,3) = [-1,-1,-1,-1, 1, 1, 1, 1,-1,-1,-1,-1, 1, 1, 1, 1]'; - ordering(:,4) = [-1,-1,-1,-1,-1,-1,-1,-1, 1, 1, 1, 1, 1, 1, 1, 1]'; - - % error if the ordering hasn't been specified to high enough dimension - if N>4 - error('This function only specified up to 4 dimensions so far') - end + % set up node orderings for the shape functions + % uses similar binary setup as in hypercube_mesh.m + bin_strs = dec2bin(0:2^N - 1); + bin_mat = zeros(size(bin_strs)); + bin_mat(:) = arrayfun(@str2num, bin_strs(:)); + ordering = fliplr(2*bin_mat-1); - % Turn the ordering into a cell array for processing inside the shape + % convert ordering into a cell array for processing inside the shape % functions ordering = num2cell(ordering); - % construct the shape functions and dfunctions for i = 1:length(shape_functions) diff --git a/ProgramFiles/Utilities/hypercubemesh/hypercube_mesh.m b/ProgramFiles/Utilities/hypercubemesh/hypercube_mesh.m index 687f9fc2..1ceed6db 100644 --- a/ProgramFiles/Utilities/hypercubemesh/hypercube_mesh.m +++ b/ProgramFiles/Utilities/hypercubemesh/hypercube_mesh.m @@ -1,77 +1,46 @@ +% generate a hypercubic mesh over an arbitrarily-dimensional space + +% inputs: + % grid: ndgrid +% outputs: + % nodes: + % list (matrix) of points. each row contains columns of + % [x, y, z, w, ...] + % cubes: + % list (matrix) of points in each cube. each row contains columns + % of [corner1, corner2, ...] points + function [nodes,cubes] = hypercube_mesh(grid) -% Generate a normalized tetrahedral mesh over a hypercubic region with a -% given density + %% generate node information % turn grid into node list nodes = grid_to_columns(grid); - - % Number the nodes + + % node list metadata node_nums = zeros(size(grid{1})); - node_nums(:) = 1:numel(node_nums); - - % 2^N nodes per cube - nodes_per_cube = 2^numel(grid); + node_nums(:) = 1:numel(node_nums); %indexing of nodes + nodes_per_cube = 2^numel(grid); %2^N nodes per cube + l = size(grid{1}); %lengths of dimensions - % number of nodes in each direction - l = size(grid{1}); + %% build list of nodes in each hypercube - % Build the list of nodes in each hypercube - - % Prime cubes array (one fewer cube than node in each direction + % prime cubes array (one fewer cube than node in each direction) cubes = zeros(prod(size(node_nums)-1),nodes_per_cube); - - % number of cubes n_cubes = size(cubes,1); - - % Expand algorithm for higher dimensions - switch (numel(grid)) - - case 2 - - cubes(:,1) = reshape(node_nums(1:l(1)-1,1:l(2)-1),n_cubes,1); - cubes(:,2) = reshape(node_nums(2:l(1),1:l(2)-1),n_cubes,1); - cubes(:,3) = reshape(node_nums(2:l(1),2:l(2)),n_cubes,1); - cubes(:,4) = reshape(node_nums(1:l(1)-1,2:l(2)),n_cubes,1); - - case 3 - - cubes(:,1) = reshape(node_nums(1:l(1)-1,1:l(2)-1,1:l(3)-1),n_cubes,1); - cubes(:,2) = reshape(node_nums(2:l(1),1:l(2)-1,1:l(3)-1),n_cubes,1); - cubes(:,3) = reshape(node_nums(2:l(1),2:l(2),1:l(3)-1),n_cubes,1); - cubes(:,4) = reshape(node_nums(1:l(1)-1,2:l(2),1:l(3)-1),n_cubes,1); - - cubes(:,5) = reshape(node_nums(1:l(1)-1,1:l(2)-1,2:l(3)),n_cubes,1); - cubes(:,6) = reshape(node_nums(2:l(1),1:l(2)-1,2:l(3)),n_cubes,1); - cubes(:,7) = reshape(node_nums(2:l(1),2:l(2),2:l(3)),n_cubes,1); - cubes(:,8) = reshape(node_nums(1:l(1)-1,2:l(2),2:l(3)),n_cubes,1); - - case 4 - - cubes(:,1) = reshape(node_nums(1:l(1)-1,1:l(2)-1,1:l(3)-1,1:l(4)-1),n_cubes,1); - cubes(:,2) = reshape(node_nums(2:l(1),1:l(2)-1,1:l(3)-1,1:l(4)-1),n_cubes,1); - cubes(:,3) = reshape(node_nums(2:l(1),2:l(2),1:l(3)-1,1:l(4)-1),n_cubes,1); - cubes(:,4) = reshape(node_nums(1:l(1)-1,2:l(2),1:l(3)-1,1:l(4)-1),n_cubes,1); - - cubes(:,5) = reshape(node_nums(1:l(1)-1,1:l(2)-1,2:l(3),1:l(4)-1),n_cubes,1); - cubes(:,6) = reshape(node_nums(2:l(1),1:l(2)-1,2:l(3),1:l(4)-1),n_cubes,1); - cubes(:,7) = reshape(node_nums(2:l(1),2:l(2),2:l(3),1:l(4)-1),n_cubes,1); - cubes(:,8) = reshape(node_nums(1:l(1)-1,2:l(2),2:l(3),1:l(4)-1),n_cubes,1); - - cubes(:,9) = reshape(node_nums(1:l(1)-1,1:l(2)-1,1:l(3)-1,2:l(4)),n_cubes,1); - cubes(:,10) = reshape(node_nums(2:l(1),1:l(2)-1,1:l(3)-1,2:l(4)),n_cubes,1); - cubes(:,11) = reshape(node_nums(2:l(1),2:l(2),1:l(3)-1,2:l(4)),n_cubes,1); - cubes(:,12) = reshape(node_nums(1:l(1)-1,2:l(2),1:l(3)-1,2:l(4)),n_cubes,1); - - cubes(:,13) = reshape(node_nums(1:l(1)-1,1:l(2)-1,2:l(3),2:l(4)),n_cubes,1); - cubes(:,14) = reshape(node_nums(2:l(1),1:l(2)-1,2:l(3),2:l(4)),n_cubes,1); - cubes(:,15) = reshape(node_nums(2:l(1),2:l(2),2:l(3),2:l(4)),n_cubes,1); - cubes(:,16) = reshape(node_nums(1:l(1)-1,2:l(2),2:l(3),2:l(4)),n_cubes,1); - - otherwise - - error('hypercube_mesh not yet implemented for N>4 dimensions'); - - end - - + + % get possible ways to vary start/end points (done with binary) + bin_strs = dec2bin(0:nodes_per_cube - 1); + bin_mat = zeros(size(bin_strs)); + bin_mat(:) = arrayfun(@str2num, bin_strs(:)); + % use binary values to assign cube vertices + for vertex = 1:nodes_per_cube + % get nodes (in each dimension) + idxs = cell(size(l)); + for dim = 1:length(l) + offset = bin_mat(vertex, dim); + idxs{dim} = 1 + offset : l(dim) - 1 + offset; + end + % get the id of each node stored by matlab (rather than coordinate) + cubes(:, vertex) = reshape(node_nums(idxs{:}), n_cubes, 1); + end end \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/fixed_jacobian_derivative.m b/ProgramFiles/Utilities/kinematics/fixed_jacobian_derivative.m new file mode 100644 index 00000000..8c0a8c70 --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/fixed_jacobian_derivative.m @@ -0,0 +1,32 @@ +function dJdq = fixed_jacobian_derivative(J) +% Determine how many joint angles are in the system; the first three terms +% in the Jacobian are for the body coordinates, with any additional columns +% corresponding to the joint space of the system +n_joints = size(J{1},2); +m_links = length(J); + +dJdq = cell(m_links,1); +template = cell(n_joints,n_joints); +zero_vec = zeros(3,1); +% If we're working with symbolic variables, then we need to explicitly make +% the array symbolic, because matlab tries to cast items being inserted +% into an array into the array class, rather than converting the array to +% accomodate the class of the items being inserted +if isa(J{1},'sym') + zero_vec = sym(zero_vec); +end +template(1:end) = {zero_vec}; +dJdq(1:end) = {template}; + + +% Calculate the Jacobian for each link +for link = 1:m_links + % Calculating dJalpha/dalpha + for idx = 1:n_joints + for idx2 = idx+1:n_joints + dJdq{link}{idx,idx2} = lie_bracket_SE2(J{link}(:,idx),J{link}(:,idx2)); + end + end +end + +end % jacobian_derivative \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/fixed_jacobian_second_derivative.m b/ProgramFiles/Utilities/kinematics/fixed_jacobian_second_derivative.m new file mode 100644 index 00000000..45b4ee3b --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/fixed_jacobian_second_derivative.m @@ -0,0 +1,28 @@ +function ddJdqq = fixed_jacobian_second_derivative(J,dJdq) + +n_joints = size(J{1},2); +m_links = length(J); + +ddJdqq = cell(m_links,1); +template = cell(n_joints,n_joints,n_joints); +zero_vec = zeros(3,1); +% If we're working with symbolic variables, then we need to explicitly make +% the array symbolic, because matlab tries to cast items being inserted +% into an array into the array class, rather than converting the array to +% accomodate the class of the items being inserted +if isa(J{1},'sym') + zero_vec = sym(zero_vec); +end +template(1:end) = {zero_vec}; +ddJdqq(1:end) = {template}; + +% +for link = 1:m_links + for idx = 1:n_joints + for idx2 = idx+1:n_joints + for idx3 = idx+1:n_joints + ddJdqq{link}{idx,idx2,idx3} = lie_bracket_SE2(dJdq{link}{idx,idx2},J{link}(:,idx3)); + end + end + end +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/lie_bracket_SE2.m b/ProgramFiles/Utilities/kinematics/lie_bracket_SE2.m new file mode 100644 index 00000000..4861bf11 --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/lie_bracket_SE2.m @@ -0,0 +1,19 @@ +function lie_bracket = lie_bracket_SE2(a,b) +% Calculates the lie bracket of two vectors a, b which are members of the +% set SE2. The vectors a and b should be 3x1 vectors of the form: +% a = [a_x; a_y; a_theta] +% b = [b_x; b_y; b_theta] +% with the subscripts denoting the x, y, and theta coordinates of the +% vectors, respectively. + +% The lie bracket of two vectors finds the net effect of making sequential, +% infinitesimal moves in the a, b, -a, -b directions; it returns a vector +% whose values are obtained by using a small angle approximation of TeLg +% around theta = 0. This is equivalent to the conventional lie bracket +% [X,Y] = XY - YX where X and Y are SE2 matrices with small angle +% approximations used. + +lie_bracket = [b(3)*a(2) - a(3)*b(2); % x-direction term + a(3)*b(1) - b(3)*a(1); % y-direction term + 0]; % theta-direction term +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/mat_to_vec_SE2_lie.m b/ProgramFiles/Utilities/kinematics/mat_to_vec_SE2_lie.m new file mode 100644 index 00000000..1851e214 --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/mat_to_vec_SE2_lie.m @@ -0,0 +1,6 @@ +function vector = mat_to_vec_SE2_lie(matrix) +%Convert SE(2) matrices to a set of row vectors + +vector = [matrix(1,3);matrix(2,3);matrix(2,1)]; + +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/mat_to_vec_SO3.m b/ProgramFiles/Utilities/kinematics/mat_to_vec_SO3.m new file mode 100644 index 00000000..0699cdca --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/mat_to_vec_SO3.m @@ -0,0 +1,3 @@ +function v = rot_vec(m) + v = [m(3,2) m(1,3) m(2,1)]'; +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/mobile_jacobian_derivative.m b/ProgramFiles/Utilities/kinematics/mobile_jacobian_derivative.m new file mode 100644 index 00000000..45e18d4e --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/mobile_jacobian_derivative.m @@ -0,0 +1,73 @@ +function dJdq = mobile_jacobian_derivative(J_full) +% Calculates the partial of the full Jacobian J_full with respect to the +% configuration variables q = [g_circ r]', where g_circ is the system's +% global coordinate configuration and r is the local shape-space +% configuration. +% +% Inputs for a system with m links and q of size (n by 1): +% +% J_full: Full Jacobian matrix for the system, including entries for all +% configuration variables q; (m by 1) cell array where each cell contains +% a (3 by n) matrix representing the Jacobian of link m. +% +% Output: +% +% dJdq: Partial of full Jacobian with respect to the configuration +% variables q; (m by 1) cell array where each entry corresponds to +% the partial derivative of the Jacobian for link m, which is another +% cell array of size (n by n) where each entry (j,k) corresponds to a +% three-element vector that represents the partial of Jacobian column +% j with respect to configuration variable k. + +% Determine how many joint angles are in the system; the first three terms +% in the Jacobian are for the body coordinates, with any additional columns +% corresponding to the joint space of the system +n_joints = size(J_full{1},2) - 3; +m_links = length(J_full); + +% Prototype a template to fit into each level of the output cell array +dJdq = cell(m_links,1); +template = cell(3+n_joints,3+n_joints); +zero_vec = zeros(3,1); +% If we're working with symbolic variables, then we need to explicitly make +% the array symbolic, because matlab tries to cast items being inserted +% into an array into the array class, rather than converting the array to +% accomodate the class of the items being inserted +if isa(J_full{1},'sym') + zero_vec = sym(zero_vec); +end +template(1:end) = {zero_vec}; + +dJdq(1:end) = {template}; + + +for link = 1:m_links +% The Jacobian derivative dJ/dq is of the form +% +% dJ/dq = [dJg/dRg, dJg/dalpha; dJalpha/dRg, dJalpha/dalpha] +% +% where dJg/dRg is a 3x3 augmented matrix of three-element zero vectors, +% dJg/dalpha is a 3xm augmented matrix with each element i,j as the lie +% bracket [Jg_i,Jalpha_j] for each body coordinate i and joint j, +% dJalpha/dRg is a mx3 augmented matrix of three-element zero vectors, and +% dJalpha/dalpha is the lie bracket [Jalpha_i,Jalpha_j] for each joint i +% and j with j > i. + + % No calculation necessary for dJg/dRg and dJalpha/dRg - leave dJdq in + % these areas as zero vectors + + % Calculating dJg/dalpha + for idx = 1:3 + for idx2 = 1:n_joints + dJdq{link}{idx,3+idx2} = lie_bracket_SE2(J_full{link}(:,idx),J_full{link}(:,3+idx2)); + end + end + + % Calculating dJalpha/dalpha + for idx = 1:n_joints + for idx2 = idx+1:n_joints + dJdq{link}{3+idx,3+idx2} = lie_bracket_SE2(J_full{link}(:,3+idx),J_full{link}(:,3+idx2)); + end + end +end +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/mobile_jacobian_second_derivative.m b/ProgramFiles/Utilities/kinematics/mobile_jacobian_second_derivative.m new file mode 100644 index 00000000..58466249 --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/mobile_jacobian_second_derivative.m @@ -0,0 +1,67 @@ +function ddJdqq = mobile_jacobian_second_derivative(J_full,dJdq) +% Determine how many joint angles are in the system; the first three terms +% in the Jacobian are for the body coordinates, with any additional columns +% corresponding to the joint space of the system +n_joints = size(J_full{1},2) - 3; +m_links = length(J_full); + +% We need ddJdqq for every link, and each link's second derivative will be +% in terms of dRg and dalpha. +ddJdqq = cell(m_links,1); +template = cell(3+n_joints,3+n_joints,3+n_joints); +template(1:end) = {zeros(3,1)}; +ddJdqq(1:end) = {template}; +% If we're working with symbolic variables, then we need to explicitly make +% the array symbolic, because matlab tries to cast items being inserted +% into an array into the array class, rather than converting the array to +% accomodate the class of the items being inserted +if isa(J_full{1},'sym') + ddJdqq = sym(ddJdqq); +end + +for link = 1:m_links +% The Jacobian derivative dJ/dq is of the form +% +% dJ/dq = [dJg/dRg, dJg/dalpha; dJalpha/dRg, dJalpha/dalpha] +% +% where dJg/dRg is a 3x3 augmented matrix of three-element zero vectors, +% dJg/dalpha is a 3xm augmented matrix with each element i,j as the lie +% bracket [Jg_i,Jalpha_j] for each body coordinate i and joint j, +% dJalpha/dRg is a mx3 augmented matrix of three-element zero vectors, and +% dJalpha/dalpha is the lie bracket [Jalpha_i,Jalpha_j] for each joint i +% and j with j > i. +% The second derivative of the Jacobian is simply dJ/dq with additional +% partial derivatives d/dRg and d/dalpha applied. For notation purposes, +% these partials are kept as separate sheets in the third dimension of +% ddJdqq. + + % No calculation is necessary for the second partial for the d/dRg + % sheets, as the partials of these terms is always zero. Leave these + % (the first three sheets) as the zero vectors from the template. + + % The partial terms dJg/dRg and dJalpha/dRg are always zero; thus no + % calculation is necessary for the second partial d/dalpha in the first + % three columns of the fourth sheet and onward. Leave these as zero + % vectors from the template. + + % Calculating ddJg/dalpha^2 + for idx = 1:3 % Each of Jg1, Jg2, Jg3 + for idx2 = 1:n_joints % Each dJg/dalpha + for idx3 = 1:n_joints % Second d/dalpha for each alpha + ddJdqq{link}{idx,3+idx2,3+idx3} = ... + lie_bracket_SE2(dJdq{link}{idx,3+idx2},J_full{link}(:,3+idx3)); + end + end + end + + % Calculating ddJalpha/dalpha^2 + for idx = 1:n_joints % Each of Jalpha1, Jalpha2, ... + for idx2 = idx+1:n_joints % Each dJalpha/dalpha + for idx3 = idx+1:n_joints % Second d/dalpha for each alpha + ddJdqq{link}{3+idx,3+idx2,3+idx3} = ... + lie_bracket_SE2(dJdq{link}{3+idx,3+idx2},J_full{link}(:,3+idx3)); + end + end + end +end +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/kinematics/vec_to_mat_SE2_lie.m b/ProgramFiles/Utilities/kinematics/vec_to_mat_SE2_lie.m new file mode 100644 index 00000000..e80f89d6 --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/vec_to_mat_SE2_lie.m @@ -0,0 +1,9 @@ +function matrix = vec_to_mat_SE2_lie(vector) + +theta = vector(3); +x = vector(1); +y = vector(2); + +matrix = [0,-theta,x;theta,0,y;0,0,0]; + +end diff --git a/ProgramFiles/Utilities/kinematics/vec_to_mat_SO3.m b/ProgramFiles/Utilities/kinematics/vec_to_mat_SO3.m new file mode 100644 index 00000000..4ea59458 --- /dev/null +++ b/ProgramFiles/Utilities/kinematics/vec_to_mat_SO3.m @@ -0,0 +1,3 @@ +function m = rot_mat(v) + m = [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/metricellipsefield.m b/ProgramFiles/Utilities/metricellipsefield.m index 6436c820..aec7ad14 100644 --- a/ProgramFiles/Utilities/metricellipsefield.m +++ b/ProgramFiles/Utilities/metricellipsefield.m @@ -26,7 +26,7 @@ [u,s,v] = cellfun(@(m) svd(inv(m)),M,'UniformOutput',false); % Apply the transform corresponding to M to the circles - circles = cellfun(@(u,s,v,c)(u*sqrt(s)*v')*c,u,s,v,circles,'UniformOutput',false); + circles = cellfun(@(u,s,c)(u*sqrt(s))*c,u,s,circles,'UniformOutput',false); %circles = cellfun(@(u,s,v,c)(sqrt(s)*v')*c,u,s,v,circles,'UniformOutput',false); plot_options = varargin{1}; @@ -35,16 +35,17 @@ case 'tissot-cross' % Calculate the svd of the metric tensor - [u,s,v] = cellfun(@(m) svd(inv(m)),M,'UniformOutput',false); + [u,s,~] = cellfun(@(m) svd(inv(m)),M,'UniformOutput',false); % Apply the transform corresponding to M to the circles - circles = cellfun(@(u,s,v,c)(u*sqrt(s)*v')*c,u,s,v,circles,'UniformOutput',false); + circles = cellfun(@(u,s,c)(u*sqrt(s))*c,u,s,circles,'UniformOutput',false); % Create the crosses crosses = cellfun(@(u,s)... - [u(1,1)*s(1,1) -u(1,1)*s(1,1);...% NaN u(1,2)*s(2,2) u(1,2)*s(2,2);... - u(2,1)*s(1,1) -u(2,1)*s(1,1);... NaN u(2,2)*s(2,2) -u(2,2)*s(2,2) - ],u,s,'UniformOutput',false); + u*[-sqrt(s(1,1)) sqrt(s(1,1)) NaN 0 0;...[u(1,1)*s(1,1) -u(1,1)*s(1,1);...% NaN u(1,2)*s(2,2) u(1,2)*s(2,2);... + 0 0 NaN -sqrt(s(2,2)) sqrt(s(2,2))...u(2,1)*s(1,1) -u(2,1)*s(1,1);... NaN u(2,2)*s(2,2) -u(2,2)*s(2,2) + ]... + ,u,s,'UniformOutput',false); plot_options = varargin{1}; plot_options_crosses = varargin{2}; @@ -83,9 +84,9 @@ % Recenter the circles circles = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w],circles,num2cell(x),num2cell(y),'UniformOutput',false); -if exist('crosses','var') - crosses = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w],crosses,num2cell(x),num2cell(y),'UniformOutput',false); -end +% if exist('crosses','var') +% crosses = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w],crosses,num2cell(x),num2cell(y),'UniformOutput',false); +% end %%%%%%%%%%%%%%%%% % Fill in default values for plot options @@ -130,9 +131,14 @@ case 'tissot-cross' - h_cross = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),plot_options_crosses{:}),crosses,'UniformOutput',false); + %h_cross = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),plot_options_crosses{:}),crosses,'UniformOutput',false); - h_ellipse = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),plot_options{:}),circles,'UniformOutput',false); - + h_cross=cell(size(crosses)); + for i =1:numel(h_cross) + h_cross{i} = line('xdata',crosses{i}(1,:)+x(i),'ydata',crosses{i}(2,:)+y(i),plot_options_crosses{:}); + end + + h_ellipse = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),plot_options{:}),circles,'UniformOutput',false); + end \ No newline at end of file diff --git a/ProgramFiles/Utilities/metricellipsefield_convert.m b/ProgramFiles/Utilities/metricellipsefield_convert.m new file mode 100644 index 00000000..996a6f52 --- /dev/null +++ b/ProgramFiles/Utilities/metricellipsefield_convert.m @@ -0,0 +1,201 @@ +function h = metricellipsefield_convert(x,y,M,style,convert,stretch,varargin) +%Plot an ellipse field based on the singular values of a metric +%tensor M. M should be specified as a cell array with with the same +%dimensions as x and y, and each cell containing a 2x2 matrix containing +%its value at the corresponding x,y location + + +%%%%%%%%%%%%% +% Construct the ellipses for each location + +% Make the circle primitive +th = linspace(0,2*pi,50); +xc = cos(th); +yc = sin(th); + +% Replicate the circle primitive into a cell array at each x,y location +circles = repmat({[xc;yc]},size(x)); + +% Pull out the stretch name +stretchnames = {'stretch','surface'}; +if stretch + stretchname = stretchnames{stretch}; +end + +switch style + + % The tissot indicatrix, showing linear stretch + case 'tissot' + + % Calculate the svd of the metric tensor + [u,s,v] = cellfun(@(m) svd(inv(m)),M,'UniformOutput',false); + + % Apply the transform corresponding to M to the circles + circles = cellfun(@(u,s,c)(u*sqrt(s))*c,u,s,circles,'UniformOutput',false); + %circles = cellfun(@(u,s,v,c)(sqrt(s)*v')*c,u,s,v,circles,'UniformOutput',false); + + plot_options = varargin{1}; + + % the tissot indicatrix, with major and minor axis crosses added + case 'tissot-cross' + + % Calculate the svd of the metric tensor + [u,s,~] = cellfun(@(m) svd(inv(m)),M,'UniformOutput',false); + + % Apply the transform corresponding to M to the circles + circles = cellfun(@(u,s,c)(u*sqrt(s))*c,u,s,circles,'UniformOutput',false); + + % Create the crosses + crosses = cellfun(@(u,s)... + u*[-sqrt(s(1,1)) sqrt(s(1,1)) NaN 0 0;...[u(1,1)*s(1,1) -u(1,1)*s(1,1);...% NaN u(1,2)*s(2,2) u(1,2)*s(2,2);... + 0 0 NaN -sqrt(s(2,2)) sqrt(s(2,2))...u(2,1)*s(1,1) -u(2,1)*s(1,1);... NaN u(2,2)*s(2,2) -u(2,2)*s(2,2) + ]... + ,u,s,'UniformOutput',false); + + plot_options = varargin{1}; + plot_options_crosses = varargin{2}; + + + +end + + + +% Find the greatest x or y range in the circles +xrange = cellfun(@(u)range(u(1,:)),circles); +yrange = cellfun(@(u)range(u(2,:)),circles); + +xrange = max(xrange(:)); +yrange = max(yrange(:)); + +% Find the smallest spacing in each direction +xspacing = min(diff(x(:,1))); +yspacing = min(diff(y(1,:))); + +% Set the percentage of the spacing that the largest element should occupy +max_fill = 0.6; + +% Determine if x or y fitting is the limiting factor for the plot and set +% the scaling accordingly +scale_factor = min(xspacing/xrange, yspacing/yrange)*max_fill; + +% Multiply all the circles by the scale factor +circles = cellfun(@(u)u*scale_factor,circles,'UniformOutput',false); + +if exist('crosses','var') + crosses = cellfun(@(u)u*scale_factor,crosses,'UniformOutput',false); +end + + +% Recenter the circles and stretch them if there is a conversion transform +if stretch + +% switch stretch +% +% case 1 %metric stretch +% % Multiply all the circles by their respective Jacobians +% J = arrayfun(@(u,v) convert.(stretchname).jacobian(u,v),x,y,'UniformOutput',false); +% circles = cellfun(@(u,v)u*v,J,circles,'UniformOutput',false); +% if exist('crosses','var') +% crosses = cellfun(@(u,v)u*v,J,crosses,'UniformOutput',false); +% end +% +% [x_f, y_f] = convert.(stretchname).old_to_new_points(x,y); +% +% circles = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w],circles,num2cell(x_f),num2cell(y_f),'UniformOutput',false); +% if exist('crosses','var') +% crosses = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w],crosses,num2cell(x_f),num2cell(y_f),'UniformOutput',false); +% end +% +% case 2 %metric surface + % Multiply all the circles by their respective Jacobians + J = arrayfun(@(u,v) convert.(stretchname).jacobian(u,v),x,y,'UniformOutput',false); + + % Boost Jacobian dimension if needed + if stretch == 1 + J = cellfun(@(u)[u;0 0],J,'UniformOutput',false); + end + + circles = cellfun(@(u,v)u*v,J,circles,'UniformOutput',false); + if exist('crosses','var') + crosses = cellfun(@(u,v)u*v,J,crosses,'UniformOutput',false); + end + + [x_f, y_f, z_f] = convert.(stretchname).old_to_new_points(x,y); + + circles = cellfun(@(u,v,w,t) [u(1,:)+v;u(2,:)+w;u(3,:)+t],circles,num2cell(x_f),num2cell(y_f),num2cell(z_f),'UniformOutput',false); + if exist('crosses','var') + crosses = cellfun(@(u,v,w,t) [u(1,:)+v;u(2,:)+w;u(3,:)+t],crosses,num2cell(x_f),num2cell(y_f),num2cell(z_f),'UniformOutput',false); + end + + +% end + +else + + circles = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w;zeros(size(u(1,:)))],circles,num2cell(x),num2cell(y),'UniformOutput',false); + if exist('crosses','var') + crosses = cellfun(@(u,v,w) [u(1,:)+v;u(2,:)+w;zeros(size(u(1,:)))],crosses,num2cell(x),num2cell(y),'UniformOutput',false); + end + +end + + + + + + +%%%%%%%%%%%%%%%%% +% Fill in default values for plot options + +% Ensure plot options exists +if ~exist('plot_options','var') + + plot_options = {}; + +end + + +% Color +if ~any(strcmpi('edgecolor',plot_options(1:2:end))) + + plot_options = [plot_options,{'EdgeColor','k'}]; + +end +if ~any(strcmpi('facecolor',plot_options(1:2:end))) + + plot_options = [plot_options,{'FaceColor','none'}]; + +end + +% Parent +if isempty(plot_options) || ~any(strmatch('parent',lower(plot_options(1:2:end)))) + + f = figure; + ax = axes('Parent',f); + plot_options = [plot_options,{'Parent',ax}]; + +end + +%%%%%%%%%%%%%%%%%% +% Make the ellipses + +switch style + + case 'tissot' + + h = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),'ZData',u(3,:),plot_options{:}),circles,'UniformOutput',false); + + case 'tissot-cross' + + %h_cross = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),plot_options_crosses{:}),crosses,'UniformOutput',false); + + + h_cross=cell(size(crosses)); + for i =1:numel(h_cross) + h_cross{i} = line('xdata',crosses{i}(1,:),'ydata',crosses{i}(2,:),'zdata',crosses{i}(3,:),plot_options_crosses{:}); + end + + h_ellipse = cellfun(@(u)patch('XData',u(1,:),'YData',u(2,:),'ZData',u(3,:),plot_options{:}),circles,'UniformOutput',false); + +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/ndgradient.m b/ProgramFiles/Utilities/ndgradient.m new file mode 100644 index 00000000..b52130ed --- /dev/null +++ b/ProgramFiles/Utilities/ndgradient.m @@ -0,0 +1,12 @@ +function varargout = ndgradient(varargin) +% Wrapper on gradient to work with ndgrid instead of meshgrid + +% For now we assume that there is a grid being provided. If making this +% handle all of gradient's edge cases, will need to generalize +varargout = cell(1,numel(varargin)-1); + +grad_permute = [2,1,3:numel(varargout)]; + +[varargout{grad_permute}] = gradient(varargin{1},varargin{grad_permute+1}); + +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/se2_local_lie_bracket.m b/ProgramFiles/Utilities/se2_local_lie_bracket.m similarity index 100% rename from ProgramFiles/sys_calcsystem_fcns/se2_local_lie_bracket.m rename to ProgramFiles/Utilities/se2_local_lie_bracket.m diff --git a/ProgramFiles/Utilities/validate_dMalpha_dalpha_calc.m b/ProgramFiles/Utilities/validate_dMalpha_dalpha_calc.m new file mode 100644 index 00000000..d77c0d48 --- /dev/null +++ b/ProgramFiles/Utilities/validate_dMalpha_dalpha_calc.m @@ -0,0 +1,59 @@ +function valid = validate_dMalpha_dalpha_calc() +% Define jacobian using values from Ross's book for rotary-rotary arm +syms l1 l2 a1 a2 m1 m2 I1 I2 da1 da2 real; +J1 = [0 0; l1/2 0; 1 0]; +J2 = [l1*sin(a2) 0; l2/2+l1*cos(a2) l2/2; 1 1]; +J = {J1, J2}; + +% Derivative of J with respect to the shape variables +dJ1da1 = sym([0 0; 0 0; 0 0]); +dJ1da2 = sym([0 0; 0 0; 0 0]); +dJ2da1 = sym([0 0; 0 0; 0 0]); +dJ2da2 = sym([l1*cos(a2) 0; -l1*sin(a2) 0; 0 0]); +% Check the partials against the jacobian derivative calculator +dJdq = fixed_jacobian_derivative(J); +test1 = isequaln(dJ1da1,cell2sym(dJdq{1}(:,1)')); +test2 = isequaln(dJ1da2,cell2sym(dJdq{1}(:,2)')); +test3 = isequaln(dJ2da1,cell2sym(dJdq{2}(:,1)')); +test4 = isequaln(dJ2da2,cell2sym(dJdq{2}(:,2)')); +if ~(test1 && test2 && test3 && test4) + error('Mismatch in Jacobian derivative calculation') +end + +% Local inertia tensors +u1 = diag([m1 m1 I1]); +u2 = diag([m2 m2 I2]); + +% % Check the mass matrix - debugging only +% M = J1'*u1*J1 + J2'*u2*J2; +% simplify(M) + +% Find what the partial_mass_matrix function says are the partial mass +% terms +dMda = partial_mass_matrix(J,dJdq,{u1, u2},[]); +% Check these against the book's asserted values +dMda1 = sym([0 0; 0 0]); +dMda2 = -m2*l1*l2*sin(a2)/2*[2 1; 1 0]; +test1 = isequaln(dMda{1},dMda1); +test2 = isequaln(dMda{2},dMda2); +if ~(test1 && test2) +% error('Mismatch in dMdalpha calculation') + valid = false; +else + valid = true; +end + +shape = [a1, a2]; +dshape = [da1, da2]; +C = calc_coriolis_matrix(dMda,shape,dshape); +% Check to see if the output of calc_coriolis_matrix is the same as the +% book's +Cbook = -m2*l1*l2*sin(a2)/2*[2*da1*da2+da2^2;da1*da2] + ... + m2*l1*l2*sin(a2)/2*[0; da1^2 + da1*da2]; +Cbook = simplify(Cbook); +if ~isequaln(C,Cbook) + valid = false; +else + valid = true; +end +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/verify_mass_derivative.m b/ProgramFiles/Utilities/verify_mass_derivative.m new file mode 100644 index 00000000..c94041cb --- /dev/null +++ b/ProgramFiles/Utilities/verify_mass_derivative.m @@ -0,0 +1,44 @@ +function valid = verify_mass_derivative(s,err_tol) + % Set a default error tolerance if one wasn't provided + if ~exist('err_tol','var') || isempty(err_tol) + err_tol = 1e-6; + end + % Pick a random shape in the range of valid shapes + b = s.grid_range(2); + a = s.grid_range(1); + shape = (b-a).*rand(1,2) + a; + shapelist = num2cell(shape); + + % Evaluate M at the random shape + M = cellfun(@(C) interpn(s.grid.mass_eval{:},C,... + shapelist{:},'spline'),s.massfield.mass_eval.content.M_alpha); + + % Evaluate M at a small delta_alpha in both alpha_1 and alpha_2 + % directions to verify if dM_alphadalpha is accurate + dalpha = 0.001; + shapelist_a1 = shapelist; + shapelist_a1{1} = shapelist_a1{1} + dalpha; + M_a1 = cellfun(@(C) interpn(s.grid.mass_eval{:},C,... + shapelist_a1{:},'spline'),s.massfield.mass_eval.content.M_alpha); + shapelist_a2 = shapelist; + shapelist_a2{2} = shapelist_a2{2} + dalpha; + M_a2 = cellfun(@(C) interpn(s.grid.mass_eval{:},C,... + shapelist_a2{:},'spline'),s.massfield.mass_eval.content.M_alpha); + + % Get what dM_alphadalpha says from analytical precalculation + dM_alphadalpha = cell(size(shapelist)); + for i = 1:length(shapelist) + dM_alphadalpha{i} = cellfun(@(C) interpn(s.grid.coriolis_eval{:},C,... + shapelist{:},'spline'),s.coriolisfield.coriolis_eval.content.dM_alphadalpha{i}); + end + + % Check the difference between dM_alphadalpha and the numerical deltas + err_a1 = (M_a1-M)./dalpha - dM_alphadalpha{1}; + err_a2 = (M_a2-M)./dalpha - dM_alphadalpha{2}; + + if any(any(abs(err_a1) > err_tol)) || any(any(abs(err_a2) > err_tol)) + valid = false; + else + valid = true; + end +end \ No newline at end of file diff --git a/ProgramFiles/Utilities/verify_shape_equations.m b/ProgramFiles/Utilities/verify_shape_equations.m new file mode 100644 index 00000000..2daaf990 --- /dev/null +++ b/ProgramFiles/Utilities/verify_shape_equations.m @@ -0,0 +1,30 @@ +function valid = verify_shape_equations(p,err_tol) +% Verifies if the definition of the shape equations phi_def, dphi_def, and +% ddphi_def are valid against each other by checking the numerical +% derivatives of each against the analytic derivative + +% Set a default error tolerance if one isn't provided +if ~exist('err_tol','var') || err_tol == [] + err_tol = 1e-3; +end + +% Set the time delta to be used to check the derivatives +dt = 0.001; +% Test ten different values +for i = 1:10 + % Pick a random time value in the interval (0,1) + t = rand(1); + % Take the shape value at the time t and t+dt + shape = [p.phi_def(t-dt); p.phi_def(t+dt)]; + dshape = [p.dphi_def(t-dt); p.dphi_def(t); p.dphi_def(t+dt)]; + ddshape = p.ddphi_def(t); + + err_dshape = dshape(2,:) - (shape(2,:)-shape(1,:))/(2*dt) + err_ddshape = ddshape - (dshape(3,:)-dshape(1,:))/(2*dt) + + if any(abs(err_dshape) > err_tol) || any(abs(err_ddshape) > err_tol) + valid = false; + return; + end +end +valid = true; \ No newline at end of file diff --git a/ProgramFiles/sys_calcpath_fcns/visibility.m b/ProgramFiles/Utilities/visibility.m similarity index 100% rename from ProgramFiles/sys_calcpath_fcns/visibility.m rename to ProgramFiles/Utilities/visibility.m diff --git a/ProgramFiles/sys_calcpath.m b/ProgramFiles/sys_calcpath.m index 63c0dfc1..20be892a 100644 --- a/ProgramFiles/sys_calcpath.m +++ b/ProgramFiles/sys_calcpath.m @@ -51,7 +51,7 @@ p = find_loci(s,p); %Get the resulting fiber motion - p = find_g(s,p); %#ok + p = find_g_sys(s,p); %#ok diff --git a/ProgramFiles/sys_calcpath_fcns/find_g_functional.m b/ProgramFiles/sys_calcpath_fcns/find_g_functional.m deleted file mode 100644 index 772abf63..00000000 --- a/ProgramFiles/sys_calcpath_fcns/find_g_functional.m +++ /dev/null @@ -1,228 +0,0 @@ -function p = find_g(s,p) -% get the position change and related data from integrating the shape -% change in p over the system in s - - % Set the initial values of the integrals - n_dim = size(s.height,1); - - % Iterate over the paths - n_paths = length(p.phi_fun); - for i = 1:n_paths - - intstart = struct('bvi', {zeros(n_dim,1)},... - 'bvi_opt', {zeros(n_dim,1)},... - 'G', {zeros(n_dim,1)},... - 'G_opt', {zeros(n_dim,1)},... - 'pathlength', {0}); - - % Iterate over the segments in that path - n_segments = length(p.phi_fun{i}); - for j = 1:n_segments - - %%%% - % Integrate the body velocity integral in the original - % coordinates - - % Solution for all the displacements - sol = ode45(@(t,y) se2_integrator_all_terms... - (t,y,s,p.phi_fun{i}{j},p.dphi_fun{i}{j}) ... - ,p.time{i}{j}([1 end]) ... - ,zeros(12,1)); - - - % Extract the BVI and displacement functions relative to the start of the segment - p.bvi_fun_local{i}{j} = @(t) extract_eval(sol,t,1:3); - p.bvi_opt_fun_local{i}{j} = @(t) extract_eval(sol,t,4:6); - p.G_fun_local{i}{j} = @(t) extract_eval(sol,t,7:9); - p.G_opt_fun_local{i}{j} = @(t) extract_eval(sol,t,10:12); - - % Ditto for the functions relative to the start of the shape - % change - - % bvi original - p.bvi_fun{i}{j} = @(t) (repmat(intstart.bvi,size(t(:)')) + p.bvi_fun_local{i}{j}(t)')'; - intstart.bvi = p.bvi_fun{i}{j}(p.time{i}{j}(end))'; - - % bvi optimized - p.bvi_opt_fun{i}{j} = @(t) (repmat(intstart.bvi_opt,size(t(:)'))... - + p.bvi_opt_fun_local{i}{j}(t)')'; - intstart.bvi_opt = p.bvi_opt_fun{i}{j}(p.time{i}{j}(end))'; - - % displacement original - x = intstart.G(1); - y = intstart.G(2); - theta = intstart.G(3); - p.G_fun{i}{j} = @(t) (repmat([x; y; theta],size(t(:)')) + ... - [cos(theta) -sin(theta) 0; - sin(theta) cos(theta) 0; - 0 0 1] * p.G_fun_local{i}{j}(t)')'; - intstart.G = p.G_fun{i}{j}(p.time{i}{j}(end))'; - - % displacement optimized - x = intstart.G_opt(1); - y = intstart.G_opt(2); - theta = intstart.G_opt(3); - p.G_opt_fun{i}{j} = @(t) (repmat([x; y; theta],size(t(:)')) + ... - [cos(theta) -sin(theta) 0; - sin(theta) cos(theta) 0; - 0 0 1] * p.G_opt_fun_local{i}{j}(t)')'; - - % set the initial value for the next segment - intstart.G_opt = p.G_opt_fun{i}{j}(p.time{i}{j}(end))'; - - - -% sol = ode45(@(t,y) se2_integrator... -% (t,y,s,p.phi_fun{i}{j},p.dphi_fun{i}{j},'bvi','original') ... -% ,p.time{i}{j}([1 end]) ... -% ,zeros(size(intstart.bvi))); -% -% % Extract the BVI function relative to the start of the segment -% p.bvi_fun_local{i}{j} = @(t) deval(sol,t)'; -% -% % Extract the BVI function relative to the start of the shape -% % change -% p.bvi_fun{i}{j} = @(t) (repmat(intstart.bvi,size(t(:)')) + p.bvi_fun_local{i}{j}(t)')'; -% -% % set the initial value for the next segment -% intstart.bvi = p.bvi_fun{i}{j}(p.time{i}{j}(end))'; -% -% %%%% -% % Integrate the body velocity integral in the optimized -% % coordinates -% sol = ode45(@(t,y) se2_integrator... -% (t,y,s,p.phi_fun{i}{j},p.dphi_fun{i}{j},'bvi','optimized') ... -% ,p.time{i}{j}([1 end]) ... -% ,zeros(size(intstart.bvi_opt))); -% -% % Extract the BVI function relative to the start of the segment -% p.bvi_opt_fun_local{i}{j} = @(t) deval(sol,t)'; -% -% % Extract the BVI function relative to the start of the shape -% % change -% p.bvi_opt_fun{i}{j} = @(t) (repmat(intstart.bvi_opt,size(t(:)'))... -% + p.bvi_opt_fun_local{i}{j}(t)')'; -% -% % set the initial value for the next segment -% intstart.bvi_opt = p.bvi_opt_fun{i}{j}(p.time{i}{j}(end))'; -% -% %%%% -% % Integrate the position trajectory in the original -% % coordinates -% sol = ode45(@(t,y) se2_integrator... -% (t,y,s,p.phi_fun{i}{j},p.dphi_fun{i}{j},'disp','original') ... -% ,p.time{i}{j}([1 end]) ... -% ,zeros(size(intstart.G))); -% -% % Extract the position trajectory wrt the start of this segment -% p.G_fun_local{i}{j} = @(t) deval(sol,t)'; -% -% % Extract the position trajectory wrt the start of the shape -% % change -% x = intstart.G(1); -% y = intstart.G(2); -% theta = intstart.G(3); -% p.G_fun{i}{j} = @(t) (repmat([x; y; theta],size(t(:)')) + ... -% [cos(theta) -sin(theta) 0; -% sin(theta) cos(theta) 0; -% 0 0 1] * p.G_fun_local{i}{j}(t)')'; -% -% % set the initial value for the next segment -% intstart.G = p.G_fun{i}{j}(p.time{i}{j}(end))'; -% -% %%%% -% % Integrate the position trajectory in the optimal coordinates -% sol = ode45(@(t,y) se2_integrator... -% (t,y,s,p.phi_fun{i}{j},p.dphi_fun{i}{j},'disp','optimized') ... -% ,p.time{i}{j}([1 end]) ... -% ,zeros(size(intstart.G_opt))); -% -% % Extract the position trajectory wrt the start of this segment -% p.G_opt_fun_local{i}{j} = @(t) deval(sol,t)'; -% -% % Extract the position trajectory wrt the start of the shape -% % change -% x = intstart.G_opt(1); -% y = intstart.G_opt(2); -% theta = intstart.G_opt(3); -% p.G_opt_fun{i}{j} = @(t) (repmat([x; y; theta],size(t(:)')) + ... -% [cos(theta) -sin(theta) 0; -% sin(theta) cos(theta) 0; -% 0 0 1] * p.G_opt_fun_local{i}{j}(t)')'; -% -% % set the initial value for the next segment -% intstart.G_opt = p.G_opt_fun{i}{j}(p.time{i}{j}(end))'; - - %%%% - % Integrate the pathlength according to the system distance metric - sol = ode45(@(t,y) pathlength_integrator ... - (t,y,s.metric,p.phi_fun{i}{j},p.dphi_fun{i}{j}) ... - ,p.time{i}{j}([1 end]) ... - ,zeros(size(intstart.pathlength))); - - % Extract the cost of this segment - p.S_fun_local{i}{j} = @(t) deval(sol,t)'; - - % Extract the cost with respect to the start of the shape - % change - p.S_fun{i}{j} = @(t) (intstart.pathlength + p.S_fun_local{i}{j}(t)')'; - - - % set the initial value for the next segment - intstart.pathlength = p.S_fun{i}{j}(p.time{i}{j}(end))'; - - %%%%%%%%%%%%%%%%%% - % Evaluate the position and cost loci - p.G_locus{i}{j}.bvi = p.bvi_fun{i}{j}(p.time{i}{j}); - p.G_locus{i}{j}.bvi_opt = p.bvi_opt_fun{i}{j}(p.time{i}{j}); - p.G_locus{i}{j}.G = p.G_fun{i}{j}(p.time{i}{j}); - p.G_locus{i}{j}.G_opt = p.G_opt_fun{i}{j}(p.time{i}{j}); - p.G_locus{i}{j}.S = p.S_fun{i}{j}(p.time{i}{j}); - - - - end - - %%%%%%% - % After all segments have been computed, concatenate the segments - - % Get the time limits - start_times = cellfun(@(t) min(t),p.time{i}); - end_times = cellfun(@(t) max(t),p.time{i}); - all_limits = [start_times(:) end_times(:)]; - - p.bvi_fun_full{i} = @(t) concatenate_functions(p.bvi_fun{i},all_limits,t); - p.bvi_opt_fun_full{i} = @(t) concatenate_functions(p.bvi_opt_fun{i},all_limits,t); - - p.G_fun_full{i} = @(t) concatenate_functions(p.G_fun{i},all_limits,t); - p.G_opt_fun_full{i} = @(t) concatenate_functions(p.G_opt_fun{i},all_limits,t); - - p.S_fun_full{i} = @(t) concatenate_functions(p.S_fun{i},all_limits,t); - - %%%%%%%%%%%%%%%%%% - % Evaluate the position and cost loci for the shape change - p.G_locus_full{i}.bvi = p.bvi_fun_full{i}(p.time_full{i}); - p.G_locus_full{i}.bvi_opt = p.bvi_opt_fun_full{i}(p.time_full{i}); - p.G_locus_full{i}.G = p.G_fun_full{i}(p.time_full{i}); - p.G_locus_full{i}.G_opt = p.G_opt_fun_full{i}(p.time_full{i}); - p.G_locus_full{i}.S = p.S_fun_full{i}(p.time_full{i}); - - - - - - end - - % Get the cBVI for the system - if ~strcmp(p.cBVI_method{i}{1},'none') - [p.cBVI p.cBVI_opt] = integrate_cBVI(s,p); - end - -end - -function v = extract_eval(sol,t,vals) - - u = deval(sol,t)'; - v = u(:,vals); - -end \ No newline at end of file diff --git a/ProgramFiles/sys_calcpath_fcns/find_g_sys.m b/ProgramFiles/sys_calcpath_fcns/find_g_sys.m new file mode 100644 index 00000000..c20af7e8 --- /dev/null +++ b/ProgramFiles/sys_calcpath_fcns/find_g_sys.m @@ -0,0 +1,65 @@ +% find_g_sys.m +% sysplotter wrapper for integrated position, respecting config. space +% note: SO(3) positions are in matrix form, pending choice of coordinate + +function p = find_g_sys(s,p) + % switch between integrators based on group + if s.conf_space == LieGroups.SE2 + % use working legacy setup for now + % this ought to be consolidated during rewrite + p = find_g(s,p); + elseif s.conf_space == LieGroups.SO3 + % redundant so3 wrapper (below) for simplicity + p = find_g_so3(s,p); + end +end + +% so3 integration wrapper (produces same outputs as find_g) +function p = find_g_so3(s,p) + % respect multiple paths + for i = 1:length(p.phi_fun) + % integrate segments separately + for j = 1:length(p.phi_fun{i}) + % get local g with so3 fixed-step integrator + [~, g_all] = so3_integrator(p.time{i}{j},... + p.phi_fun{i}{j},... + p.dphi_fun{i}{j},... + s.grid.eval,... + s.vecfield.eval.content.Avec); + [~, g_all_opt] = so3_integrator(p.time{i}{j},... + p.phi_fun{i}{j},... + p.dphi_fun{i}{j},... + s.grid.eval,... + s.vecfield.eval.content.Avec_optimized); + p.G_locus{i,j}.G_local = g_all; + p.G_locus{i,j}.G_opt_local = g_all_opt; + % lift each point into previous frame + if j == 1 + % no previous frame + p.G_locus{i,j}.G = p.G_locus{i,j}.G_local; %init. cond.? + p.G_locus{i,j}.G_opt = p.G_locus{i,j}.G_opt_local; + continue; + end + % multiply ea. point by end of last segment + % this notation is awful, can we improve with another pass? + p.G_locus{i,j}.G = cell(size(p.G_locus{i,j}.G_local)); + p.G_locus{i,j}.G_opt = cell(size(p.G_locus{i,j}.G_opt_local)); + for k = 1:length(p.G_locus{i,j-1}) + p.G_locus{i,j}.G{k} = p.G_locus{i,j-1}.G{end} * p.G_locus{i,j}.G_local{k}; + p.G_locus{i,j}.G_opt{k} = p.G_locus{i,j-1}.G_opt{end} * p.G_locus{i,j}.G_opt_local{k}; + end + end + % concat all configurations + p.G_locus_full{i}.G = []; + p.G_locus_full{i}.G_opt = []; + for j = 1:length(p.phi_fun{i}) + p.G_locus_full{i}.G = [p.G_locus_full{i}.G, p.G_locus{i,j}.G]; + p.G_locus_full{i}.G_opt = [p.G_locus_full{i}.G_opt, p.G_locus{i,j}.G_opt]; + end + % compute cBVI + % should be general-purpose already + if ~strcmp(p.cBVI_method{i}{1},'none') + [p.cBVI, p.cBVI_opt] = integrate_cBVI(s,p); + end + end +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcpath_fcns/integrate_cBVI.m b/ProgramFiles/sys_calcpath_fcns/integrate_cBVI.m index 6131295c..6d6d46ac 100644 --- a/ProgramFiles/sys_calcpath_fcns/integrate_cBVI.m +++ b/ProgramFiles/sys_calcpath_fcns/integrate_cBVI.m @@ -39,7 +39,7 @@ ,struct('method','gauss','tol',1e-6))... ,(1:n_g)'); - catch + catch ME if numel(s.grid.eval) == 2 warning('Gait curve not sufficiently convex for simple integration. Specify either ''delaunay'' or ''section'' in p.complex_curve{i} to invoke that integration method') diff --git a/ProgramFiles/sys_calcpath_fcns/pathlength_integrator.m b/ProgramFiles/sys_calcpath_fcns/pathlength_integrator.m index df534725..ec5d540d 100644 --- a/ProgramFiles/sys_calcpath_fcns/pathlength_integrator.m +++ b/ProgramFiles/sys_calcpath_fcns/pathlength_integrator.m @@ -2,12 +2,14 @@ % Get the shape and shape derivative at the current time shape = phi_fun(t); + shape = shape(:); shapelist = num2cell(shape); dshape = dphi_fun(t); + dshape = dshape(:); n_dim=length(s.vecfield.eval.content.Avec_optimized(1,:)); if length(shape)n_dim - shape=shape(1,1:n_dim); + shape=shape(1:n_dim); end shapelist = num2cell(shape); if length(dshape)>n_dim diff --git a/ProgramFiles/sys_calcpath_fcns/se2_integrator_all_terms.m b/ProgramFiles/sys_calcpath_fcns/se2_integrator_all_terms.m index 173ca9e7..18f83bac 100644 --- a/ProgramFiles/sys_calcpath_fcns/se2_integrator_all_terms.m +++ b/ProgramFiles/sys_calcpath_fcns/se2_integrator_all_terms.m @@ -5,13 +5,14 @@ % Get the shape and shape derivative at the current time shape = phi_fun(t); + shape = shape(:); shapelist = num2cell(shape); dshape = dphi_fun(t); - dshape1=dshape(:); + dshape=dshape(:); n_dim=length(s.vecfield.eval.content.Avec_optimized(1,:)); if length(shape)n_dim - shape=shape(1,1:n_dim); + shape=shape(1:n_dim); end shapelist = num2cell(shape); if length(dshape)>n_dim diff --git a/ProgramFiles/sys_calcpath_fcns/so3_integrator.m b/ProgramFiles/sys_calcpath_fcns/so3_integrator.m new file mode 100644 index 00000000..042d2e6d --- /dev/null +++ b/ProgramFiles/sys_calcpath_fcns/so3_integrator.m @@ -0,0 +1,46 @@ +% so3_integrator +% computes displacement over time for a kinematic system on SO(3) +% inputs: + % t_vec: vector of timesteps + % alpha_fun: vector-valued function of time, producing shape variables + % d_alpha_fun: time derivative of alpha + % grid: ndgrid for A + % A: local connection tensor +% outputs: + % g_circ: exponential coordinates of displacements + % g: cell of xyz transform matrices with respect to timesteps t + +function [g_circ, g] = so3_integrator(t_vec, alpha_fun, d_alpha_fun, grid, A) + % cayley fn for orthogonalization + cay = @(A) (eye(3)+A)\(eye(3)-A); + % preallocate cell of matrices + g = cell(1,length(t_vec)); + g{1} = eye(3); + % for each timestep (gap in t): + for i = 2:length(t_vec) + % get shape alpha, ss velocity d_alpha + t = t_vec(i); + alpha_vec = alpha_fun(t); + alpha_cell = num2cell(alpha_vec); + d_alpha_vec = d_alpha_fun(t); + % get LC matrix A at alpha (interpolate) + A_mat = zeros(size(A)); + for entry = 1:numel(A) + A_mat(entry) = interpn(grid{:},A{entry},alpha_cell{:}); + end + % use d_alpha to produce xyz velocity + g_circ_mat = vec_to_mat_SO3(A_mat * d_alpha_vec); + % exponentiate + g_mat_t = expm((t_vec(i)-t_vec(i-1))*g_circ_mat); + % cumprod with previous cell + g{i} = g{i-1}*g_mat_t; + % re-orthogonalize + %g{i} = g{i}/det(g{i}); %bad method + g{i} = cay(cay(g{i})); + end + % take matrix log of ea. cell for return + g_circ = zeros(3,length(t_vec)); + for i = 1:length(t_vec) + g_circ(:,i) = mat_to_vec_SO3(logm(g{i})); + end +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem.m b/ProgramFiles/sys_calcsystem.m index f753846a..905c04dd 100644 --- a/ProgramFiles/sys_calcsystem.m +++ b/ProgramFiles/sys_calcsystem.m @@ -26,36 +26,47 @@ %%%%% %Processing stages, passed off to secondary functions - - %Create grids for evaluating the connection functions - s = create_grids(s); - + % Ensure that there is a connection and a metric s = ensure_connection_and_metric(s); - - + + %Create grids for evaluating the connection functions + s = create_grids(s); %Evaluate the connection and metric over the fine grid for calculations and the coarse %grid for vector display s = evaluate_connection(s); s = evaluate_metric(s); - + s = evaluate_metric_derivatives(s); %Merge components of evaluated connection and metric s = merge_connection(s); s = merge_metric(s); - %Calculate optimal coordinate choice - s = optimize_coordinate_choice(s); + %Calculate optimal coordinate choice, respecting group + optimize = s.conf_space.optimizer_fn; + s = optimize(s); %Calculate the constraint curvature functions from the connection - s = calc_constraint_curvature(s); + s = calc_ccf_sys(s); %Build a stretch function corresponding to the metric only if %the system is 2 dimensional if length(s.grid_range)/2<3 s = calc_stretch_functions(s); end + + % Check for whether system type variable exists in structure; default to + % 'drag' type if non-existent and throw an error if an unsuitable value is + % present + if isfield(s,'system_type') + if ~( strcmpi(s.system_type,'drag') || strcmpi(s.system_type,'inertia') ) + error('Invalid system_type flag in system struct; must be ''drag'' or ''inertia''') + end + else + disp('No system_type flag set in system struct, defaulting to ''drag''') + s.system_type = 'drag'; + end %Save out the updated system properties save(outfile,'s') diff --git a/ProgramFiles/sys_calcsystem_fcns/calc_ccf.m b/ProgramFiles/sys_calcsystem_fcns/calc_ccf.m new file mode 100644 index 00000000..957bb682 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/calc_ccf.m @@ -0,0 +1,96 @@ +% calc_ccf.m +% computes constraint curvature over a grid +% inputs: + % grid_points: cell array of point arrays over which A is defined + % A: local connection tensor (any position, shape space) + % mat_fun: + % anonymous function, mapping configuration vector representation + % to corresponding matrix representation + % vec_fun: + % inverse of mat_fun; should return column vector +% outputs: + % DA: total lie bracket (CCF) vector + % dA: exterior derivative of A + % represented as (n_dim)x(n_2_forms) cell array + % involves choice of basis order - currently (earlier)^(later) + % todo: order specification requires: + % different indexing method + % structure containing signs + % lb: local Lie bracket of A columns + +function [DA, dA, lb] = calc_ccf(grid_points, A, mat_fun, vec_fun) + %% compute dA 2-form + % preallocate dA: n_dim(xyz) x shvar_dim choose 2 (num. of 2-forms) + dA = cell(size(A,1), nchoosek(size(A,2), 2)); + % iter. through ea. dim. (xyz) in A + for i = 1:size(A,1) + % compute gradient of ea. shape w.r.t. ea. other + % repr. as J (rows are shvars, cols are der. wrt. other shvar) + partials = cell(size(A,2)); + order = [2 1 3:size(A,2)]; %ordering for gradient + grad_Aij = cell(1, size(A,2)); %temp. storage for ea. gradient + for j = 1:size(A,2) + % get gradient (partial of this shape wrt ea. other, everywhere) + [grad_Aij{order}] = gradient(A{i,j},grid_points{order}); + partials(j,:) = grad_Aij; + end + % iter. thru J (all rows, col < row): + entry = 1; +% for r = 1:size(A,2) +% for c = 1:r + for c = 1:size(A,2) + for r = c:size(A,2) + % combine like bases (respecting wedge product rules) + % if doing a different choice of basis, change: + % which entry (r,c) pairs are saved in + % sign choices for TR and BL off-diagonals + if r == c + % skip zero wedge products + continue; + end + % two partials for this basis + dA{i,entry} = partials{r,c} - partials{c,r}; + + entry = entry + 1; + end + end + end + + %% compute local Lie bracket + % convert A; working pointwise + A_pts = celltensorconvert(A); + % compute Lie bracket at ea. point (parfor -> speed) + lb_pts = cell(size(A_pts)); + parfor i = 1:numel(A_pts) + % suppressed warning about function calls in parfor; unavoidable + %#ok<*PFBNS> + % preallocate space + lb_pts{i} = zeros(size(dA)); + % iterate thru. combinations of bases + entry = 1; +% for aj = 1:size(A,2) +% for ak = 1:aj + for ak = 1:size(A,2) + for aj = ak:size(A,2) + % skip zero wedge products + if aj == ak + continue; + end + % get local Lie bracket of shapes + % done according to configuration space + X = mat_fun(A_pts{i}(:,aj)); + Y = mat_fun(A_pts{i}(:,ak)); + lb_pts{i}(:,entry) = vec_fun(X*Y-Y*X); + entry = entry + 1; + end + end + end + lb = celltensorconvert(lb_pts); + + %% combine for DA + DA = cell(size(dA)); + for i = 1:numel(dA) + DA{i} = dA{i} - lb{i}; %sign choice for consistency + end + +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/calc_ccf_sys.m b/ProgramFiles/sys_calcsystem_fcns/calc_ccf_sys.m new file mode 100644 index 00000000..2b963f9d --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/calc_ccf_sys.m @@ -0,0 +1,16 @@ +% calc_ccf_sys.m +% sysplotter wrapper for constraint curvature function + +function s = calc_ccf_sys(s) + % get grid points in ea. dim + grid = s.grid.eval; + samples = cellfun(@(x) unique(x), grid, 'UniformOutput', false); + % get matrix, vector repr. fns + mat_fn = s.conf_space.mat_fn; + vec_fn = s.conf_space.vec_fn; + % compute CCF for orig. opt coords + A_orig = s.vecfield.eval.content.Avec; + A_opt = s.vecfield.eval.content.Avec_optimized; + [s.DA, s.dA, s.lb] = calc_ccf(samples, A_orig, mat_fn, vec_fn); + [s.DA_optimized, s.dA_optimized, s.lb_optimized] = calc_ccf(samples, A_opt, mat_fn, vec_fn); +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/create_grids.m b/ProgramFiles/sys_calcsystem_fcns/create_grids.m index 74faadf7..4ebf6caf 100644 --- a/ProgramFiles/sys_calcsystem_fcns/create_grids.m +++ b/ProgramFiles/sys_calcsystem_fcns/create_grids.m @@ -15,7 +15,7 @@ %list of grids that will be made - grid_list = {'vector','scalar','eval','metric_eval','metric_display','finite_element'}; + grid_list = {'vector','scalar','eval','metric_eval','metric_display','finite_element','mass_eval','coriolis_eval'}; % Create a cell array to hold the grid primitives gridprim = cell(length(s.grid_range)/2,1); @@ -50,5 +50,29 @@ [s.grid.(grid_list{i}){:}] = ndgrid(gridprim{:}); end + + %%%%%%% + % Create visualization grid if not specified elsewhere + + if isfield(s,'visual') && ~isfield(s.visual,'grid') + + % Get the number of shape variables + n_shape = nargin(s.A_num); + + % Create a cell to hold the grid elements + s.visual.grid = cell(n_shape,1); + + % Fill in the grid points using ndgrid. If only one set of grid points + % is supplied, use them for all dimensions. Otherwise, use the ith set + % of grid values for the ith shape dimension + + if isa(s.visual.grid_spacing,'numeric') + s.visual.grid_spacing = {s.visual.grid_spacing}; + end + + [s.visual.grid{:}] = ndgrid(s.visual.grid_spacing{:}); + + end + end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/ensure_connection_and_metric.m b/ProgramFiles/sys_calcsystem_fcns/ensure_connection_and_metric.m index 1e0aa58c..17fb4645 100644 --- a/ProgramFiles/sys_calcsystem_fcns/ensure_connection_and_metric.m +++ b/ProgramFiles/sys_calcsystem_fcns/ensure_connection_and_metric.m @@ -1,6 +1,8 @@ -function s = ensure_connection_and_metric(s) - +% ensure_connection_and_metric.m +% sanitizes local connection, metric, and group definition for a system +function s = ensure_connection_and_metric(s) + %% connection checks % If the connection was specified as s.A, copy this into s.A_num if ~isfield(s,'A_num') if isfield(s,'A') @@ -9,8 +11,6 @@ error('No local connection was specified as s.A or s.A_num in the sysf_file') end end - - % Get the number of shape variables n_shape = nargin(s.A_num); @@ -21,17 +21,24 @@ shape_test_point(idx) = ... (rand(1)*(s.grid_range(2*idx)-s.grid_range((2*idx)-1)))-s.grid_range((2*idx)-1); end - shape_test_list = num2cell(shape_test_point); - - %Ensure presence of a metric that is a square symmetric matrix + % Ensure existence of configuration space + if ~isfield(s, 'conf_space') + s.conf_space = LieGroups.SE2; + warning('No configuration space specified; assuming SE(2). To avoid this warning, add "s.conf_space = LieGroups.SE2" to the sysf.'); + end + % Enforce configuration space dimensions + A_dim = size(s.A_num(shape_test_list{:}),1); + if A_dim ~= s.conf_space.n_dim + error(['Supplied local connection is not of same dimensionality (' num2str(A_dim) ') as configuration space (' num2str(s.conf_space.n_dim) ').']); + end + + %% metric checks + % Ensure presence of a metric that is a square symmetric matrix if ~isfield(s,'metric') - s.metric = @(a,varargin) repmat(eye(size(shape_test_list,1)),size(a)); - else - % test evaluation of the matrix metric_test = s.metric(shape_test_list{:}); @@ -44,16 +51,9 @@ % Hard check for appropriate size elseif metric_size(1) ~= n_shape error('Metric matrix is not of correct size for system') -% % Soft check for symmetry + % Soft check for symmetry elseif ~(norm(metric_test-metric_test.',inf)<(norm(metric_test,inf)/10^6)) warning('Metric matrix does not appear to be symmetric') end - - end - - - - - end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/evaluate_metric_derivatives.m b/ProgramFiles/sys_calcsystem_fcns/evaluate_metric_derivatives.m new file mode 100644 index 00000000..ea44a338 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/evaluate_metric_derivatives.m @@ -0,0 +1,82 @@ +%Evaluate the connection over the fine grid for calculations and the coarse +%grid for vector display +function s = evaluate_metric_derivatives(s) + + % First, extract the metric and the grid + M = s.metricfield.metric_eval.content.metric; + grid = s.grid.metric_eval; + baseline = grid_to_baseline(grid); + + % Now construct an nx1 cell array to hold the derivatives of M with + % respect to the shape variables + dM = repmat({M},numel(grid),1); + + %%%%%% + % Now fill the cell array with the gradient of M (using ndgradient + % because it matches with ndgrid) + + % Cell array to temporarily hold the output of gradient + grad_components = cell(numel(grid),1); + + % Loop over every element of the metric + for idx_metric = 1:numel(M) + + % Get the gradient of that component, using ndmetric to correspond + % with the ndgrid used by our code + [grad_components{:}] = ndgradient(M{idx_metric},baseline{:}); + + % Loop over all derivative components, inserting the gradient + % component into the appropriate location + for idx_component = 1:numel(dM) + dM{idx_component}{idx_metric} = grad_components{idx_component}; + end + + end + + %%%%%%%%%%%%%%%% + % Second derivative + + % Now construct an nxn cell array to hold the derivatives of M with + % respect to the shape variables + ddM = repmat({M},numel(grid)); + + % Loop over every element of the metric + for idx_metric = 1:numel(M) + + % Loop over every element of the gradient + for idx_component1 = 1:numel(dM) + + % Get the gradient of that component of the gradient + [grad_components{:}] = ndgradient(dM{idx_component1}{idx_metric},baseline{:}); + + % Loop over all components of this derivative, inserting the + % gradient into the appropriate location + for idx_component2 = 1:numel(dM) + ddM{idx_component1,idx_component2}{idx_metric} = grad_components{idx_component2}; + end + end + + end + + %%%%%%%%%%%%%%%% + % Finally, save the output into the structure + s.coriolisfield.coriolis_eval.content.dM = dM; + s.coriolisfield.coriolis_eval.content.ddM = ddM; + + +% dmdalpha = shape_partial_mass(M_full,J_full,local_inertias,jointangles,A_eval,A_grid) + +% %list of all components of the local connection and metric that may be present +% component_list = {'metric','metric_den'}; +% +% +% % Evaluate all components in the list +% +% s = evaluate_tensors_in_system_file(s,component_list,{'metric_eval','metric_eval'},'metricfield'); +% +% % resample metric at a resolution appropriate for displaying as an +% % ellipse field +% s = resample_tensors_in_system_file(s,component_list,'metric_eval',{'metric_display','metric_display'},'metricfield'); + + +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/evaluate_tensors_in_system_file.m b/ProgramFiles/sys_calcsystem_fcns/evaluate_tensors_in_system_file.m index 290ff649..feb02c31 100644 --- a/ProgramFiles/sys_calcsystem_fcns/evaluate_tensors_in_system_file.m +++ b/ProgramFiles/sys_calcsystem_fcns/evaluate_tensors_in_system_file.m @@ -49,11 +49,7 @@ end end - - - - end -end +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/cell_layer_swap.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/cell_layer_swap.m new file mode 100644 index 00000000..b65c7171 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/cell_layer_swap.m @@ -0,0 +1,21 @@ +function B = cell_layer_swap(A) +% Takes an NxM cell array of YxZ cell arrays, and returns an YxZ cell array of +% NxM cell arrays + + +% Create a cell containing a cell whose dimensions are taken from A +new_cell = {cell(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 diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/convert_points.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/convert_points.m index 64c63287..283a66f1 100644 --- a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/convert_points.m +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/convert_points.m @@ -1,6 +1,6 @@ % Take points in the one set of coordinates, and express them in the % power_normalized coordinates -function [x_new, y_new] = convert_points(x_start,y_start,x_end,y_end,x_p,y_p) +function [x_new, y_new, z_new] = convert_points(x_start,y_start,x_end,y_end,x_p,y_p) %Identify any NaN elements nanIDx = isnan(x_p); @@ -17,6 +17,9 @@ %Reinsert the NaNs x_new(nanIDx) = NaN; y_new(nanIDy) = NaN; + + + z_new = zeros(size(x_new)); end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/convert_points_two_three.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/convert_points_two_three.m new file mode 100644 index 00000000..aca9b7e3 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/convert_points_two_three.m @@ -0,0 +1,23 @@ +% Take points in the one set of coordinates, and express them in the +% power_normalized coordinates +function [x_new, y_new, z_new] = convert_points_two_three(x_start,y_start,x_end,y_end,z_end,x_p,y_p) + + %Identify any NaN elements + nanIDx = isnan(x_p); + nanIDy = isnan(y_p); + + %Replace the NaN with valid holding values + x_p(nanIDx) = min(x_start(:)); + y_p(nanIDy) = min(y_start(:)); + + % interpolate + x_new = interpn(x_start,y_start,x_end,x_p,y_p,'cubic'); + y_new = interpn(x_start,y_start,y_end,x_p,y_p,'cubic'); + z_new = interpn(x_start,y_start,z_end,x_p,y_p,'cubic'); + + %Reinsert the NaNs + x_new(nanIDx) = NaN; + y_new(nanIDy) = NaN; + z_new(nanIDy) = NaN; + +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/fast_flatten_metric.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/fast_flatten_metric.m index 0f160cab..b0c2d357 100644 --- a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/fast_flatten_metric.m +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/fast_flatten_metric.m @@ -71,43 +71,240 @@ % Append the masked springs attribute onto the end of the spring array springs = [springs,masked_springs]; + method='metric_stretch'; - %%%%%%%%%% - % Processing step - % Relax the springs, with neutral lengths scaled by the mean of the - % neutral lengths so that the average neutral length is 1 - [final_x,final_y,sol] = relax_springs(x_scaled,y_scaled,springs,neutral_lengths/mean_neutral_length,0.01); + % Find the flattened coordinates + [final_x,final_y,sol] = relax_springs(x_scaled,y_scaled,springs,neutral_lengths/mean_neutral_length,0.01); + n = numel(x_scaled); + D_old = graph_matrix(springs, neutral_lengths, n); + convert.stretch.rv = stress(D_old, final_x, final_y); - % restore actual probelm scale + % Convert the metric to each location's tensor at a single + % location + metric = celltensorconvert(metric); + + % restore actual probelm scale final_x = final_x*mean_neutral_length; final_y = final_y*mean_neutral_length; + final_z = zeros(size(final_x)); - % Convert the metric to each location's tensor at a single - % location - metric = celltensorconvert(metric); - - - - %%%%%%%%%%%% - %%%%%%%%%%%% - %Build the output functions + %Build the output functions % Convert points - convert.old_to_new_points = @(x_old,y_old) convert_points(griddual{:},final_x,final_y,x_old,y_old); + convert.stretch.old_to_new_points = @(x_old,y_old) convert_points(griddual{:},final_x,final_y,x_old,y_old); Fx = TriScatteredInterp([final_x(:) final_y(:)],griddual{1}(:)); Fy = TriScatteredInterp([final_x(:) final_y(:)],griddual{2}(:)); - convert.new_to_old_points = @(x_new,y_new) multiTriInterp(Fx,Fy,x_new,y_new); + Fz = zeros(size(Fx)); + convert.stretch.new_to_old_points = @(x_new,y_new) multiTriInterp(Fx,Fy,Fz,x_new,y_new); % Jacobian from old to new tangent vectors final_jacobian = find_jacobian(griddual{:},final_x,final_y); - convert.jacobian = @(x_p,y_p) interpolate_cellwise_tensor(griddual{:},x_p,y_p,final_jacobian); + convert.stretch.jacobian = @(x_p,y_p) interpolate_cellwise_tensor(griddual{:},x_p,y_p,final_jacobian); % Metric in new space - final_jacobian_metric = arrayfun(convert.jacobian,grid{:},'UniformOutput',false); + final_jacobian_metric = arrayfun(convert.stretch.jacobian,grid{:},'UniformOutput',false); final_new_metric = cellfun(@(j,m) j'\m/j,final_jacobian_metric,(metric),'UniformOutput',false); - convert.new_metric = @(x_p,y_p) interpolate_cellwise_tensor(grid{:},x_p,y_p,celltensorconvert(final_new_metric)); - convert.old_metric = @(x_p,y_p) interpolate_cellwise_tensor(grid{:},x_p,y_p,celltensorconvert(metric)); + convert.stretch.new_metric = @(x_p,y_p) interpolate_cellwise_tensor(grid{:},x_p,y_p,celltensorconvert(final_new_metric)); + convert.stretch.old_metric = @(x_p,y_p) interpolate_cellwise_tensor(grid{:},x_p,y_p,celltensorconvert(metric)); + + %%%%%%%%%%%%%%%%%%%%%%%% + %%%%%%%%%%%%%%%%%%%%%%%% + + method='metric_surface'; + + varargin = [1 1]; + [final_x1, final_y1,final_z1, R, D,EI] = isomap2(x_scaled, y_scaled, springs, neutral_lengths, varargin(1), varargin(2)); + % Make the shape a dome instead of a bowl + if mean(final_z1(:)) < mean([final_z1(:,1);final_z1(:,end);final_z1(1,:)';final_z1(end,:)']) + final_z1 = -final_z1; + end + + convert.surface.rv = R; + convert.surface.D = D; + + + % Jacobian from old to new tangent vectors + jacobian = find_jacobian_two_three(griddual{:},final_x1,final_y1,final_z1); + convert.surface.jacobian = @(x_p,y_p) interpolate_cellwise_tensor(griddual{:},x_p,y_p,jacobian); + +% jacobian3 = find_jacobian3(griddual{:},final_x,final_y,final_z); +% Jacobian3 = @(x_p,y_p) interpolate_cellwise_tensor(griddual{:},x_p,y_p,jacobian); + % Calculate the new jacobian evaluated at the grid points + jacobian_metric = arrayfun(convert.surface.jacobian,grid{:},'UniformOutput',false); + new_metric = cellfun(@(j,m) j'\m/j,jacobian_metric,(metric),'UniformOutput',false); + + % Normalize by the average metric determinant, using product of + % singular values since we are up-projecting into higher dimension + met_grid = new_metric; + [~,SVs,~] = cellfun(@(m) svd(m),met_grid,'UniformOutput',false); + det_grid = cellfun(@(S) S(1,1)*S(2,2),SVs); + det_original = cellfun(@(m) det(m),metric); + %note that this is the determinant of the original metric, for correct + %integration. Assumption here is that the original coordinates are a + %regular grid + + mean_det = (det_grid(:)'*det_original(:))/sum(det_original(:)); + + % Scale the final positions so that the determinant at the origin is 1 + final_x1 = final_x1/(mean_det^(.25)); + final_y1 = final_y1/(mean_det^(.25)); + final_z1 = final_z1/(mean_det^(.25)); + EI.A = EI.A/(mean_det^(.25)); + EI.B = EI.B/(mean_det^(.25)); + EI.C = EI.C/(mean_det^(.25)); + % Convert points + convert.surface.old_to_new_points = @(x_old,y_old) convert_points_two_three(griddual{:},final_x1,final_y1,final_z1,x_old,y_old); + Fx = TriScatteredInterp([final_x1(:) final_y1(:) final_z1(:)],griddual{1}(:)); + Fy = TriScatteredInterp([final_x1(:) final_y1(:) final_z1(:)],griddual{2}(:)); + convert.surface.new_to_old_points = @(x_new,y_new) multiTriInterp(Fx,Fy,x_new,y_new,z_new); +% % Jacobian from old to new tangent vectors +% final_jacobian = find_jacobian(griddual{:},final_x,final_y); +% convert.surface.jacobian = @(x_p,y_p) interpolate_cellwise_tensor(griddual{:},x_p,y_p,final_jacobian); +% +% % Metric in new space +% final_jacobian_metric = arrayfun(convert.surface.jacobian,grid{:},'UniformOutput',false); +% final_new_metric = cellfun(@(j,m) j'\m/j,final_jacobian_metric,(metric),'UniformOutput',false); +% convert.surface.new_metric = @(x_p,y_p) interpolate_cellwise_tensor(grid{:},x_p,y_p,celltensorconvert(final_new_metric)); +% convert.surface.old_metric = @(x_p,y_p) interpolate_cellwise_tensor(grid{:},x_p,y_p,celltensorconvert(metric)); +% + % Sampled points + if strcmp(method, 'metric_surface') + convert.surface.old_x = griddual{1}; + convert.surface.old_y = griddual{2}; + convert.surface.new_x = final_x1; + convert.surface.new_y = final_y1; + convert.surface.new_z = final_z1; + convert.surface.EI = EI; + end + + + + + + + + + + +% +% +% +% if strcmp(method, 'null') +% method = 'metric_stretch'; +% end +% +% % Find the flattened coordinates +% if strcmp(method, 'metric_stretch') +% % [final_x,final_y,sol] = relax_springs(x,y,springs,neutral_lengths,0.1); +% [final_x,final_y,sol] = relax_springs(x_scaled,y_scaled,springs,neutral_lengths/mean_neutral_length,0.01); +% n = numel(x_scaled); +% D_old = graph_matrix(springs, neutral_lengths, n); +% convert.rv = stress(D_old, final_x, final_y); +% elseif strcmp(method, 'metric_surface') +% varargin = [1 1]; +% [final_x, final_y,final_z, R, D,EI] = isomap(x_scaled, y_scaled, springs, neutral_lengths, varargin(1), varargin(2)); +% convert.rv = R; +% convert.D = D; +% else +% error('Invalid method'); +% end +% +% % restore actual probelm scale +% final_x = final_x*mean_neutral_length; +% final_y = final_y*mean_neutral_length; +% +% +% +% %%%%%%%%%% +% % Processing step +% % Relax the springs, with neutral lengths scaled by the mean of the +% % neutral lengths so that the average neutral length is 1 +% % [final_x,final_y,sol] = relax_springs(x_scaled,y_scaled,springs,neutral_lengths/mean_neutral_length,0.01); +% +% % Convert the metric to each location's tensor at a single +% % location +% metric = celltensorconvert(metric); +% +% if strcmp(method, 'metric_surface') +% +% +% % Jacobian from old to new tangent vectors +% jacobian = find_jacobian(griddual{:},final_x,final_y); +% Jacobian = @(x_p,y_p) interpolate_cellwise_tensor(griddual{:},x_p,y_p,jacobian); +% +% % jacobian3 = find_jacobian3(griddual{:},final_x,final_y,final_z); +% % Jacobian3 = @(x_p,y_p) interpolate_cellwise_tensor(griddual{:},x_p,y_p,jacobian); +% % Calculate the new jacobian evaluated at the grid points +% jacobian_metric = arrayfun(Jacobian,grid{:},'UniformOutput',false); +% new_metric = cellfun(@(j,m) j'\m/j,jacobian_metric,(metric),'UniformOutput',false); +% +% % Normalize by the average metric determinant +% met_grid = new_metric; +% det_grid = cellfun(@(m) det(m),met_grid); +% det_original = cellfun(@(m) det(m),metric); +% %note that this is the determinant of the original metric, for correct +% %integration. Assumption here is that the original coordinates are a +% %regular grid +% +% mean_det = (det_grid(:)'*det_original(:))/sum(det_original(:)); +% +% % Scale the final positions so that the determinant at the origin is 1 +% final_x = final_x/(mean_det^(.25)); +% final_y = final_y/(mean_det^(.25)); +% EI.A = EI.A/(mean_det^(.25)); +% EI.B = EI.B/(mean_det^(.25)); +% EI.C = EI.C/(mean_det^(.25)); +% else +% +% % restore actual probelm scale +% final_x = final_x*mean_neutral_length; +% final_y = final_y*mean_neutral_length; +% +% end +% +% +% %%%%%%%%%% +% % % Processing step +% % % Relax the springs, with neutral lengths scaled by the mean of the +% % % neutral lengths so that the average neutral length is 1 +% % [final_x,final_y,sol] = relax_springs(x_scaled,y_scaled,springs,neutral_lengths/mean_neutral_length,0.01); +% % +% % % restore actual probelm scale +% % final_x = final_x*mean_neutral_length; +% % final_y = final_y*mean_neutral_length; +% % +% +% +% %%%%%%%%%%%% +% %%%%%%%%%%%% +% %Build the output functions +% +% % Convert points +% convert.old_to_new_points = @(x_old,y_old) convert_points(griddual{:},final_x,final_y,x_old,y_old); +% Fx = TriScatteredInterp([final_x(:) final_y(:)],griddual{1}(:)); +% Fy = TriScatteredInterp([final_x(:) final_y(:)],griddual{2}(:)); +% convert.new_to_old_points = @(x_new,y_new) multiTriInterp(Fx,Fy,x_new,y_new); +% +% % Jacobian from old to new tangent vectors +% final_jacobian = find_jacobian(griddual{:},final_x,final_y); +% convert.jacobian = @(x_p,y_p) interpolate_cellwise_tensor(griddual{:},x_p,y_p,final_jacobian); +% +% % Metric in new space +% final_jacobian_metric = arrayfun(convert.jacobian,grid{:},'UniformOutput',false); +% final_new_metric = cellfun(@(j,m) j'\m/j,final_jacobian_metric,(metric),'UniformOutput',false); +% convert.new_metric = @(x_p,y_p) interpolate_cellwise_tensor(grid{:},x_p,y_p,celltensorconvert(final_new_metric)); +% convert.old_metric = @(x_p,y_p) interpolate_cellwise_tensor(grid{:},x_p,y_p,celltensorconvert(metric)); +% +% % Sampled points +% if strcmp(method, 'metric_surface') +% convert.old_x = griddual{1}; +% convert.old_y = griddual{2}; +% convert.new_x = final_x; +% convert.new_y = final_y; +% convert.EI = EI; +% end +% end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/find_jacobian3.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/find_jacobian3.m new file mode 100644 index 00000000..08a6c85b --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/find_jacobian3.m @@ -0,0 +1,13 @@ +function jacobian = find_jacobian3(x_start,y_start,x_final,y_final,z_final) + + z_start = zeros(size(x_start)); + % Jacobian is a cell array, with each element as large as the grid + jacobian = cell(3,3); + + % Populate the jacobian from old to new coordinates + [jacobian{1,3},jacobian{1,2}, jacobian{1,1}] = gradient(x_final,z_start(1,:),y_start(1,:),x_start(:,1)); + [jacobian{2,3},jacobian{2,2}, jacobian{2,1}] = gradient(y_final,z_start(1,:),y_start(1,:),x_start(:,1)); + [jacobian{3,3},jacobian{3,2}, jacobian{3,1}] = gradient(z_final,z_start(1,:),y_start(1,:),x_start(:,1)); + + +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/find_jacobian_two_three.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/find_jacobian_two_three.m new file mode 100644 index 00000000..d96ae6d3 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/find_jacobian_two_three.m @@ -0,0 +1,11 @@ +function jacobian = find_jacobian_two_three(x_start,y_start,x_final,y_final,z_final) + + % Jacobian is a cell array, with each element as large as the grid + jacobian = cell(3,2); + + % Populate the jacobian from old to new coordinates + [jacobian{1,2}, jacobian{1,1}] = gradient(x_final,y_start(1,:),x_start(:,1)); + [jacobian{2,2}, jacobian{2,1}] = gradient(y_final,y_start(1,:),x_start(:,1)); + [jacobian{3,2}, jacobian{3,1}] = gradient(z_final,y_start(1,:),x_start(:,1)); + +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/IsomapI.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/IsomapI.m new file mode 100644 index 00000000..fba6a01a --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/IsomapI.m @@ -0,0 +1,220 @@ +function [Y, R, E] = IsomapI(D, n_fcn, n_size, options); + +% ISOMAP Computes Isomap embedding using the algorithm of +% Tenenbaum, de Silva, and Langford (2000). +% +% [Y, R, E] = isomap(D, n_fcn, n_size, options); +% +% Input: +% D = N x N matrix of distances (where N is the number of data points) +% n_fcn = neighborhood function ('epsilon' or 'k') +% n_size = neighborhood size (value for epsilon or k) +% +% options.dims = (row) vector of embedding dimensionalities to use +% (1:10 = default) +% options.comp = which connected component to embed, if more than one. +% (1 = largest (default), 2 = second largest, ...) +% options.display = plot residual variance and 2-D embedding? +% (1 = yes (default), 0 = no) +% options.overlay = overlay graph on 2-D embedding? +% (1 = yes (default), 0 = no) +% options.verbose = display progress reports? +% (1 = yes (default), 0 = no) +% +% Output: +% Y = Y.coords is a cell array, with coordinates for d-dimensional embeddings +% in Y.coords{d}. Y.index contains the indices of the points embedded. +% R = residual variances for embeddings in Y +% E = edge matrix for neighborhood graph +% + +% BEGIN COPYRIGHT NOTICE +% +% Isomap code -- (c) 1998-2000 Josh Tenenbaum +% +% This code is provided as is, with no guarantees except that +% bugs are almost surely present. Published reports of research +% using this code (or a modified version) should cite the +% article that describes the algorithm: +% +% J. B. Tenenbaum, V. de Silva, J. C. Langford (2000). A global +% geometric framework for nonlinear dimensionality reduction. +% Science 290 (5500): 2319-2323, 22 December 2000. +% +% Comments and bug reports are welcome. Email to jbt@psych.stanford.edu. +% I would also appreciate hearing about how you used this code, +% improvements that you have made to it, or translations into other +% languages. +% +% You are free to modify, extend or distribute this code, as long +% as this copyright notice is included whole and unchanged. +% +% END COPYRIGHT NOTICE + + +%%%%% Step 0: Initialization and Parameters %%%%% + +N = size(D,1); +if ~(N==size(D,2)) + error('D must be a square matrix'); +end; +if n_fcn=='k' + K = n_size; + if ~(K==round(K)) + error('Number of neighbors for k method must be an integer'); + end +elseif n_fcn=='epsilon' + epsilon = n_size; +else + error('Neighborhood function must be either epsilon or k'); +end +if nargin < 3 + error('Too few input arguments'); +elseif nargin < 4 + options = struct('dims',1:10,'overlay',1,'comp',1,'display',1,'verbose',1); +end +INF = 1000*max(max(D))*N; %% effectively infinite distance + +if ~isfield(options,'dims') + options.dims = 1:10; +end +if ~isfield(options,'overlay') + options.overlay = 1; +end +if ~isfield(options,'comp') + options.comp = 1; +end +if ~isfield(options,'display') + options.display = 1; +end +if ~isfield(options,'verbose') + options.verbose = 1; +end +dims = options.dims; +comp = options.comp; +overlay = options.overlay; +displ = options.display; +verbose = options.verbose; + +Y.coords = cell(length(dims),1); +R = zeros(1,length(dims)); + +%%%%% Step 1: Construct neighborhood graph %%%%% +disp('Constructing neighborhood graph...'); + +if n_fcn == 'k' + [tmp, ind] = sort(D); + for i=1:N + D(i,ind((2+K):end,i)) = INF; + end +elseif n_fcn == 'epsilon' + warning off %% Next line causes an unnecessary warning, so turn it off + D = D./(D<=epsilon); + D = min(D,INF); + warning on +end + +D = min(D,D'); %% Make sure distance matrix is symmetric + +if (overlay == 1) + E = int8(1-(D==INF)); %% Edge information for subsequent graph overlay +end + +% Finite entries in D now correspond to distances between neighboring points. +% Infinite entries (really, equal to INF) in D now correspond to +% non-neighoring points. + +%%%%% Step 2: Compute shortest paths %%%%% +disp('Computing shortest paths...'); + +% We use Floyd's algorithm, which produces the best performance in Matlab. +% Dijkstra's algorithm is significantly more efficient for sparse graphs, +% but requires for-loops that are very slow to run in Matlab. A significantly +% faster implementation of Isomap that calls a MEX file for Dijkstra's +% algorithm can be found in isomap2.m (and the accompanying files +% dijkstra.c and dijkstra.dll). + +tic; +for k=1:N + D = min(D,repmat(D(:,k),[1 N])+repmat(D(k,:),[N 1])); + if ((verbose == 1) & (rem(k,20) == 0)) + disp([' Iteration: ' num2str(k) ' Estimated time to completion: ' num2str((N-k)*toc/k/60) ' minutes']); + end +end + +%%%%% Remove outliers from graph %%%%% +disp('Checking for outliers...'); +n_connect = sum(~(D==INF)); %% number of points each point connects to +[tmp, firsts] = min(D==INF); %% first point each point connects to +[comps, I, J] = unique(firsts); %% represent each connected component once +size_comps = n_connect(comps); %% size of each connected component +[tmp, comp_order] = sort(size_comps); %% sort connected components by size +comps = comps(comp_order(end:-1:1)); +size_comps = size_comps(comp_order(end:-1:1)); +n_comps = length(comps); %% number of connected components +if (comp>n_comps) + comp=1; %% default: use largest component +end +disp([' Number of connected components in graph: ' num2str(n_comps)]); +disp([' Embedding component ' num2str(comp) ' with ' num2str(size_comps(comp)) ' points.']); +Y.index = find(firsts==comps(comp)); + +D = D(Y.index, Y.index); +N = length(Y.index); + +%%%%% Step 3: Construct low-dimensional embeddings (Classical MDS) %%%%% +disp('Constructing low-dimensional embeddings (Classical MDS)...'); + +opt.disp = 0; +[vec, val] = eigs(-.5*(D.^2 - sum(D.^2)'*ones(1,N)/N - ones(N,1)*sum(D.^2)/N + sum(sum(D.^2))/(N^2)), max(dims), 'LR', opt); + +h = real(diag(val)); +[foo,sorth] = sort(h); sorth = sorth(end:-1:1); +val = real(diag(val(sorth,sorth))); +vec = vec(:,sorth); + +D = reshape(D,N^2,1); +for di = 1:length(dims) + if (dims(di)<=N) + Y.coords{di} = real(vec(:,1:dims(di)).*(ones(N,1)*sqrt(val(1:dims(di)))'))'; + r2 = 1-corrcoef(reshape(real(L2_distance(Y.coords{di}, Y.coords{di})),N^2,1),D).^2; + R(di) = r2(2,1); + if (verbose == 1) + disp([' Isomap on ' num2str(N) ' points with dimensionality ' num2str(dims(di)) ' --> residual variance = ' num2str(R(di))]); + end + end +end + +clear D; + +%%%%%%%%%%%%%%%%%% Graphics %%%%%%%%%%%%%%%%%% + +if (displ==1) + %%%%% Plot fall-off of residual variance with dimensionality %%%%% + figure; + hold on + plot(dims, R, 'bo'); + plot(dims, R, 'b-'); + hold off + ylabel('Residual variance'); + xlabel('Isomap dimensionality'); + + %%%%% Plot two-dimensional configuration %%%%% + twod = find(dims==2); + if ~isempty(twod) + figure; + hold on; + plot(Y.coords{twod}(1,:), Y.coords{twod}(2,:), 'ro'); + if (overlay == 1) + gplot(E(Y.index, Y.index), [Y.coords{twod}(1,:); Y.coords{twod}(2,:)]'); + title('Two-dimensional Isomap embedding (with neighborhood graph).'); + else + title('Two-dimensional Isomap.'); + end + hold off; + end +end + +return; + + diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/IsomapII.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/IsomapII.m new file mode 100644 index 00000000..4ef8ffa4 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/IsomapII.m @@ -0,0 +1,376 @@ +function [Y, R, E] = IsomapII(D, n_fcn, n_size, options); + +% ISOMAPII Computes Isomap embedding using an advanced version of +% the algorithm in Tenenbaum, de Silva, and Langford (2000), +% which can take advantage of sparsity in the graph and +% redundancy in the distances. +% +% [Y, R, E] = isomapII(D, n_fcn, n_size, options); +% +% Input: +% D = input-space distances between pairs of N points, which can +% take 1 of 3 forms: +% (1) a full N x N matrix (as in isomap.m) +% (2) a sparse N x N matrix (missing entries are treated as INF) +% (3) the name of a function (e.g. 'd_fun') that takes +% one argument, i, and returns a row vector containng the +% distances from all N points to point i. +% +% n_fcn = neighborhood function ('epsilon' or 'k') +% n_size = neighborhood size (value for epsilon or k) +% +% options = optional structure of options: +% options.dims = (row) vector of embedding dimensionalities to use +% (1:10 = default) +% options.comp = which connected component to embed, if more than one. +% (1 = largest (default), 2 = second largest, ...) +% options.display = plot residual variance and 2-D embedding? +% (1 = yes (default), 0 = no) +% options.overlay = overlay graph on 2-D embedding? +% (1 = yes (default), 0 = no) +% options.verbose = display progress reports? +% (1 = yes (default), 0 = no) +% options.dijkstra = use dijkstra's algorithm for shortest paths with +% full N x N distance matrix. +% (1 = yes (default), 0 = use Floyd; Floyd should +% be used only if you are unable to MEX dijkstra.cpp) +% options.Kmax = maximum number of neighbors (used for sparse versions +% of epsilon; by default, estimated by random sample) +% options.landmarks = (row) vector of landmark points to use in MDS. +% (MDS finds the configuration that best approximates +% the distances from all points to the landmark points. +% The default landmark points are 1:N (i.e. all the points), +% which is equivalent to classical MDS. Good +% results may often be obtained using a number of +% landmarks that is much smaller than N, but much +% larger than the data's intrinsic dimensionality. +% Note that this extension is experimental! For +% discussion, see Steyvers, de Silva, and Tenenbaum +% (in preparation).) +% +% Output: +% Y = Y.coords is a cell array, with coordinates for d-dimensional embeddings +% in Y.coords{d}. Y.index contains the indices of the points embedded. +% R = residual variances for embeddings in Y +% E = edge matrix for neighborhood graph +% + +% BEGIN COPYRIGHT NOTICE +% +% Isomap II code -- (c) 1998-2000 Josh Tenenbaum +% +% This code is provided as is, with no guarantees except that +% bugs are almost surely present. Published reports of research +% using this code (or a modified version) should cite the +% article that describes the algorithm: +% +% J. B. Tenenbaum, V. de Silva, J. C. Langford (2000). A global +% geometric framework for nonlinear dimensionality reduction. +% Science 290 (5500): 2319-2323, 22 December 2000. +% +% Comments and bug reports are welcome. Email to jbt@psych.stanford.edu. +% I would also appreciate hearing about how you used this code, +% improvements that you have made to it, or translations into other +% languages. +% +% You are free to modify, extend or distribute this code, as long +% as this copyright notice is included whole and unchanged. +% +% END COPYRIGHT NOTICE + + +%%%%% Step 0: Initialization and Parameters %%%%% + +if nargin < 3 + error('Too few input arguments'); +elseif nargin < 4 + options = struct('dims',1:10,'overlay',1,'comp',1,'display',1,'dijkstra',1,'verbose',1); +end + +if ischar(D) + mode = 3; + d_func = D; + N = length(feval(d_func,1)); +elseif issparse(D) + mode = 2; + N = size(D,1); + if ~(N==size(D,2)) + error('D must be a square matrix'); + end; +else + mode = 1; + N = size(D,1); + if ~(N==size(D,2)) + error('D must be a square matrix'); + end; +end + +if n_fcn=='k' + K = n_size; + if ~(K==round(K)) + error('Number of neighbors for k method must be an integer'); + end + if ((mode==2) & ~(min(sum(D'>0))>=K)) + error('Sparse D matrix must contain at least K nonzero entries in each row'); + end +elseif n_fcn=='epsilon' + epsilon = n_size; + if isfield(options,'Kmax') + K = options.Kmax; + elseif (mode==3) %% estimate maximum equivalent K %% + tmp = zeros(10,N); + for i=1:10 + tmp(i,:) = feval(d_func,ceil(N*rand)); + end + K = 2*max(sum(tmp'n_comps) + comp=1; %% default: use largest component +end +Y.index = find(firsts==comps(comp)); %% list of points in relevant component +Y.index = setdiff(Y.index,find(isinf(min(D)))); %% prune points that don't connect + %% to any landmarks +N = length(Y.index); +[tmp, landmarks, land_ind] = intersect(landmarks,Y.index); + %% list of landmarks in component +nl = length(landmarks); +D = full(D(landmarks,Y.index))'; +disp([' Number of connected components in graph: ' num2str(n_comps)]); +disp([' Embedding component ' num2str(comp) ' with ' num2str(length(Y.index)) ' points.']); + +dims = unique(min(dims,nl-1)); %% don't embed in more dimensions than landmarks-1 +if (nl==N) + opt.disp = 0; + [vec, val] = eigs(-.5*(D.^2 - sum(D.^2)'*ones(1,N)/N - ones(N,1)*sum(D.^2)/N + sum(sum(D.^2))/(N^2)), max(dims), 'LR', opt); +else + subB = -.5*(D.^2 - sum(D'.^2)'*ones(1,nl)/nl - ones(N,1)*sum(D.^2)/N+sum(sum(D.^2))/(N*nl)); + opt.disp = 0; + [alpha,beta] = eigs(subB'*subB, max(dims), 'LR', opt); + val = beta.^(1/2); + vec = subB*alpha*inv(val); + clear subB alpha beta; +end +h = real(diag(val)); +[foo,sorth] = sort(h); sorth = sorth(end:-1:1); +val = real(diag(val(sorth,sorth))); +vec = vec(:,sorth); + +D = reshape(D,N*nl,1); +for di = 1:length(dims) + Y.coords{di} = real(vec(:,1:dims(di)).*(ones(N,1)*sqrt(val(1:dims(di)))'))'; + r2 = 1-corrcoef(reshape(real(L2_distance(Y.coords{di}, Y.coords{di}(:,land_ind))),N*nl,1),D).^2; + R(di) = r2(2,1); + if (verbose == 1) + disp([' Isomap on ' num2str(N) ' points with dimensionality ' num2str(dims(di)) ' --> residual variance = ' num2str(R(di))]); + end +end + +clear D; + +%%%%%%%%%%%%%%%%%% Graphics %%%%%%%%%%%%%%%%%% + +if (displ==1) + %%%%% Plot fall-off of residual variance with dimensionality %%%%% + figure; + hold on + plot(dims, R, 'bo'); + plot(dims, R, 'b-'); + hold off + ylabel('Residual variance'); + xlabel('Isomap dimensionality'); + + %%%%% Plot two-dimensional configuration %%%%% + twod = find(dims==2); + if ~isempty(twod) + figure; + hold on; + plot(Y.coords{twod}(1,:), Y.coords{twod}(2,:), 'ro'); + if (overlay == 1) + gplot(E(Y.index, Y.index), [Y.coords{twod}(1,:); Y.coords{twod}(2,:)]'); + title('Two-dimensional Isomap embedding (with neighborhood graph).'); + else + title('Two-dimensional Isomap.'); + end + hold off; + end +end + +return; + + + + + + + diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/L2_distance.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/L2_distance.m new file mode 100644 index 00000000..a925f0fb --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/L2_distance.m @@ -0,0 +1,72 @@ +function d = L2_distance(a,b,df) +% L2_DISTANCE - computes Euclidean distance matrix +% +% E = L2_distance(A,B) +% +% A - (DxM) matrix +% B - (DxN) matrix +% df = 1, force diagonals to be zero; 0 (default), do not force +% +% Returns: +% E - (MxN) Euclidean distances between vectors in A and B +% +% +% Description : +% This fully vectorized (VERY FAST!) m-file computes the +% Euclidean distance between two vectors by: +% +% ||A-B|| = sqrt ( ||A||^2 + ||B||^2 - 2*A.B ) +% +% Example : +% A = rand(400,100); B = rand(400,200); +% d = distance(A,B); + +% Author : Roland Bunschoten +% University of Amsterdam +% Intelligent Autonomous Systems (IAS) group +% Kruislaan 403 1098 SJ Amsterdam +% tel.(+31)20-5257524 +% bunschot@wins.uva.nl +% Last Rev : Wed Oct 20 08:58:08 MET DST 1999 +% Tested : PC Matlab v5.2 and Solaris Matlab v5.3 + +% Copyright notice: You are free to modify, extend and distribute +% this code granted that the author of the original code is +% mentioned as the original author of the code. + +% Fixed by JBT (3/18/00) to work for 1-dimensional vectors +% and to warn for imaginary numbers. Also ensures that +% output is all real, and allows the option of forcing diagonals to +% be zero. + +if (nargin < 2) + error('Not enough input arguments'); +end + +if (nargin < 3) + df = 0; % by default, do not force 0 on the diagonal +end + +if (size(a,1) ~= size(b,1)) + error('A and B should be of same dimensionality'); +end + +if ~(isreal(a)*isreal(b)) + disp('Warning: running distance.m with imaginary numbers. Results may be off.'); +end + +if (size(a,1) == 1) + a = [a; zeros(1,size(a,2))]; + b = [b; zeros(1,size(b,2))]; +end + +aa=sum(a.*a); bb=sum(b.*b); ab=a'*b; +d = sqrt(repmat(aa',[1 size(bb,2)]) + repmat(bb,[size(aa,2) 1]) - 2*ab); + +% make sure result is all real +d = real(d); + +% force 0 on the diagonal? +if (df==1) + d = d.*(1-eye(size(d))); +end diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/center.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/center.m new file mode 100644 index 00000000..de803a65 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/center.m @@ -0,0 +1,10 @@ +function cent = center(D, N, nl) +%CENTER Centers a matrix +% M is the matrix to be centered +% N is the original number of rows +% nl is the desired number of columns + +% cent = -.5*(D.^2 - sum(D'.^2)'*ones(1,nl)/nl - ones(N,1)*sum(D.^2)/N+sum(sum(D.^2))/(N*nl)); + cent =-0.5*((eye(N)-1/N*ones(N,N))*D.^2*(eye(N)-1/N*ones(N,N))); +end + diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/dijk.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/dijk.m new file mode 100644 index 00000000..4143080c --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/dijk.m @@ -0,0 +1,109 @@ +function D = dijk(A,s,t) +%DIJK Shortest paths from nodes 's' to nodes 't' using Dijkstra algorithm. +% D = dijk(A,s,t) +% A = n x n node-node weighted adjacency matrix of arc lengths +% (Note: A(i,j) = 0 => Arc (i,j) does not exist; +% A(i,j) = NaN => Arc (i,j) exists with 0 weight) +% s = FROM node indices +% = [] (default), paths from all nodes +% t = TO node indices +% = [] (default), paths to all nodes +% D = |s| x |t| matrix of shortest path distances from 's' to 't' +% = [D(i,j)], where D(i,j) = distance from node 'i' to node 'j' +% +% (If A is a triangular matrix, then computationally intensive node +% selection step not needed since graph is acyclic (triangularity is a +% sufficient, but not a necessary, condition for a graph to be acyclic) +% and A can have non-negative elements) +% +% (If |s| >> |t|, then DIJK is faster if DIJK(A',t,s) used, where D is now +% transposed and P now represents successor indices) +% +% (Based on Fig. 4.6 in Ahuja, Magnanti, and Orlin, Network Flows, +% Prentice-Hall, 1993, p. 109.) + +% Copyright (c) 1998-2000 by Michael G. Kay +% Matlog Version 1.3 29-Aug-2000 +% +% Modified by JBT, Dec 2000, to delete paths + +% Input Error Checking ****************************************************** +error(nargchk(1,3,nargin)); + +[n,cA] = size(A); + +if nargin < 2 | isempty(s), s = (1:n)'; else s = s(:); end +if nargin < 3 | isempty(t), t = (1:n)'; else t = t(:); end + +if ~any(any(tril(A) ~= 0)) % A is upper triangular + isAcyclic = 1; +elseif ~any(any(triu(A) ~= 0)) % A is lower triangular + isAcyclic = 2; +else % Graph may not be acyclic + isAcyclic = 0; +end + +if n ~= cA + error('A must be a square matrix'); +elseif ~isAcyclic & any(any(A < 0)) + error('A must be non-negative'); +elseif any(s < 1 | s > n) + error(['''s'' must be an integer between 1 and ',num2str(n)]); +elseif any(t < 1 | t > n) + error(['''t'' must be an integer between 1 and ',num2str(n)]); +end +% End (Input Error Checking) ************************************************ + +A = A'; % Use transpose to speed-up FIND for sparse A + +D = zeros(length(s),length(t)); +P = zeros(length(s),n); + +for i = 1:length(s) + j = s(i); + + Di = Inf*ones(n,1); Di(j) = 0; + + isLab = logical(zeros(length(t),1)); + if isAcyclic == 1 + nLab = j - 1; + elseif isAcyclic == 2 + nLab = n - j; + else + nLab = 0; + UnLab = 1:n; + isUnLab = logical(ones(n,1)); + end + + while nLab < n & ~all(isLab) + if isAcyclic + Dj = Di(j); + else % Node selection + [Dj,jj] = min(Di(isUnLab)); + j = UnLab(jj); + UnLab(jj) = []; + isUnLab(j) = 0; + end + + nLab = nLab + 1; + if length(t) < n, isLab = isLab | (j == t); end + + [jA,kA,Aj] = find(A(:,j)); + Aj(isnan(Aj)) = 0; + + if isempty(Aj), Dk = Inf; else Dk = Dj + Aj; end + + P(i,jA(Dk < Di(jA))) = j; + Di(jA) = min(Di(jA),Dk); + + if isAcyclic == 1 % Increment node index for upper triangular A + j = j + 1; + elseif isAcyclic == 2 % Decrement node index for lower triangular A + j = j - 1; + end + + %disp( num2str( nLab )); + end + D(i,:) = Di(t)'; +end + diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/dijkstra.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/dijkstra.m new file mode 100644 index 00000000..98cbfefb --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/dijkstra.m @@ -0,0 +1,32 @@ +function D = dijkstra( G , S ) +% -------------------------------------------------------------------- +% Mark Steyvers, Stanford University, 12/19/00 +% -------------------------------------------------------------------- +% +% DIJKSTRA Find shortest paths in graphs +% D = dijkstra( G , S ) use the full or sparse matrix G in which +% an entry (i,j) represents the arc length between nodes i and j in a +% graph. In a full matrix, the value INF represents the absence of an arc; +% in a sparse matrix, no entry at (i,j) naturally represents no arc. +% +% S is the one-dimensional matrix of source nodes for which the shortest +% to ALL other nodes in the graphs will be calculated. The output matrices +% D and P contain the shortest distances and predecessor indices respectively. +% An infinite distance is represented by INF. The predecessor indices contain +% the node indices of the node along the shortest path before the destination +% is reached. These indices are useful to construct the shortest path with the +% function pred2path (by Michael G. Kay). +% +% This function was implemented in C++. The source code can be compiled to a +% Matlab compatible mex file by the command "mex -O dijkstra.cpp" at the Matlab +% prompt. In this package, we provide a compiled .dll version that is +% compatible all Windows based machines. If you are not working on a +% Windows platform, delete the .dll version provided and recompile from +% the .cpp source file. If you do not have the Matlab compiler or a Windows +% platform, delete the .dll version and dijkstra will then call the +% Matlab function dijk.m (by Michael G. Kay). Note that this Matlab +% code is several orders of magnitude slower than the C based mex file. + +N = size( G , 1 ); +D = dijk( G , S , 1:N ); + diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/graph_matrix.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/graph_matrix.m new file mode 100644 index 00000000..accccb4a --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/graph_matrix.m @@ -0,0 +1,15 @@ +function [D, G] = graph_matrix(springs, neutral_lengths, n) +%GRAPH_MATRIX Computes the matrix of distances given springs and lengths +% D: all distances, inferred using Dijkstra +% G: only neighbor distances from spring lengths + G = inf*ones(n); + for i=1:n + G(i,i)=0; + end + idx = sub2ind(size(G), springs(:,1), springs(:,2)); + G(idx) = neutral_lengths; + idx = sub2ind(size(G), springs(:,2), springs(:,1)); + G(idx) = neutral_lengths; + D = dijkstra(G,1:n); +end + diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/isomap.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/isomap.m new file mode 100644 index 00000000..83e07e47 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/isomap.m @@ -0,0 +1,78 @@ +function [final_x, final_y,final_z, rv, D,EI] = isomap(x, y, springs, neutral_lengths, w, plot3d) +% Runs underlying MDS to finish Isomap +% x, y, springs, and neutral_lengths are from springs computation +% plot3d tells whether or not to plot 3d manifold +% w is weighting factor + + % Use spring lengths to set up sparse distance matrix + n = numel(x); + [D, G] = graph_matrix(springs, neutral_lengths, n); + +% Call Isomap + options.dims = 1:10; + options.display = 0; + options.verbose = 1; + options.G = G; + options.Kmax=8; + [Y, R, E] = isomap_fast(D, 'epsilon', 8, options); + len=length(x(:,1)); + + opts=statset('Display','iter'); + z=zeros(len*len,1); + for i=1:len/2 + z(len*(i-1)+1:len*i,1)=(i-1)*0.005/20; + z((len-i)*len+1:(len-i+1)*len,1)=(i-1)*0.005/20; + end + + [Y,stress,disparities]=mdscale(D,3,'Start',[x(:),y(:),z],'Criterion','strain','Options',opts); +Y=Y'; +% figure(14) +% sf=fit([Y(:,1),Y(:,2)],Y(:,3),'poly23'); +% plot(sf,[Y(:,1),Y(:,2)],Y(:,3)) + + % Call MDS + weights = G~=0; + weights = w*(1+weights)-(w-1); +% [Y, R] = mdimscale(D, 1:3, 'stress', ones(size(weights))); + + % 3D Mesh plot + if plot3d == 1 + a = Y(1,:); + b = Y(2,:); + c = Y(3,:); + dx = .05; + dy = .05; +% x_edge = [floor(min(a)):dx:ceil(max(a))]; +% y_edge = [floor(min(b)):dy:ceil(max(b))]; + + y_edge = linspace(min(a),max(a),23);%[(min(a)+0.01):dx:(max(a)-0.01)]; + x_edge = linspace(min(b),max(b),23);%[(min(b)+0.01):dy:(max(b)-0.01)]; + [A,B] = ndgrid(x_edge,y_edge); + C = griddata(a,b,c,A,B,'cubic'); + + A = reshape(a,[],size(x,2)); + B = reshape(b,[],size(x,2)); + C = reshape(c,[],size(x,2)); + +% figure; +% % mesh(A,B,C); +% surf(A,B,C); +% axis equal; + end + + new_coords = Y; + final_x = reshape(new_coords(1,:), size(x)); + final_y = reshape(new_coords(2,:), size(y)); + final_z = reshape(new_coords(3,:), size(y)); + rv = R(2); + +% final_x = symmetry_vector(final_x); +% final_y = symmetry_vector(final_y); +% final_z = symmetry_vector(final_z); + + % export extra info + EI.A = A; + EI.B = B; + EI.C = C; + +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/isomap_fast.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/isomap_fast.m new file mode 100644 index 00000000..e89f9ae9 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/isomap_fast.m @@ -0,0 +1,245 @@ +function [Y, R, E] = isomap_fast(D, n_fcn, n_size, options); + +% ISOMAP_FAST Computes Isomap embedding using an advanced version of +% the algorithm in Tenenbaum, de Silva, and Langford (2000), +% which can take advantage of sparsity in the graph and +% redundancy in the distances. +% +% [Y, R, E] = isomapII(D, n_fcn, n_size, options); +% +% Input: +% D = input-space distances between pairs of N points, which can +% take 1 of 3 forms: +% (1) a full N x N matrix (as in isomap.m) +% (2) a sparse N x N matrix (missing entries are treated as INF) +% (3) the name of a function (e.g. 'd_fun') that takes +% one argument, i, and returns a row vector containng the +% distances from all N points to point i. +% +% n_fcn = neighborhood function ('epsilon' or 'k') +% n_size = neighborhood size (value for epsilon or k) +% +% options = optional structure of options: +% options.dims = (row) vector of embedding dimensionalities to use +% (1:10 = default) +% options.comp = which connected component to embed, if more than one. +% (1 = largest (default), 2 = second largest, ...) +% options.display = plot residual variance and 2-D embedding? +% (1 = yes (default), 0 = no) +% options.overlay = overlay graph on 2-D embedding? +% (1 = yes (default), 0 = no) +% options.verbose = display progress reports? +% (1 = yes (default), 0 = no) +% options.dijkstra = use dijkstra's algorithm for shortest paths with +% full N x N distance matrix. +% (1 = yes (default), 0 = use Floyd; Floyd should +% be used only if you are unable to MEX dijkstra.cpp) +% options.Kmax = maximum number of neighbors (used for sparse versions +% of epsilon; by default, estimated by random sample) +% options.landmarks = (row) vector of landmark points to use in MDS. +% (MDS finds the configuration that best approximates +% the distances from all points to the landmark points. +% The default landmark points are 1:N (i.e. all the points), +% which is equivalent to classical MDS. Good +% results may often be obtained using a number of +% landmarks that is much smaller than N, but much +% larger than the data's intrinsic dimensionality. +% Note that this extension is experimental! For +% discussion, see Steyvers, de Silva, and Tenenbaum +% (in preparation).) +% +% Output: +% Y = Y.coords is a cell array, with coordinates for d-dimensional embeddings +% in Y.coords{d}. Y.index contains the indices of the points embedded. +% R = residual variances for embeddings in Y +% E = edge matrix for neighborhood graph +% + +%%%%% Step 0: Initialization and Parameters %%%%% + +if nargin < 3 + error('Too few input arguments'); +elseif nargin < 4 + options = struct('dims',1:10,'overlay',1,'comp',1,'display',1,'dijkstra',1,'verbose',1); +end + +if ischar(D) + mode = 3; + d_func = D; + N = length(feval(d_func,1)); +elseif issparse(D) + mode = 2; + N = size(D,1); + if ~(N==size(D,2)) + error('D must be a square matrix'); + end; +else + mode = 1; + N = size(D,1); + if ~(N==size(D,2)) + error('D must be a square matrix'); + end; +end + +if n_fcn=='k' + K = n_size; + if ~(K==round(K)) + error('Number of neighbors for k method must be an integer'); + end + if ((mode==2) & ~(min(sum(D'>0))>=K)) + error('Sparse D matrix must contain at least K nonzero entries in each row'); + end +elseif n_fcn=='epsilon' + epsilon = n_size; + if isfield(options,'Kmax') + K = options.Kmax; + elseif (mode==3) %% estimate maximum equivalent K %% + tmp = zeros(10,N); + for i=1:10 + tmp(i,:) = feval(d_func,ceil(N*rand)); + end + K = 2*max(sum(tmp'n_comps) + comp=1; %% default: use largest component +end + +Y.index = find(firsts==comps(comp)); %% list of points in relevant component +Y.index = setdiff(Y.index,find(isinf(min(D)))); %% prune points that don't connect to any landmarks +N = length(Y.index); +[tmp, landmarks, land_ind] = intersect(landmarks,Y.index); %% list of landmarks in component +nl = length(landmarks); +D = full(D(landmarks,Y.index))'; +%disp([' Number of connected components in graph: ' num2str(n_comps)]); +%disp([' Embedding component ' num2str(comp) ' with ' num2str(length(Y.index)) ' points.']); + + +opt.display = 0; +dims = unique(min(dims,nl-1)); %% don't embed in more dimensions than landmarks-1 +if (nl==N) + subB = center(D,N,N); + [vec, val] = eigs(subB, max(dims), 'LR', opt); +else + subB = center(D,N,nl); + [alpha,beta] = eigs(subB'*subB, max(dims), 'LR', opt); + val = beta.^(1/2); + vec = subB*alpha*inv(val); +end + +h = real(diag(val)); +[~,sorth] = sort(h); sorth = sorth(end:-1:1); +val = real(diag(val(sorth,sorth))); +vec = vec(:,sorth); + +D = reshape(D,N*nl,1); +for di = 1:length(dims) + Y.coords{di} = real(vec(:,1:dims(di)).*(ones(N,1)*sqrt(val(1:dims(di)))'))'; + % this actually computes matrixnorm of distance matrices (centered) + r2 = 1-corrcoef(reshape(real(L2_distance(Y.coords{di}, Y.coords{di}(:,land_ind))),N*nl,1),D).^2; + R(di) = r2(2,1); + % R(di) = norm(Y.coords{di}'*Y.coords{di} - subB, 'fro')^2 / norm(subB, 'fro')^2; + if (verbose == 1) + %disp([' Isomap on ' num2str(N) ' points with dimensionality ' num2str(dims(di)) ' --> residual variance = ' num2str(R(di))]); + end +end + + +%%%%%%%%%%%%%%%%%% Graphics %%%%%%%%%%%%%%%%%% +if (displ==1) + %%%%% Plot fall-off of residual variance with dimensionality %%%%% + figure; + hold on + plot(dims, R, 'bo'); + plot(dims, R, 'b-'); + hold off + ylabel('Residual variance'); + xlabel('Isomap dimensionality'); + + %%%%% Plot two-dimensional configuration %%%%% + twod = find(dims==2); + if ~isempty(twod) + figure; + hold on; + plot(Y.coords{twod}(1,:), Y.coords{twod}(2,:), 'ro'); + if (overlay == 1) + gplot(E(Y.index, Y.index), [Y.coords{twod}(1,:); Y.coords{twod}(2,:)]'); + title('Two-dimensional Isomap embedding (with neighborhood graph).'); + else + title('Two-dimensional Isomap.'); + end + hold off; + end +end + +return; \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/stress.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/stress.m new file mode 100644 index 00000000..6a009d9f --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap/stress.m @@ -0,0 +1,7 @@ +function S = stress(D, new_x, new_y) +%STRESS Computes the normalized stress + + D_new = pdist2([new_x(:) new_y(:)], [new_x(:) new_y(:)]); + S = sqrt(sum(sum(triu(D-D_new).^2)) / sum(sum(triu(D).^2))); +end + diff --git a/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap2.m b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap2.m new file mode 100644 index 00000000..f86993cc --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/fast_flatten_metric/isomap2.m @@ -0,0 +1,60 @@ +function [final_x, final_y,final_z, rv, D,EI] = isomap2(x, y, springs, neutral_lengths, w, plot3d) +% Runs underlying MDS to finish Isomap +% x, y, springs, and neutral_lengths are from springs computation +% plot3d tells whether or not to plot 3d manifold +% w is weighting factor + + % Use spring lengths to set up sparse distance matrix + n = numel(x); + [D, G] = graph_matrix(springs, neutral_lengths, n); + +% Call Isomap + options.dims = 1:3; + options.display = 0; + options.verbose = 0; + options.G = G; + [Y, R, E] = isomap_fast(D, 'k', 24, options); + + % Call MDS + weights = G~=0; + weights = w*(1+weights)-(w-1); +% [Y, R] = mdimscale(D, 1:3, 'stress', ones(size(weights))); + + % 3D Mesh plot + if plot3d == 1 + a = Y.coords{3,1}(1,:); + b = Y.coords{3,1}(2,:); + c = Y.coords{3,1}(3,:); + dx = .05; + dy = .05; + +% x_edge = [floor(min(a)):dx:ceil(max(a))]; +% y_edge = [floor(min(b)):dy:ceil(max(b))]; + + y_edge = linspace(min(a),max(a),23);%[(min(a)+0.01):dx:(max(a)-0.01)]; + x_edge = linspace(min(b),max(b),23);%[(min(b)+0.01):dy:(max(b)-0.01)]; + [A,B] = ndgrid(x_edge,y_edge); + C = griddata(a,b,c,A,B,'nearest'); + +% figure; +% % mesh(A,B,C); +% surf(A,B,C); +% axis equal; + end + + new_coords = Y.coords{3,1}; + final_y = reshape(-new_coords(1,:), size(x)); + final_x = reshape(-new_coords(2,:), size(y)); + final_z = reshape(new_coords(3,:), size(y)); + rv = R(2); + +% final_x = symmetry_vector(final_x); +% final_y = symmetry_vector(final_y); +% final_z = symmetry_vector(final_z); + + % export extra info + EI.A = A; + EI.B = B; + EI.C = C; + +end \ No newline at end of file diff --git a/ProgramFiles/sys_calcsystem_fcns/lle_sysplotter.m b/ProgramFiles/sys_calcsystem_fcns/lle_sysplotter.m new file mode 100644 index 00000000..bf9e67c7 --- /dev/null +++ b/ProgramFiles/sys_calcsystem_fcns/lle_sysplotter.m @@ -0,0 +1,133 @@ +function [mappedX, mapping] = lle_sysplotter(X, no_dims, distance, neighborhood, eig_impl) +%LLE Runs the locally linear embedding algorithm +% +% mappedX = lle(X, no_dims, k, eig_impl) +% +% Runs the local linear embedding algorithm on dataset X to reduces its +% dimensionality to no_dims. In the LLE algorithm, the number of neighbors +% can be specified by k. +% The function returns the embedded coordinates in mappedX. +% +% + +% This file is part of the Matlab Toolbox for Dimensionality Reduction. +% The toolbox can be obtained from http://homepage.tudelft.nl/19j49 +% You are free to use, change, or redistribute this code in any way you +% want for non-commercial purposes. However, it is appreciated if you +% maintain the name of the original author. +% +% (C) Laurens van der Maaten, Delft University of Technology + + + if ~exist('no_dims', 'var') + no_dims = 2; + end + if ~exist('k', 'var') + k = 12; + end + if ~exist('eig_impl', 'var') + eig_impl = 'Matlab'; + end + + % Get dimensionality and number of dimensions + [n, d] = size(X); + + % Compute pairwise distances and find nearest neighbors (vectorized implementation) + disp('Finding nearest neighbors...'); + [distance, neighborhood] = find_nn(X, k); + + % Identify largest connected component of the neighborhood graph + blocks = components(distance)'; + count = zeros(1, max(blocks)); + for i=1:max(blocks) + count(i) = length(find(blocks == i)); + end + [count, block_no] = max(count); + conn_comp = find(blocks == block_no); + + % Update the neighborhood relations + tmp = 1:n; + tmp = tmp(conn_comp); + new_ind = zeros(n, 1); + for i=1:n + ii = find(tmp == i); + if ~isempty(ii), new_ind(i) = ii; end + end + neighborhood = neighborhood(conn_comp,:)'; + for i=1:n + neighborhood(neighborhood == i) = new_ind(i); + end + n = numel(conn_comp); + X = X(conn_comp,:)'; + max_k = size(neighborhood, 1); + + % Find reconstruction weights for all points by solving the MSE problem + % of reconstructing a point from each neighbours. A used constraint is + % that the sum of the reconstruction weights for a point should be 1. + disp('Compute reconstruction weights...'); + if k > d + tol = 1e-5; + else + tol = 0; + end + + % Construct reconstruction weight matrix + W = zeros(max_k, n); + for i=1:n + nbhd = neighborhood(:,i); + nbhd = nbhd(nbhd ~= 0); + kt = numel(nbhd); + z = bsxfun(@minus, X(:,nbhd), X(:,i)); % Shift point to origin + C = z' * z; % Compute local covariance + C = C + eye(kt, kt) * tol * trace(C); % Regularization of covariance (if K > D) + wi = C \ ones(kt, 1); % Solve linear system + wi = wi / sum(wi); % Make sure that sum is 1 + W(:,i) = [wi; nan(max_k - kt, 1)]; + end + + % Now that we have the reconstruction weights matrix, we define the + % sparse cost matrix M = (I-W)'*(I-W). + M = sparse(1:n, 1:n, ones(1, n), n, n, 4 * max_k * n); + for i=1:n + w = W(:,i); + j = neighborhood(:,i); + indices = find(j ~= 0 & ~isnan(w)); + j = j(indices); + w = w(indices); + M(i, j) = M(i, j) - w'; + M(j, i) = M(j, i) - w; + M(j, j) = M(j, j) + w * w'; + end + + % For sparse datasets, we might end up with NaNs or Infs in M. We just set them to zero for now... + M(isnan(M)) = 0; + M(isinf(M)) = 0; + + % The embedding is computed from the bottom eigenvectors of this cost matrix + disp('Compute embedding (solve eigenproblem)...'); + tol = 0; + if strcmp(eig_impl, 'JDQR') + options.Disp = 0; + options.LSolver = 'bicgstab'; + [mappedX, eigenvals] = jdqr(M + eps * eye(n), no_dims + 1, tol, options); + else + options.disp = 0; + options.isreal = 1; + options.issym = 1; + [mappedX, eigenvals] = eigs(M + eps * eye(n), no_dims + 1, tol, options); % only need bottom (no_dims + 1) eigenvectors + end + [eigenvals, ind] = sort(diag(eigenvals), 'ascend'); + if size(mappedX, 2) < no_dims + 1 + no_dims = size(mappedX, 2) - 1; + warning(['Target dimensionality reduced to ' num2str(no_dims) '...']); + end + eigenvals = eigenvals(2:no_dims + 1); + mappedX = mappedX(:,ind(2:no_dims + 1)); % throw away zero eigenvector/value + + % Save information on the mapping + mapping.k = k; + mapping.X = X'; + mapping.vec = mappedX; + mapping.val = eigenvals; + mapping.conn_comp = conn_comp; + mapping.nbhd = distance; diff --git a/ProgramFiles/sys_calcsystem_fcns/test_function_type.m b/ProgramFiles/sys_calcsystem_fcns/test_function_type.m index 092a5fa9..c2c7f5b3 100755 --- a/ProgramFiles/sys_calcsystem_fcns/test_function_type.m +++ b/ProgramFiles/sys_calcsystem_fcns/test_function_type.m @@ -28,7 +28,7 @@ A_test_single = tensorfunction(range_mid{:}); % This is to make the system throw an error if tensorfunction is bad, because the try line on double_midpoint will treat an error as indicating a non-vector function try - A_test = tensorfunction(double_midpoint{:}); %#ok + A_test = tensorfunction(double_midpoint{:}); catch @@ -168,11 +168,3 @@ end - - - - - - - - diff --git a/ProgramFiles/sys_draw.m b/ProgramFiles/sys_draw.m index 98dd7b61..c793207c 100644 --- a/ProgramFiles/sys_draw.m +++ b/ProgramFiles/sys_draw.m @@ -1,8 +1,8 @@ -function plot_info = sys_draw(plot_structure,sys,shch,progress,update,resolution,handles) +function plot_info = sys_draw(plot_structure,sys,shch,stretch2,progress,update,resolution,handles) %make sure plot data file is up to date if update - sys_update(sys,shch,progress,handles) + sys_update(sys,shch,stretch2,progress,handles) set_plot_resolution_from_file(handles) %%%%%%%%%%%%%% @@ -13,29 +13,31 @@ resolution.scalar_range = get(handles.scalarresolution,'UserData'); end - - - % Get the setup configuration file - configfile = './sysplotter_config'; - load(configfile,'datapath') - - - %merge the system and shape change file names into one - plot_data_file = [sys '__' shch]; + % Get the setup configuration file + configfile = './sysplotter_config'; + load(configfile,'datapath') - %load the system and path data - load(fullfile(datapath, plot_data_file),'s','p') + %merge the system and shape change file names into one + plot_data_file = [sys '__' shch]; - %plot all plots called for - for i = 1:length(plot_structure) + %load the system and path data + load(fullfile(datapath, plot_data_file),'s','p') + + if s.conf_space == LieGroups.SE2 + %plot all plots called for + for i = 1:length(plot_structure) - %generate the handle of a specific plot command from the structure - eval(['plot_command = @' plot_structure(i).category '_draw;']); + %generate the handle of a specific plot command from the structure + eval(['plot_command = @' plot_structure(i).category '_draw;']); - %call that plot command - plot_info(i,1) = plot_command(s,p,plot_structure(i),sys,shch,resolution); + %call that plot command + plot_info(i,1) = plot_command(s,p,plot_structure(i),sys,shch,resolution); - end + end + else + warning('Native plotting not yet supported for non-SE2 position spaces'); + plot_info = []; + end %Show full progress bar waitbar2a(1,progress,'Finished plotting') diff --git a/ProgramFiles/sys_draw_fcns/CCF_draw.m b/ProgramFiles/sys_draw_fcns/CCF_draw.m old mode 100755 new mode 100644 index 7335d8f8..832a8d2d --- a/ProgramFiles/sys_draw_fcns/CCF_draw.m +++ b/ProgramFiles/sys_draw_fcns/CCF_draw.m @@ -117,7 +117,7 @@ ,cellfun(@(x,y) x-y,s.(['DA_optimized' CCF_addendum])... ,s.(['dA_optimized' CCF_addendum]),'UniformOutput',false)); - if n_dim == 2 + if n_dim == 2 h1aname = 'DA'; h1bname = 'dA'; @@ -161,16 +161,36 @@ % (multiply the constraint curvature function by the inverse of the jacobian's % determinant) + grid_orig = grid; % Save the original grid for contour-hacking if plot_info.stretch && (numel(s.grid.eval) == 2) - % Get the value by which to scale the constraint curvature function - ascale = arrayfun(@(x,y) 1/det(s.convert.jacobian(x,y)),grid{:}); + switch plot_info.stretch + + case 1 % This is the 2-d stretch + + % Get the value by which to scale the constraint curvature function + ascale = arrayfun(@(x,y) 1/det(s.convert.stretch.jacobian(x,y)),grid{:}); - % Apply the jacobian to the vectors - H = cellfun(@(x) x.*ascale,H,'UniformOutput',false); + % Apply the jacobian to the vectors + H = cellfun(@(x) x.*ascale,H,'UniformOutput',false); - % Convert the grid points to their new locations - [grid{:}] = s.convert.old_to_new_points(grid{:}); + % Convert the grid points to their new locations + [grid{:}] = s.convert.stretch.old_to_new_points(grid{:}); + + case 2 % This is embedding the surface in 3-d space + + % Get the value by which to scale the constraint curvature function + J_grid = arrayfun(@(x,y) s.convert.surface.jacobian(x,y),grid{:},'UniformOutput',false); + J_det = cellfun(@(J) norm(cross(J(:,1),J(:,2))),J_grid); + + % Apply the jacobian to the vectors + H = cellfun(@(x) x./J_det,H,'UniformOutput',false); + + % Convert the grid points to their new locations + [grid{:},grid_extra] = s.convert.surface.old_to_new_points(grid{:}); + + end + end @@ -187,265 +207,415 @@ %get which constraint curvature function to use function_number = strcmp(plot_info.components{i}, CCF_list); - switch plot_info.style - - case 'surface' - -% if s.singularity -% -% H{function_number}(singularity_location) = NaN; -% -% end - if n_dim==2 - %Plot the constraint curvature function - meshhandle = surf(ax,grid{:},H{function_number}); + + % Handle 2-d plots differently from 3-d plots + if n_dim == 2 + + % Handle metric surface plots differently from + % unstretched or metric stretched plots + if plot_info.stretch == 2 + + % Specific plotting commands based on the plot style + % selected + switch plot_info.style + + % "surface" plot with metric surface should use the + % surface from the metric, with color taken from the + % ccf + case 'surface' + + meshhandle = surf(ax,grid{:},grid_extra,H{function_number}); + colormap(Colorset.colormap); + shading(ax,'interp') + axis(ax,'equal') + axis(ax,'tight') + view(ax,3) + + % ideally, "contour" would allow us to overlay a + % contour plot of one function on the surface of + % another, but this isnt easy. code below uses + % undocumented matlab features + case 'contour' + + [~, meshhandle] = contour(ax,grid_orig{:},H{function_number},7,'linewidth',2,'Fill','on'); + drawnow + contours_on_surface(s.convert,meshhandle,[grid; {grid_extra}]) + addlistener(meshhandle, 'MarkedClean', @(src,evnt)contours_on_surface(s.convert,meshhandle,[grid; {grid_extra}])); + + colormap(Colorset.colormap_contour); + %shading(ax,'interp') + axis(ax,'equal') + axis(ax,'tight') + view(ax,3) + otherwise + + error('plot_info.style does not contain a valid option') + end + + % This "else" matches unstretched or 2-d stretched plots + else - if n_dim>2 -% curvinterest=H{function_number,:}; -% for j=1:length(curvinterest(:,1,1)) -% for k=1:length(curvinterest(1,:,1)) -% for l=1:length(curvinterest(1,1,:)) -% totalcurvvalue(j,k,l)=sqrt(H{function_number,1}(j,k,l)^2+H{function_number,2}(j,k,l)^2+H{function_number,3}(j,k,l)^2); -% end -% end + % Specific plotting commands based on the plot style + % selected + switch plot_info.style + + % "surface" plot with metric surface should use the + % surface from the metric, with color taken from the + % ccf + case 'surface' + + meshhandle = surf(ax,grid{:},H{function_number}); + colormap(Colorset.colormap); + axis(ax,'tight') + %shading(ax,'interp') + view(ax,3) + + case 'contour' + + [~, meshhandle] = contour(ax,grid{:},H{function_number},7,'linewidth',2); + axis(ax,'equal') + axis(ax,'tight') + + colormap(Colorset.colormap_contour); + otherwise + + error('plot_info.style does not contain a valid option') + + end + + end + + % This "else" catches the case where the shape space has more than + % two dimensions. The code figures out the plane intersecting the + % maximum-norm point on the ccf in the direction that most + % interacts with it; it badly needs commenting + else + + %switch plot_info.style + + %case 'surface' + +% % create an empty cell array with as many entries as +% % there are dimensions +% y=cell(1,n_dim); +% +% % populate this cell array with zeros +% y(:)={0}; +% +% % Build up a (squared) norm of the constraint curvature +% % (using for now the simple in-coordinates norm, could +% % introduce the metric later) +% Hnorm = zeros(size(H{1})); +% for idx_normbuild = 1:size(H,2) +% Hnorm = Hnorm+H{function_number,idx_normbuild}.^2; +% end +% +% % Get the maximum value, and the index of that value, +% % for the current CCF +% [~, Imax] = max(Hnorm(:)); +% [Imax_x,Imax_y,Imax_z] = ind2sub(size(H{function_number}),Imax); +% +% % Populate y with the location of that point +% for idx_maxpoint = 1:n_dim +% y{idx_maxpoint} = grid{idx_maxpoint}(Imax); +% end +% +% +% + % create a cell array with two fewer elements than + % there are dimensions + idxt=cell(1,n_dim-2); + + % populate this cell array with ones + idxt(1,:)={1}; +% +% % For every dimension in the problem, create a copy of +% % the grid? Not sure why this is named +% % "interpstatecurvature" +% for j=1:1:n_dim +% interpstatecurvature{j}=grid{j,1}; % end % -% [max1,ind1]=max(totalcurvvalue); -% [max2,ind2]=max(max1); -% [max3,ind3]=max(max2); +% % Get the value of the curvature at the point y +% for j=1:n_dim*(n_dim-1)/2 +% curvature(:,j)=interpn(interpstatecurvature{:},H{function_number,j},y{:},'cubic'); +% end +% +% % Create a skew-symmetric matrix describing the +% % orientation of the curvature form +% B=[0,curvature(1),curvature(2); +% -curvature(1),0,curvature(n_dim); +% -curvature(2),-curvature(n_dim),0]; % -% totalcurvvalue(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3); +% % Get the eigenvectors and values of the curvature +% % orientation +% [V,D]=eig(B); +% +% % Sort the eigenvalues and eigenvectors +% [d,ind] = sort(diag(D)); +% Ds = D(ind,ind); +% Vs = V(:,ind); +% % -% y=[grid{1}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3),grid{2}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3),grid{3}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3)]; +% % Get the real component of the eigenvector with the +% % largest eigenvalue +% X=real(Vs(:,end)); % - y=cell(1,n_dim); - y(:)={0}; - idxt=cell(1,n_dim-2); - idxt(1,:)={1}; - for j=1:1:n_dim - interpstatecurvature{j}=grid{j,1}; - end - - for j=1:n_dim*(n_dim-1)/2 - curvature(:,j)=interpn(interpstatecurvature{:},H{function_number,j},y{:},'cubic'); - end - - B=[0,curvature(1),curvature(2);-curvature(1),0,curvature(n_dim);-curvature(2),-curvature(n_dim),0]; - - [V,D]=eig(B); - [d,ind] = sort(diag(D)); - Ds = D(ind,ind); - Vs = V(:,ind); - - - X=real(Vs(:,end)); - Y=(Vs(:,end)-X)/(sqrt(-1)); - - Xnorm=X/(norm(X)); - Ynorm=Y/(norm(Y)); +% % Get a vector that is right-hand-positive orthogonal +% % to the preceding vector +% Y=(Vs(:,end)-X)/(sqrt(-1)); +% +% % Normalize the X and Y vectors (to make up for the +% % fact that they lost length when mapped to real values +% Xnorm=X/(norm(X)); +% Ynorm=Y/(norm(Y)); % normal=cross(Xnorm,Ynorm); % y=zeros(1,n_dim) % projnorm=y*normal; - + pointonplane=zeros(1,n_dim);%y'-(y'-projnorm*normal); +% % Test code +% Xnorm = [1;0;0]; +% Ynorm = [0; 0; 1]; +% y = {0 0 0}; + + [maxpoint, maxplane, Imax_x, Imax_y, Imax_z,interpstatecurvature] = CCF_maxpoint(H(function_number,:),grid); + Xnorm = maxplane(:,1); + Ynorm = maxplane(:,2); + + % Pull out a 2d x and y grid Xtemp=grid{1,1}(:,:,idxt{:}); Ytemp=grid{2,1}(:,:,idxt{:}); - arsize=length(grid{1,1}(:,1)); + %arsize=length(grid{1,1}(:,1)); idxt2=cell(1,n_dim-3); idxt2(1,:)={0}; - for m=1:1:arsize - for j=1:1:arsize - xgrid(m,j)=Xtemp(m,j)*Xnorm(1)+Ytemp(m,j)*Ynorm(1)+pointonplane(1); - ygrid(m,j)=Xtemp(m,j)*Xnorm(2)+Ytemp(m,j)*Ynorm(2)+pointonplane(2); - zgrid(m,j)=Xtemp(m,j)*Xnorm(3)+Ytemp(m,j)*Ynorm(3)+pointonplane(3); + + % Loop over points on the x y grid + zgrid_at_max = Xtemp(Imax_x,Imax_y)*Xnorm(3)+Ytemp(Imax_x,Imax_y)*Ynorm(3); + for m=1:1:size(Xtemp,1) + for j=1:1:size(Xtemp,2) + + % Rotate the grid so that it lines up with + % the curvature two-form + xgrid(m,j)=Xtemp(m,j)*Xnorm(1)+Ytemp(m,j)*Ynorm(1);%+pointonplane(1); + ygrid(m,j)=Xtemp(m,j)*Xnorm(2)+Ytemp(m,j)*Ynorm(2);%+pointonplane(2); + zgrid(m,j)=Xtemp(m,j)*Xnorm(3)+Ytemp(m,j)*Ynorm(3);%+pointonplane(3); + +% % Raise or lower the grid so that the +% % maxpoint is on the surface +% zshift = maxpoint{3}-zgrid_at_max;%(zgrid_at_max-y{3}); +% zgrid(m,j)=zgrid(m,j) + zshift; + + end + + end + + pointdist = (xgrid-maxpoint{1}).^2 + (ygrid-maxpoint{2}).^2 + (zgrid-maxpoint{3}).^2; + [~,mindistI] = min(pointdist,[],'all','linear'); + + planeshift = [maxpoint{1} - xgrid(mindistI); maxpoint{2} - ygrid(mindistI); maxpoint{3} - zgrid(mindistI)]; + + xgrid = xgrid+planeshift(1); + ygrid = ygrid+planeshift(2); + zgrid = zgrid+planeshift(3); + + for m=1:1:size(Xtemp,1) + for j=1:1:size(Xtemp,2) + for k=1:2 - curvaturetemp(:,k)=interpn(interpstatecurvature{:},H{function_number,k},xgrid(m,j),ygrid(m,j),zgrid(m,j),idxt2{:},'spline'); + curvaturetemp(:,k)=interpn(... + interpstatecurvature{:},... + H{function_number,k},... + xgrid(m,j),ygrid(m,j),zgrid(m,j),... + idxt2{:},... + 'spline'); end - curvaturetemp(:,3)=interpn(interpstatecurvature{:},H{function_number,n_dim},xgrid(m,j),ygrid(m,j),zgrid(m,j),idxt2{:},'spline'); + curvaturetemp(:,3)=interpn(interpstatecurvature{:},H{function_number,n_dim},xgrid(m,j),ygrid(m,j),zgrid(m,j),idxt2{:},... + 'spline'); curvatureproj(m,j)=curvaturetemp(1)*(Xnorm(1)*Ynorm(2)-Ynorm(1)*Xnorm(2))+curvaturetemp(2)*(Xnorm(1)*Ynorm(3)-Ynorm(1)*Xnorm(3))+curvaturetemp(3)*(Xnorm(2)*Ynorm(3)-Ynorm(2)*Xnorm(3)); end end - + meshhandle=pcolor(xgrid,ygrid,-curvatureproj,'Parent',ax); meshhandle.ZData=zgrid; + hold on - + colormap(Colorset.colormap); shading(ax,'interp') view(ax,3) - - end - - - %If there's a shape change involved, plot it - if ~strcmp(shch,'null') - if n_dim==2 - overlay_shape_change_3d_surf(ax,p,zdata{function_number,:},plot_info.stretch,s.convert,true); - end - if n_dim>2 - meshhandle.FaceAlpha=0.9; - line('Parent',ax,'XData',p.phi_locus_full{i}.shape(:,1),'YData',p.phi_locus_full{i}.shape(:,2),'ZData',p.phi_locus_full{i}.shape(:,3),'Color',Colorset.spot,'LineWidth',6,'parent',ax); - end - end - - %square axes - axis(ax,'tight') - - - %Put an outline box around the plot - nicebox(ax,'on') - - %load the color map - coloration = Colorset.colormap; - if s.singularity - coloration = coloration.^5; - end - - - case 'contour' - if n_dim==2 - %Plot the constraint curvature function - [junk, meshhandle] = contour(ax,grid{:},H{function_number},7,'linewidth',2); - end - - if n_dim>2 +% case 'contour' +% +% y=cell(1,n_dim); +% y(:)={0}; +% idxt=cell(1,n_dim-2); +% idxt(1,:)={1}; +% for j=1:1:n_dim +% interpstatecurvature{j}=grid{j,1}; +% end +% +% for j=1:n_dim*(n_dim-1)/2 +% curvature(:,j)=interpn(interpstatecurvature{:},H{function_number,j},y{:},'cubic'); +% end +% +% B=[0,curvature(1),curvature(2);-curvature(1),0,curvature(n_dim);-curvature(2),-curvature(n_dim),0]; +% +% [V,D]=eig(B); +% [d,ind] = sort(diag(D)); +% Ds = D(ind,ind); +% Vs = V(:,ind); +% +% +% X=real(Vs(:,end)); +% Y=(Vs(:,end)-X)/(sqrt(-1)); % -% curvinterest=H{function_number,:}; +% Xnorm=X/(norm(X)); +% Ynorm=Y/(norm(Y)); % -% for j=1:length(curvinterest(:,1,1)) -% for k=1:length(curvinterest(1,:,1)) -% for l=1:length(curvinterest(1,1,:)) -% totalcurvvalue(j,k,l)=sqrt(H{function_number,1}(j,k,l)^2+H{function_number,2}(j,k,l)^2+H{function_number,3}(j,k,l)^2); +% Xtemp=grid{1,1}(:,:,idxt{:}); +% Ytemp=grid{2,1}(:,:,idxt{:}); +% +% arsize=length(grid{1,1}(:,1)); +% idxt2=cell(1,n_dim-3); +% idxt2(1,:)={ceil(arsize/2)}; +% idxt3=cell(1,n_dim-3); +% idxt3(1,:)={0}; +% for m=1:1:arsize +% for j=1:1:arsize +% xgrid(m,j)=Xtemp(m,j)*Xnorm(1)+Ytemp(m,j)*Ynorm(1); +% ygrid(m,j)=Xtemp(m,j)*Xnorm(2)+Ytemp(m,j)*Ynorm(2); +% zgrid(m,j)=Xtemp(m,j)*Xnorm(3)+Ytemp(m,j)*Ynorm(3); +% end +% end +% +% +% for idx1=1:length(grid{1,1}(:,1)) +% for idx2=1:length(grid{2,1}(:,1)) +% for idx3=1:length(grid{3,1}(:,1)) +% for k=1:n_dim*(n_dim-1)/2 +% curvaturetemp(:,k)=interpn(interpstatecurvature{:},H{function_number,k},grid{2,1}(idx1,idx2,idx3,idxt2{:}),grid{1,1}(idx1,idx2,idx3,idxt2{:}),grid{3,1}(idx1,idx2,idx3,idxt2{:}),idxt3{:},'cubic'); +% end +% curvatureproj(idx1,idx2,idx3)=curvaturetemp(1)*(Xnorm(1)*Ynorm(2)-Ynorm(1)*Xnorm(2))+curvaturetemp(2)*(Xnorm(1)*Ynorm(3)-Ynorm(1)*Xnorm(3))+curvaturetemp(n_dim)*(Xnorm(2)*Ynorm(3)-Ynorm(2)*Xnorm(3)); % end % end % end +% % -% [max1,ind1]=max(totalcurvvalue); -% [max2,ind2]=max(max1); -% [max3,ind3]=max(max2); +% meshhandle=contourslice(grid{2,1}(:,:,:,idxt2{:}),grid{1,1}(:,:,:,idxt2{:}),grid{3,1}(:,:,:,idxt2{:}),curvatureproj,xgrid,ygrid,zgrid,'parent',ax); +% view(ax,3) % -% totalcurvvalue(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3) +% hold on % -% y=[grid{1}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3),grid{2}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3),grid{3}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3)]; -% - y=cell(1,n_dim); - y(:)={0}; - idxt=cell(1,n_dim-2); - idxt(1,:)={1}; - for j=1:1:n_dim - interpstatecurvature{j}=grid{j,1}; - end +% colormap(Colorset.colormap_contour); +% view(ax,3) +% end - for j=1:n_dim*(n_dim-1)/2 - curvature(:,j)=interpn(interpstatecurvature{:},H{function_number,j},y{:},'cubic'); - end - - B=[0,curvature(1),curvature(2);-curvature(1),0,curvature(n_dim);-curvature(2),-curvature(n_dim),0]; - [V,D]=eig(B); - [d,ind] = sort(diag(D)); - Ds = D(ind,ind); - Vs = V(:,ind); - - - X=real(Vs(:,end)); - Y=(Vs(:,end)-X)/(sqrt(-1)); - - Xnorm=X/(norm(X)); - Ynorm=Y/(norm(Y)); + end + - Xtemp=grid{1,1}(:,:,idxt{:}); - Ytemp=grid{2,1}(:,:,idxt{:}); - - arsize=length(grid{1,1}(:,1)); - idxt2=cell(1,n_dim-3); - idxt2(1,:)={ceil(arsize/2)}; - idxt3=cell(1,n_dim-3); - idxt3(1,:)={0}; - for m=1:1:arsize - for j=1:1:arsize - xgrid(m,j)=Xtemp(m,j)*Xnorm(1)+Ytemp(m,j)*Ynorm(1); - ygrid(m,j)=Xtemp(m,j)*Xnorm(2)+Ytemp(m,j)*Ynorm(2); - zgrid(m,j)=Xtemp(m,j)*Xnorm(3)+Ytemp(m,j)*Ynorm(3); + + + %If there's a shape change involved, plot it + if ~strcmp(shch,'null') + if n_dim==2 + switch plot_info.stretch + case {0,1} % No stretch or 2-d stretch + switch plot_info.style + case 'contour' + overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); + case 'surface' + overlay_shape_change_3d_surf(ax,p,zdata{function_number,:},plot_info.stretch,s.convert,true); end - end + + case 2 % Surface-embedded stretch + + overlay_shape_change_metricsurf(ax,p,s.convert.surface.old_to_new_points,Colorset) + %overlay_shape_change_3d_surf(ax,p,grid_extra,plot_info.stretch,s.convert,false) + %overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); + end + + end + if n_dim>2 + if strcmp(plot_info.style,'surface') + meshhandle.FaceAlpha=0.9; + end + line('Parent',ax,'XData',p.phi_locus_full{i}.shape(:,1),'YData',p.phi_locus_full{i}.shape(:,2),'ZData',p.phi_locus_full{i}.shape(:,3),'Color',Colorset.spot,'LineWidth',6,'parent',ax); + end + end + %tight axes + axis(ax,'tight') - for idx1=1:length(grid{1,1}(:,1)) - for idx2=1:length(grid{2,1}(:,1)) - for idx3=1:length(grid{3,1}(:,1)) - for k=1:n_dim*(n_dim-1)/2 - curvaturetemp(:,k)=interpn(interpstatecurvature{:},H{function_number,k},grid{2,1}(idx1,idx2,idx3,idxt2{:}),grid{1,1}(idx1,idx2,idx3,idxt2{:}),grid{3,1}(idx1,idx2,idx3,idxt2{:}),idxt3{:},'cubic'); - end - curvatureproj(idx1,idx2,idx3)=curvaturetemp(1)*(Xnorm(1)*Ynorm(2)-Ynorm(1)*Xnorm(2))+curvaturetemp(2)*(Xnorm(1)*Ynorm(3)-Ynorm(1)*Xnorm(3))+curvaturetemp(n_dim)*(Xnorm(2)*Ynorm(3)-Ynorm(2)*Xnorm(3)); - end - end - end - - meshhandle=contourslice(grid{2,1}(:,:,:,idxt2{:}),grid{1,1}(:,:,:,idxt2{:}),grid{3,1}(:,:,:,idxt2{:}),curvatureproj,xgrid,ygrid,zgrid,'parent',ax); - view(ax,3) + %Put an outline box around the plot + box(ax,'on') + set(ax,'BoxStyle','back'); + % Make edges if coordinates have changed + if plot_info.stretch && (numel(s.grid.eval) == 2) %&& (strcmp(plot_info.style,'contour')) + + edgeres = 30; + + oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... + s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; + oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... + linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; + + switch plot_info.stretch + + case 1 % 2-d stretch - end - - - %If there's a shape change involved, plot it - if ~strcmp(shch,'null') - if n_dim==2 - overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); - end - if n_dim>2 - line('Parent',ax,'XData',p.phi_locus_full{i}.shape(:,1),'YData',p.phi_locus_full{i}.shape(:,2),'ZData',p.phi_locus_full{i}.shape(:,3),'Color',Colorset.spot,'LineWidth',6,'parent',ax); + if ~strcmp(plot_info.style,'surface') + + [x_edge,y_edge] = s.convert.stretch.old_to_new_points(oldx_edge,oldy_edge); + + l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); + end - - end - - - % Make edges if coordinates have changed - if plot_info.stretch && (numel(s.grid.eval) == 2) - - edgeres = 30; - - oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... - s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; - oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... - linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; - [x_edge,y_edge] = s.convert.old_to_new_points(oldx_edge,oldy_edge); - - l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); - - end - - %Put an outline box around the plot - box(ax,'on') + case 2 % 3-d surface embedding + + [x_edge,y_edge,z_edge] = s.convert.surface.old_to_new_points(oldx_edge,oldy_edge); - %equal axes sized to match grid or new dimensions if - %stretched - - if plot_info.stretch && (numel(s.grid.eval) == 2) - axis(ax,'equal'); - axis(ax,[min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:))]); - else - axis(ax,'equal','tight'); - end + l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Zdata',z_edge,'Color','k','LineWidth',1); + end + + elseif numel(s.grid.eval) > 2 + + edgeres = 30; + + oldy_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... + s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; + oldx_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... + linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; + + x_edge = oldx_edge * Xnorm(1) + oldy_edge * Ynorm(1) +planeshift(1); + y_edge = oldx_edge * Xnorm(2) + oldy_edge * Ynorm(2) +planeshift(2); + z_edge = (oldx_edge * Xnorm(3)) + (oldy_edge * Ynorm(3)) +planeshift(3); + + l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Zdata',z_edge,'Color',[.5 .5 .5],'LineWidth',1); + end - %set the color map - coloration = Colorset.colormap_contour; +% %equal axes sized to match grid or new dimensions if +% %stretched +% +% if plot_info.stretch && (numel(s.grid.eval) == 2) +% axis(ax,'equal'); +% axis(ax,[min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:))]); +% else +% axis(ax,'equal','tight'); +% end - otherwise - - error('Unknown plot style for the constraint curvature function') - end + %Iterate up the tree to find the figure that contains the current %axis @@ -464,7 +634,7 @@ end - set(parH,'Colormap',coloration); + %set(parH,'Colormap',coloration); %center the color map around zero Clim = get(ax,'Clim'); %get the current color limits @@ -498,8 +668,8 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot, and set the button %down callback for the mesh to the same - set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); - set(meshhandle,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch}); + set(meshhandle,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch}); else diff --git a/ProgramFiles/sys_draw_fcns/CCF_draw_old.m b/ProgramFiles/sys_draw_fcns/CCF_draw_old.m new file mode 100755 index 00000000..139498fb --- /dev/null +++ b/ProgramFiles/sys_draw_fcns/CCF_draw_old.m @@ -0,0 +1,617 @@ +function plot_info = CCF_draw_old(s,p,plot_info,sys,shch,resolution) +%Draw the constraint curvature function + + %Get the configuration file, and extract the Colorpath + configfile = 'sysplotter_config'; + configfile = fullfile(fileparts(mfilename('fullpath')),'..',configfile); + load(configfile,'Colorset'); + + %constraint curvature function list + CCF_list = {'X','Y','T','Xopt','Yopt','Topt'}; + + %Ensure that there are figure axes to plot into, and create new windows + %for those axes if necessary + plot_info = ensure_figure_axes(plot_info); + + % Get the number of shape dimensions + n_dim = numel(s.grid.eval); + + % get the number of position dimensions + n_g = numel(s.DA); + + %if there is a singularity, deal with it for display + if s.singularity + + CCF_addendum = '_scaled'; + + singularity_location = logical(sum(cat(3,s.vecfield.eval.singularities{:}),3)); + %singularity_location = imdilate(singularity_location,[0 1 0;1 1 1;0 1 0]); + + %make the ztext reflect the arctangent scaling + ztext = 'arctan(H)'; + + else + + CCF_addendum = ''; + + ztext = 'H'; + + end + + + %%%%% + %constraint curvature function drawing + + %Extract the constraint curvature function + + %choose which constraint curvature function to plot + switch plot_info.CCFtype + + case 'DA' + + H = cat(1,s.(['DA' CCF_addendum])... + ,s.(['DA_optimized' CCF_addendum])); + + if n_dim == 2 + + h1name = 'DA'; + h2name = 'DA_optimized'; + + h1 = cell(n_g,1); + h2 = cell(n_g,1); + + if ~strcmp(shch,'null') + + for i = 1:numel(p.phi_locus) + + for j = 1:numel(p.phi_locus_full{i}.dA) + + h1{j}{i} = p.phi_locus_full{i}.(h1name){j}; + h2{j}{i} = p.phi_locus_full{i}.(h2name){j}; + + end + + + end + + zdata = cat(1,h1,h2); + end + + + end + + case 'dA' + + H = cat(1,s.(['dA' CCF_addendum])... + ,s.(['dA_optimized' CCF_addendum])); + + if n_dim == 2 + + h1name = 'dA'; + h2name = 'dA_optimized'; + + h1 = cell(n_g,1); + h2 = cell(n_g,1); + + if ~strcmp(shch,'null') + for i = 1:numel(p.phi_locus) + + for j = 1:numel(p.phi_locus_full{i}.dA) + + h1{j}{i} = p.phi_locus_full{i}.(h1name){j}; + h2{j}{i} = p.phi_locus_full{i}.(h2name){j}; + + end + + end + + zdata = cat(1,h1,h2); + end + + end + + case 'LA' + + H = cat(1,cellfun(@(x,y) x-y,s.(['DA' CCF_addendum])... + ,s.(['dA' CCF_addendum]),'UniformOutput',false)... + ,cellfun(@(x,y) x-y,s.(['DA_optimized' CCF_addendum])... + ,s.(['dA_optimized' CCF_addendum]),'UniformOutput',false)); + + if n_dim == 2 + + h1aname = 'DA'; + h1bname = 'dA'; + h2aname = 'DA_optimized'; + h2bname = 'dA_optimized'; + + h1 = cell(n_g,1); + h2 = cell(n_g,1); + + if ~strcmp(shch,'null') + for i = 1:numel(p.phi_locus) + + for j = 1:numel(p.phi_locus_full{i}.dA) + + h1{j}{i} = p.phi_locus_full{i}.(h1aname){j}-p.phi_locus_full{i}.(h1bname){j}; + h2{j}{i} = p.phi_locus_full{i}.(h2aname){j}-p.phi_locus_full{i}.(h2bname){j}; + + end + + end + + zdata = cat(1,h1,h2); + end + + end + otherwise + + error('Unknown CCF function type') + + end + + %Extract the plotting grid + grid = s.grid.eval; + + %% + % Convert the function to the plotting grid specified in the gui + [H,grid] = plotting_interp(H,grid,resolution,'scalar'); + + %%%%% + % If the shape coordinates should be transformed, make the conversion + % (multiply the constraint curvature function by the inverse of the jacobian's + % determinant) + + if plot_info.stretch && (numel(s.grid.eval) == 2) + + if plot_info.stretch==1 + % Get the value by which to scale the constraint curvature function + ascale = arrayfun(@(x,y) 1/det(s.convert.stretch.jacobian(x,y)),grid{:}); + + % Apply the jacobian to the vectors + H = cellfun(@(x) x.*ascale,H,'UniformOutput',false); + + % Convert the grid points to their new locations + [grid{:}] = s.convert.stretch.old_to_new_points(grid{:}); + end + + end + + + + + + %Make the plots + for i = 1:length(plot_info.axes) + + %call up the relevant axis + ax =plot_info.axes(i); + + %get which constraint curvature function to use + function_number = strcmp(plot_info.components{i}, CCF_list); + + switch plot_info.style + + case 'surface' + +% if s.singularity +% +% H{function_number}(singularity_location) = NaN; +% +% end + if n_dim==2 + %Plot the constraint curvature function + meshhandle = surf(ax,grid{:},H{function_number}); + end + + if n_dim>2 +% curvinterest=H{function_number,:}; +% for j=1:length(curvinterest(:,1,1)) +% for k=1:length(curvinterest(1,:,1)) +% for l=1:length(curvinterest(1,1,:)) +% totalcurvvalue(j,k,l)=sqrt(H{function_number,1}(j,k,l)^2+H{function_number,2}(j,k,l)^2+H{function_number,3}(j,k,l)^2); +% end +% end +% end +% +% [max1,ind1]=max(totalcurvvalue); +% [max2,ind2]=max(max1); +% [max3,ind3]=max(max2); +% +% totalcurvvalue(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3); +% +% y=[grid{1}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3),grid{2}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3),grid{3}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3)]; +% + y=cell(1,n_dim); + y(:)={0}; + idxt=cell(1,n_dim-2); + idxt(1,:)={1}; + for j=1:1:n_dim + interpstatecurvature{j}=grid{j,1}; + end + + for j=1:n_dim*(n_dim-1)/2 + curvature(:,j)=interpn(interpstatecurvature{:},H{function_number,j},y{:},'cubic'); + end + + B=[0,curvature(1),curvature(2);-curvature(1),0,curvature(n_dim);-curvature(2),-curvature(n_dim),0]; + + [V,D]=eig(B); + [d,ind] = sort(diag(D)); + Ds = D(ind,ind); + Vs = V(:,ind); + + + X=real(Vs(:,end)); + Y=(Vs(:,end)-X)/(sqrt(-1)); + + Xnorm=X/(norm(X)); + Ynorm=Y/(norm(Y)); + +% normal=cross(Xnorm,Ynorm); +% y=zeros(1,n_dim) +% projnorm=y*normal; + + pointonplane=zeros(1,n_dim);%y'-(y'-projnorm*normal); + + Xtemp=grid{1,1}(:,:,idxt{:}); + Ytemp=grid{2,1}(:,:,idxt{:}); + + arsize=length(grid{1,1}(:,1)); + idxt2=cell(1,n_dim-3); + idxt2(1,:)={0}; + for m=1:1:arsize + for j=1:1:arsize + xgrid(m,j)=Xtemp(m,j)*Xnorm(1)+Ytemp(m,j)*Ynorm(1)+pointonplane(1); + ygrid(m,j)=Xtemp(m,j)*Xnorm(2)+Ytemp(m,j)*Ynorm(2)+pointonplane(2); + zgrid(m,j)=Xtemp(m,j)*Xnorm(3)+Ytemp(m,j)*Ynorm(3)+pointonplane(3); + for k=1:2 + curvaturetemp(:,k)=interpn(interpstatecurvature{:},H{function_number,k},xgrid(m,j),ygrid(m,j),zgrid(m,j),idxt2{:},'spline'); + end + curvaturetemp(:,3)=interpn(interpstatecurvature{:},H{function_number,n_dim},xgrid(m,j),ygrid(m,j),zgrid(m,j),idxt2{:},'spline'); + curvatureproj(m,j)=curvaturetemp(1)*(Xnorm(1)*Ynorm(2)-Ynorm(1)*Xnorm(2))+curvaturetemp(2)*(Xnorm(1)*Ynorm(3)-Ynorm(1)*Xnorm(3))+curvaturetemp(3)*(Xnorm(2)*Ynorm(3)-Ynorm(2)*Xnorm(3)); + end + end + + meshhandle=pcolor(xgrid,ygrid,-curvatureproj,'Parent',ax); + meshhandle.ZData=zgrid; + + hold on + + colormap(Colorset.colormap); + shading(ax,'interp') + view(ax,3) + + + end + + + %If there's a shape change involved, plot it + if ~strcmp(shch,'null') + if n_dim==2 + overlay_shape_change_3d_surf(ax,p,zdata{function_number,:},plot_info.stretch,s.convert,true); + end + if n_dim>2 + meshhandle.FaceAlpha=0.9; + line('Parent',ax,'XData',p.phi_locus_full{i}.shape(:,1),'YData',p.phi_locus_full{i}.shape(:,2),'ZData',p.phi_locus_full{i}.shape(:,3),'Color',Colorset.spot,'LineWidth',6,'parent',ax); + end + end + + %square axes + axis(ax,'tight') + + + %Put an outline box around the plot + nicebox(ax,'on') + + %load the color map + coloration = Colorset.colormap; + if s.singularity + coloration = coloration.^5; + end + + + case 'contour' + if n_dim==2 + %Plot the constraint curvature function + [junk, meshhandle] = contour(ax,grid{:},H{function_number},7,'linewidth',2); + end + + if n_dim>2 +% +% curvinterest=H{function_number,:}; +% +% for j=1:length(curvinterest(:,1,1)) +% for k=1:length(curvinterest(1,:,1)) +% for l=1:length(curvinterest(1,1,:)) +% totalcurvvalue(j,k,l)=sqrt(H{function_number,1}(j,k,l)^2+H{function_number,2}(j,k,l)^2+H{function_number,3}(j,k,l)^2); +% end +% end +% end +% +% [max1,ind1]=max(totalcurvvalue); +% [max2,ind2]=max(max1); +% [max3,ind3]=max(max2); +% +% totalcurvvalue(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3) +% +% y=[grid{1}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3),grid{2}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3),grid{3}(ind1(1,ind2(1,1,ind3),ind3),ind2(1,1,ind3),ind3)]; +% + y=cell(1,n_dim); + y(:)={0}; + idxt=cell(1,n_dim-2); + idxt(1,:)={1}; + for j=1:1:n_dim + interpstatecurvature{j}=grid{j,1}; + end + + for j=1:n_dim*(n_dim-1)/2 + curvature(:,j)=interpn(interpstatecurvature{:},H{function_number,j},y{:},'cubic'); + end + + B=[0,curvature(1),curvature(2);-curvature(1),0,curvature(n_dim);-curvature(2),-curvature(n_dim),0]; + + [V,D]=eig(B); + [d,ind] = sort(diag(D)); + Ds = D(ind,ind); + Vs = V(:,ind); + + + X=real(Vs(:,end)); + Y=(Vs(:,end)-X)/(sqrt(-1)); + + Xnorm=X/(norm(X)); + Ynorm=Y/(norm(Y)); + + Xtemp=grid{1,1}(:,:,idxt{:}); + Ytemp=grid{2,1}(:,:,idxt{:}); + + arsize=length(grid{1,1}(:,1)); + idxt2=cell(1,n_dim-3); + idxt2(1,:)={ceil(arsize/2)}; + idxt3=cell(1,n_dim-3); + idxt3(1,:)={0}; + for m=1:1:arsize + for j=1:1:arsize + xgrid(m,j)=Xtemp(m,j)*Xnorm(1)+Ytemp(m,j)*Ynorm(1); + ygrid(m,j)=Xtemp(m,j)*Xnorm(2)+Ytemp(m,j)*Ynorm(2); + zgrid(m,j)=Xtemp(m,j)*Xnorm(3)+Ytemp(m,j)*Ynorm(3); + end + end + + + for idx1=1:length(grid{1,1}(:,1)) + for idx2=1:length(grid{2,1}(:,1)) + for idx3=1:length(grid{3,1}(:,1)) + for k=1:n_dim*(n_dim-1)/2 + curvaturetemp(:,k)=interpn(interpstatecurvature{:},H{function_number,k},grid{2,1}(idx1,idx2,idx3,idxt2{:}),grid{1,1}(idx1,idx2,idx3,idxt2{:}),grid{3,1}(idx1,idx2,idx3,idxt2{:}),idxt3{:},'cubic'); + end + curvatureproj(idx1,idx2,idx3)=curvaturetemp(1)*(Xnorm(1)*Ynorm(2)-Ynorm(1)*Xnorm(2))+curvaturetemp(2)*(Xnorm(1)*Ynorm(3)-Ynorm(1)*Xnorm(3))+curvaturetemp(n_dim)*(Xnorm(2)*Ynorm(3)-Ynorm(2)*Xnorm(3)); + end + end + end + + + meshhandle=contourslice(grid{2,1}(:,:,:,idxt2{:}),grid{1,1}(:,:,:,idxt2{:}),grid{3,1}(:,:,:,idxt2{:}),curvatureproj,xgrid,ygrid,zgrid,'parent',ax); + view(ax,3) + + + end + + + %If there's a shape change involved, plot it + if ~strcmp(shch,'null') + if n_dim==2 + overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); + end + if n_dim>2 + line('Parent',ax,'XData',p.phi_locus_full{i}.shape(:,1),'YData',p.phi_locus_full{i}.shape(:,2),'ZData',p.phi_locus_full{i}.shape(:,3),'Color',Colorset.spot,'LineWidth',6,'parent',ax); + end + + end + + + % Make edges if coordinates have changed + if plot_info.stretch && (numel(s.grid.eval) == 2) + + edgeres = 30; + + oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... + s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; + oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... + linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; + + [x_edge,y_edge] = s.convert.stretch.old_to_new_points(oldx_edge,oldy_edge); + + l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); + + end + + %Put an outline box around the plot + box(ax,'on') + + %equal axes sized to match grid or new dimensions if + %stretched + + if plot_info.stretch && (numel(s.grid.eval) == 2) + axis(ax,'equal'); + axis(ax,[min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:))]); + else + axis(ax,'equal','tight'); + end + + %set the color map + coloration = Colorset.colormap_contour; + + + case 'pcolor' + + %Plot the constraint curvature function +% [junk, meshhandle] = contour(ax,grid{:},H{function_number},7,'linewidth',2); + + + + [x_new, y_new] = s.convert.surface.old_to_new_points(grid{1},grid{2}); + +% C = (s.convert.EI.C); +% C = reshape(smooth(C(:)),[],12); + + HF_isomap = griddata(s.convert.surface.EI.A,s.convert.surface.EI.B,s.convert.surface.EI.C,x_new, y_new,'cubic'); + + isomap.x_new = x_new; + isomap.y_new = y_new; + isomap.HF_isomap = HF_isomap; + + %Plot the constraint curvature function +% [junk, meshhandle] = contour(ax,grid{:},H{function_number},7,'linewidth',2); + meshhandle = pcolor(ax,grid{1},grid{2},H{function_number}); + + meshhandle.ZData = HF_isomap; + meshhandle.XData = x_new; + meshhandle.YData = y_new; + + if plot_info.stretch==2 + [grid{:}] = s.convert.surface.old_to_new_points(grid{:}); + end + + if plot_info.stretch==1 + [grid{:}] = s.convert.stretch.old_to_new_points(grid{:}); + end + %If there's a shape change involved, plot it + if ~strcmp(shch,'null') + + overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert,isomap,s); + + end + + + % Make edges if coordinates have changed + if plot_info.stretch + + edgeres = 30; + + oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... + s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; + oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... + linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; + + if plot_info.stretch==2 + [x_edge,y_edge] = s.convert.surface.old_to_new_points(oldx_edge,oldy_edge); + end + + if plot_info.stretch==1 + [x_edge,y_edge] = s.convert.stretch.old_to_new_points(oldx_edge,oldy_edge); + end + + + HF_isomap_edge = griddata(s.convert.surface.EI.A,s.convert.surface.EI.B,s.convert.surface.EI.C,x_edge,y_edge); + + l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'ZData',HF_isomap_edge,'Color','k','LineWidth',1); + +% plot3(x_edge,y_edge,HF_isomap_edge,'k','Parent',ax,'LineWidth',1) + + end + + %Put an outline box around the plot + box(ax,'on') + + %equal axes sized to match grid or new dimensions if + %stretched + + if plot_info.stretch + axis(ax,'equal'); + axis(ax,[min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:))]); + else + axis(ax,'equal','tight'); + end + + %set the color map + coloration = Colorset.colormap; + + otherwise + + error('Unknown plot style for the constraint curvature function') + + end + + %Iterate up the tree to find the figure that contains the current + %axis + parH = get(ax,'Parent'); + while 1 + + if strmatch('figure',get(parH,'Type')) + + break + + else + + parH = get(parH,'Parent'); + + end + + end + + set(parH,'Colormap',coloration); + + %center the color map around zero + Clim = get(ax,'Clim'); %get the current color limits + C_outer = max(abs(Clim)); %get the maximum distance from zero + set(ax,'Clim',[-C_outer C_outer]); %set an inclusive range around zero + + + + %Label the axes + label_shapespace_axes(ax,[],plot_info.stretch); + + %Set the tic marks + set_tics_shapespace(ax,s); + +% %make hidden lines visible +% hidden2('off',ax) + + %%%% + %Make clicking on the thumbnail open it larger in a new window + + if ~plot_info.own_figure + + %build a plot_info structure for just the current plot + plot_info_specific.axes = 'new'; + plot_info_specific.components = plot_info.components(i); + plot_info_specific.category = 'CCF'; + plot_info_specific.style = plot_info.style; + plot_info_specific.CCFtype = plot_info.CCFtype; + plot_info_specific.stretch = plot_info.stretch; + + %set the button down callback on the plot to be sys_draw with + %the argument list for the current plot, and set the button + %down callback for the mesh to the same + set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); + set(meshhandle,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); + + else + + set(get(ax,'Parent'),'Name',[CCF_list{function_number} ' Constraint Curvature Function']) + + %Mark this figure as a constraint curvature function + udata = get(plot_info.figure(i),'UserData'); + + switch plot_info.style + + case 'surface' + + udata.plottype = 'CCF-surface'; + + case 'contour' + + udata.plottype = 'CCF-contour'; + + end + + set(plot_info.figure(i),'UserData',udata); + + end + + + + end + +end \ No newline at end of file diff --git a/ProgramFiles/sys_draw_fcns/CCF_maxpoint.m b/ProgramFiles/sys_draw_fcns/CCF_maxpoint.m new file mode 100644 index 00000000..1f6638f0 --- /dev/null +++ b/ProgramFiles/sys_draw_fcns/CCF_maxpoint.m @@ -0,0 +1,85 @@ +function [maxpoint, maxplane, Imax_x, Imax_y, Imax_z,interpstatecurvature] = CCF_maxpoint(CCF,grid) + n_dim = numel(size(CCF{1})); + + % create an empty cell array with as many entries as + % there are dimensions + y=cell(1,n_dim); + + % populate this cell array with zeros + y(:)={0}; + + % Build up a (squared) norm of the constraint curvature + % (using for now the simple in-coordinates norm, could + % introduce the metric later) + CCFnorm = zeros(size(CCF{1})); + for idx_normbuild = 1:size(CCF,2) + CCFnorm = CCFnorm+CCF{idx_normbuild}.^2; + end + + + + + % Get the maximum value, and the index of that value, + % for the current CCF + [~, Imax] = max(CCFnorm(:)); + [Imax_x,Imax_y,Imax_z] = ind2sub(size(CCF{1}),Imax); + + % Populate y with the location of that point + for idx_maxpoint = 1:n_dim + y{idx_maxpoint} = grid{idx_maxpoint}(Imax); + end + + + +% % create a cell array with two fewer elements than +% % there are dimensions +% idxt=cell(1,n_dim-2); +% +% % populate this cell array with ones +% idxt(1,:)={1}; + + % For every dimension in the problem, create a copy of + % the grid? Not sure why this is named + % "interpstatecurvature" + for j=1:1:n_dim + interpstatecurvature{j}=grid{j,1}; + end + + % Get the value of the curvature at the point y + for j=1:n_dim*(n_dim-1)/2 + curvature(:,j)=interpn(interpstatecurvature{:},CCF{j},y{:},'cubic'); + end + + % Create a skew-symmetric matrix describing the + % orientation of the curvature form + B=[0,curvature(1),curvature(2); + -curvature(1),0,curvature(n_dim); + -curvature(2),-curvature(n_dim),0]; + + % Get the eigenvectors and values of the curvature + % orientation + [V,D]=eig(B); + + % Sort the eigenvalues and eigenvectors + [d,ind] = sort(diag(D)); + Ds = D(ind,ind); + Vs = V(:,ind); + + + % Get the real component of the eigenvector with the + % largest eigenvalue + X=real(Vs(:,end)); + + % Get a vector that is right-hand-positive orthogonal + % to the preceding vector + Y=(Vs(:,end)-X)/(sqrt(-1)); + + % Normalize the X and Y vectors (to make up for the + % fact that they lost length when mapped to real values + Xnorm=X/(norm(X)); + Ynorm=Y/(norm(Y)); + + maxpoint = y; + maxplane = [Xnorm,Ynorm]; + +end \ No newline at end of file diff --git a/ProgramFiles/sys_draw_fcns/ShapeSpace_draw.m b/ProgramFiles/sys_draw_fcns/ShapeSpace_draw.m index 61a35b43..2a0520b0 100644 --- a/ProgramFiles/sys_draw_fcns/ShapeSpace_draw.m +++ b/ProgramFiles/sys_draw_fcns/ShapeSpace_draw.m @@ -1,7 +1,18 @@ function plot_info = ShapeSpace_draw(s,p,plot_info,sys,shch,resolution) + % If the request is to display in optimized coordinates, append the change-of-body-frame if strcmp(plot_info.components{1},'opt') - s.geometry.baseframe = sys; + + % Ensure that the baseframe is a cell array, so that the change to + % optimized coordinates can be appended + if ~iscell(s.geometry.baseframe) + s.geometry.baseframe = {s.geometry.baseframe}; + end + + % Append the transformation to optimized coordinates + s.geometry.baseframe = [s.geometry.baseframe {sys}]; + + % Change the display name displayname = 'Minimum-perturbation coordinates'; else displayname = s.geometry.baseframe; @@ -25,7 +36,7 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot, and set the button %down callback for the mesh to the same - set(plot_info.axes,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(plot_info.axes,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); else diff --git a/ProgramFiles/sys_draw_fcns/beta_draw.m b/ProgramFiles/sys_draw_fcns/beta_draw.m index 9c18fd92..77a05e85 100644 --- a/ProgramFiles/sys_draw_fcns/beta_draw.m +++ b/ProgramFiles/sys_draw_fcns/beta_draw.m @@ -55,16 +55,16 @@ % Convert the function to the plotting grid specified in the gui [B,grid] = plotting_interp(B,grid,resolution,'scalar'); - %%%%% - % No need to change the value of the beta function for stretching, - % because it is a zero-form, not a 2-form - - if plot_info.stretch && (numel(s.grid.eval) == 2) - - % Convert the grid points to their new locations - [grid{:}] = s.convert.old_to_new_points(grid{:}); - - end +% %%%%% +% % No need to change the value of the beta function for stretching, +% % because it is a zero-form, not a 2-form +% +% if plot_info.stretch && (numel(s.grid.eval) == 2) +% +% % Convert the grid points to their new locations +% [grid{:}] = s.convert.old_to_new_points(grid{:}); +% +% end @@ -119,21 +119,21 @@ end - % Make edges if coordinates have changed - if plot_info.stretch && (numel(s.grid.eval) == 2) - - edgeres = 30; - - oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... - s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; - oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... - linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; - - [x_edge,y_edge] = s.convert.old_to_new_points(oldx_edge,oldy_edge); - - l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); - - end +% % Make edges if coordinates have changed +% if plot_info.stretch && (numel(s.grid.eval) == 2) +% +% edgeres = 30; +% +% oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... +% s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; +% oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... +% linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; +% +% [x_edge,y_edge] = s.convert.old_to_new_points(oldx_edge,oldy_edge); +% +% l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); +% +% end %Put an outline box around the plot box(ax,'on') @@ -206,8 +206,8 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot, and set the button %down callback for the mesh to the same - set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); - set(meshhandle,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); + set(meshhandle,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); else diff --git a/ProgramFiles/sys_draw_fcns/bvi_draw.m b/ProgramFiles/sys_draw_fcns/bvi_draw.m index 71e82403..608b1d91 100644 --- a/ProgramFiles/sys_draw_fcns/bvi_draw.m +++ b/ProgramFiles/sys_draw_fcns/bvi_draw.m @@ -107,7 +107,7 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot, and set the button %down callback for the mesh to the same - set(ax,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(ax,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); else diff --git a/ProgramFiles/sys_draw_fcns/contours_on_surface.m b/ProgramFiles/sys_draw_fcns/contours_on_surface.m new file mode 100644 index 00000000..ce2c30ba --- /dev/null +++ b/ProgramFiles/sys_draw_fcns/contours_on_surface.m @@ -0,0 +1,48 @@ +function contours_on_surface(convert,h,grid) + + % Set the x, y, and z bounds as tight axes + axis(h.Parent,... + [min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:)) min(grid{3}(:)) max(grid{3}(:))]) + + + + + % loop over all the contours, using undocumented + % contour handle features + for idx = 1:numel(h.EdgePrims) + + v = h.EdgePrims(idx).VertexData; + [cx,cy,cz] = convert.surface.old_to_new_points(v(1,:),v(2,:)); + c = [cx;cy;cz]; + + h.EdgePrims(idx).VertexData = single(c); + + end + + try + for idx = 1:numel(h.EdgeLoopPrims) + + v = h.EdgeLoopPrims(idx).VertexData; + [cx,cy,cz] = convert.surface.old_to_new_points(v(1,:),v(2,:)); + c = [cx;cy;cz]; + + h.EdgeLoopPrims(idx).VertexData = single(c); + + end + catch + end + + for idx = 1:numel(h.FacePrims) + + v = h.FacePrims(idx).VertexData; + [cx,cy,cz] = convert.surface.old_to_new_points(v(1,:),v(2,:)); + c = [cx;cy;cz]; + + h.FacePrims(idx).VertexData = single(c); + + h.FacePrims(idx).ColorData = uint8([255 255 255 255]'); + + end + + +end \ No newline at end of file diff --git a/ProgramFiles/sys_draw_fcns/dbeta_draw.m b/ProgramFiles/sys_draw_fcns/dbeta_draw.m index 676a9d4a..f248a4c4 100644 --- a/ProgramFiles/sys_draw_fcns/dbeta_draw.m +++ b/ProgramFiles/sys_draw_fcns/dbeta_draw.m @@ -38,155 +38,155 @@ % If the shape coordinates should be transformed, make the conversion % (multiply the vectors by the inverse jacobian) - if plot_info.stretch - - % Calculate the jacobians at the plotting points - Jac = arrayfun(s.convert.jacobian,grid{:},'UniformOutput',false); - - % Use the jacobians to convert the vectors - - % Iterate over all connection vector fields present - for i = 1:size(V,1) - - % Iterate over all vectors present - for j = 1:numel(V{i,1}) - - % Extract all components of the relevant vector - tempVin = cellfun(@(x) x(j),V(i,:)); - - % Multiply by the inverse Jacobian (because V is a - % gradient, not a flow) - tempVout = Jac{j}\tempVin(:); - - % Replace vector components - for k = 1:size(V,2) - - V{i,k}(j) = tempVout(k); - - end - - end - - - end - - - - % Use the jacobians to convert the coordinate vectors in accordance - % with the stretch transformation - - % Iterate over all coordinate vector fields present - for i = 1:size(V_coord,1) - - % Iterate over all vectors present - for j = 1:numel(V_coord{i,1}) - - % Extract all components of the relevant vector - tempVin = cellfun(@(x) x(j),V_coord(i,:)); - - % Multiply by the Jacobian (because V_coord is a flow) - tempVout = Jac{j}*tempVin(:); - - % Replace vector components - for k = 1:size(V_coord,2) - - V_coord{i,k}(j) = tempVout(k); - - end - - end - - - end - - % Rotate the coordinate vectors to get normals - V_norm = repmat({zeros(size(V{1}))},numel(grid)); - if numel(grid) == 2 - - V_norm = {V_coord{2,2} -V_coord{2,1}; - -V_coord{1,2} V_coord{1,1}}; - - elseif numel(grid) == 3 - - V_norm = {V_coord{3,3} V_coord{3,2} -V_coord{3,1}; % z coord field around y - -V_coord{1,2} V_coord{1,1} -V_coord{1,3}; % x around z - V_coord{2,1} V_coord{2,3} -V_coord{2,2}}; % y around x - - else - warning('Rotations for >3 dimensions undefined in vfield_draw clipping of stretched vector field (add them if you need them) ') - - end - - % Convert the grid points to their new locations - [grid{:}] = s.convert.old_to_new_points(grid{:}); - - - - %%%% - % Trim any vectors that go outside the boundary - - % Take the dot product of the connection vector fields and the - % normal vector fields - dprods = repmat({zeros(size(V_norm{1}))},size(V,1),size(V_norm,1)); - for i = 1:size(dprods,1) - - for j = 1:size(dprods,2) - - elementprods = cellfun(@(x,y) x.*y,V(i,:),V_norm(j,:),'UniformOutput',false); - - for k = 1:numel(elementprods) - - dprods{i,j} = dprods{i,j}+elementprods{k}; - - end - - end - - end - - % Create a cell array to hold the masking term - edgemask = repmat({zeros(size(V{1}))},size(V)); - - % Iterate along the rows of V (each row is one field) - for idxA = 1:size(V,1) - - % Iterate along the elements of dotprods in the corresponding row - % (each colum corresponds to the dot product of the current row of - % V with the then nth coordinate field - for idxB = 1:size(dprods,2) - - % Identify the index sets that correspond to the first and last - % elements along this direction of the grid - indices_start = [repmat({':'},1,idxB-1), {1}, repmat({':'},1,size(V,2)-idxB)]; - indices_end = [repmat({':'},1,idxB-1), {size(V{1},idxB)}, repmat({':'},1,size(V,2)-idxB)]; - - % Find all vectors that point out - V_test_start = dprods{idxA,idxB} < 0; - V_test_end = dprods{idxA,idxB} > 0; - - % Take the start and end indices values from the test boolean - edgemask{idxA,idxB}(indices_start{:}) = V_test_start(indices_start{:}); - edgemask{idxA,idxB}(indices_end{:}) = V_test_end(indices_end{:}); - - end - - % Combine all the edgemasks for a field into a single mask - - edgemask_merged = zeros(size(edgemask{1})); - for idxB = 1:size(V,2) - - edgemask_merged = edgemask_merged | edgemask{idxA,idxB}; - - end - - % Apply edgemask_merged to all the fields in this row of V - for idxB = 1:size(V,2) - - V{idxA,idxB}(edgemask_merged) = 0; - - end - end - end +% if plot_info.stretch +% +% % Calculate the jacobians at the plotting points +% Jac = arrayfun(s.convert.jacobian,grid{:},'UniformOutput',false); +% +% % Use the jacobians to convert the vectors +% +% % Iterate over all connection vector fields present +% for i = 1:size(V,1) +% +% % Iterate over all vectors present +% for j = 1:numel(V{i,1}) +% +% % Extract all components of the relevant vector +% tempVin = cellfun(@(x) x(j),V(i,:)); +% +% % Multiply by the inverse Jacobian (because V is a +% % gradient, not a flow) +% tempVout = Jac{j}\tempVin(:); +% +% % Replace vector components +% for k = 1:size(V,2) +% +% V{i,k}(j) = tempVout(k); +% +% end +% +% end +% +% +% end +% +% +% +% % Use the jacobians to convert the coordinate vectors in accordance +% % with the stretch transformation +% +% % Iterate over all coordinate vector fields present +% for i = 1:size(V_coord,1) +% +% % Iterate over all vectors present +% for j = 1:numel(V_coord{i,1}) +% +% % Extract all components of the relevant vector +% tempVin = cellfun(@(x) x(j),V_coord(i,:)); +% +% % Multiply by the Jacobian (because V_coord is a flow) +% tempVout = Jac{j}*tempVin(:); +% +% % Replace vector components +% for k = 1:size(V_coord,2) +% +% V_coord{i,k}(j) = tempVout(k); +% +% end +% +% end +% +% +% end +% +% % Rotate the coordinate vectors to get normals +% V_norm = repmat({zeros(size(V{1}))},numel(grid)); +% if numel(grid) == 2 +% +% V_norm = {V_coord{2,2} -V_coord{2,1}; +% -V_coord{1,2} V_coord{1,1}}; +% +% elseif numel(grid) == 3 +% +% V_norm = {V_coord{3,3} V_coord{3,2} -V_coord{3,1}; % z coord field around y +% -V_coord{1,2} V_coord{1,1} -V_coord{1,3}; % x around z +% V_coord{2,1} V_coord{2,3} -V_coord{2,2}}; % y around x +% +% else +% warning('Rotations for >3 dimensions undefined in vfield_draw clipping of stretched vector field (add them if you need them) ') +% +% end +% +% % Convert the grid points to their new locations +% [grid{:}] = s.convert.old_to_new_points(grid{:}); +% +% +% +% %%%% +% % Trim any vectors that go outside the boundary +% +% % Take the dot product of the connection vector fields and the +% % normal vector fields +% dprods = repmat({zeros(size(V_norm{1}))},size(V,1),size(V_norm,1)); +% for i = 1:size(dprods,1) +% +% for j = 1:size(dprods,2) +% +% elementprods = cellfun(@(x,y) x.*y,V(i,:),V_norm(j,:),'UniformOutput',false); +% +% for k = 1:numel(elementprods) +% +% dprods{i,j} = dprods{i,j}+elementprods{k}; +% +% end +% +% end +% +% end +% +% % Create a cell array to hold the masking term +% edgemask = repmat({zeros(size(V{1}))},size(V)); +% +% % Iterate along the rows of V (each row is one field) +% for idxA = 1:size(V,1) +% +% % Iterate along the elements of dotprods in the corresponding row +% % (each colum corresponds to the dot product of the current row of +% % V with the then nth coordinate field +% for idxB = 1:size(dprods,2) +% +% % Identify the index sets that correspond to the first and last +% % elements along this direction of the grid +% indices_start = [repmat({':'},1,idxB-1), {1}, repmat({':'},1,size(V,2)-idxB)]; +% indices_end = [repmat({':'},1,idxB-1), {size(V{1},idxB)}, repmat({':'},1,size(V,2)-idxB)]; +% +% % Find all vectors that point out +% V_test_start = dprods{idxA,idxB} < 0; +% V_test_end = dprods{idxA,idxB} > 0; +% +% % Take the start and end indices values from the test boolean +% edgemask{idxA,idxB}(indices_start{:}) = V_test_start(indices_start{:}); +% edgemask{idxA,idxB}(indices_end{:}) = V_test_end(indices_end{:}); +% +% end +% +% % Combine all the edgemasks for a field into a single mask +% +% edgemask_merged = zeros(size(edgemask{1})); +% for idxB = 1:size(V,2) +% +% edgemask_merged = edgemask_merged | edgemask{idxA,idxB}; +% +% end +% +% % Apply edgemask_merged to all the fields in this row of V +% for idxB = 1:size(V,2) +% +% V{idxA,idxB}(edgemask_merged) = 0; +% +% end +% end +% end %%% %If there's a singularity, use arctan scaling on the magnitude of the @@ -215,29 +215,29 @@ quiver3(ax,grid{1:3},V{field_number,:},'k','LineWidth',2) end - % Make edges if coordinates have changed - if plot_info.stretch - - edgeres = 30; - - oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... - s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; - oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... - linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; - - [x_edge,y_edge] = s.convert.old_to_new_points(oldx_edge,oldy_edge); - - l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); %#ok - - end - - - if plot_info.stretch - axis(ax,'equal'); - axis(ax,[min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:))]); - else - axis(ax,'equal'); - end +% % Make edges if coordinates have changed +% if plot_info.stretch +% +% edgeres = 30; +% +% oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... +% s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; +% oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... +% linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; +% +% [x_edge,y_edge] = s.convert.old_to_new_points(oldx_edge,oldy_edge); +% +% l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); %#ok +% +% end +% +% +% if plot_info.stretch +% axis(ax,'equal'); +% axis(ax,[min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:))]); +% else + axis(ax,'equal','tight'); +% end %set the display range if ~plot_info.stretch axis(ax,s.grid_range); @@ -252,7 +252,7 @@ %If there's a shape change involved, plot it if ~strcmp(shch,'null') - overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); + overlay_shape_change_2d(ax,p,0,s.convert); end @@ -270,7 +270,7 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot - set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); else diff --git a/ProgramFiles/sys_draw_fcns/disp_draw.m b/ProgramFiles/sys_draw_fcns/disp_draw.m index 289b79ca..c26f0d4e 100644 --- a/ProgramFiles/sys_draw_fcns/disp_draw.m +++ b/ProgramFiles/sys_draw_fcns/disp_draw.m @@ -130,7 +130,7 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot, and set the button %down callback for the mesh to the same - set(ax,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(ax,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); else diff --git a/ProgramFiles/sys_draw_fcns/mfield_draw.m b/ProgramFiles/sys_draw_fcns/mfield_draw.m index 9d006388..900ecb25 100644 --- a/ProgramFiles/sys_draw_fcns/mfield_draw.m +++ b/ProgramFiles/sys_draw_fcns/mfield_draw.m @@ -7,7 +7,11 @@ % metric stretch has been selected. Should probably improve this in the % future. - + %Get the configuration file, and extract the Colorpath + configfile = 'sysplotter_config'; + configfile = fullfile(fileparts(mfilename('fullpath')),'..',configfile); + load(configfile,'Colorset'); + % Get the number of dimensions n_dim = numel(s.grid.eval); @@ -16,6 +20,13 @@ plot_info = ensure_figure_axes(plot_info); +% Pull out the stretch name +stretchnames = {'stretch','surface'}; +if plot_info.stretch + stretchname = stretchnames{plot_info.stretch}; +end + + if n_dim==2 %Vector field list @@ -29,7 +40,7 @@ % Get the vector field and interpolate into the specified grid % Extract the plotting grid - grid = s.grid.metric_eval; + grid = s.grid.metric_display; % metric for evaluating ellipse field @@ -70,55 +81,55 @@ % If the shape coordinates should be transformed, make the conversion % (multiply the vectors by the inverse jacobian) - if plot_info.stretch - - M = celltensorconvert(M); - - % Calculate the jacobians at the plotting points - Jac = arrayfun(s.convert.jacobian,grid{:},'UniformOutput',false); - - % Use the jacobians to convert the metric - for i = 1:size(M,1) - for j = 1:size(M,2) - - M{i,j} = (Jac{i,j}'\M{i,j})/Jac{i,j}; - - end - end - - - % Use the jacobians to convert the coordinate vectors in accordance - % with the stretch transformation - - for i = 1:size(M{1},1) - - % Iterate over all vectors present - for j = 1:size(M{1},1) - - % Extract all components of the relevant vector - % tempEin = cellfun(@(x) x(j),E_coord(i,:)); - tempMin = [M{1}(i,j);M{2}(i,j)]; - - % Multiply by the Jacobian (because V_coord is a flow) - tempMout = Jac{i,j}*tempMin; - - % Replace vector components - - M{1}(i,j) = tempMout(1); - M{2}(i,j) = tempMout(2); - - end - - - end - - - % Convert the grid points to their new locations - [grid{:}] = s.convert.old_to_new_points(grid{:}); - - M = celltensorconvert(M); - - end +% if plot_info.stretch +% +% M = celltensorconvert(M); +% +% % Calculate the jacobians at the plotting points +% Jac = arrayfun(s.convert.stretch.jacobian,grid{:},'UniformOutput',false); +% +% % Use the jacobians to convert the metric +% for i = 1:size(M,1) +% for j = 1:size(M,2) +% +% M{i,j} = (Jac{i,j}'\M{i,j})/Jac{i,j}; +% +% end +% end +% +% +% % Use the jacobians to convert the coordinate vectors in accordance +% % with the stretch transformation +% +% for i = 1:size(M{1},1) +% +% % Iterate over all vectors present +% for j = 1:size(M{1},1) +% +% % Extract all components of the relevant vector +% % tempEin = cellfun(@(x) x(j),E_coord(i,:)); +% tempMin = [M{1}(i,j);M{2}(i,j)]; +% +% % Multiply by the Jacobian (because V_coord is a flow) +% tempMout = Jac{i,j}*tempMin; +% +% % Replace vector components +% +% M{1}(i,j) = tempMout(1); +% M{2}(i,j) = tempMout(2); +% +% end +% +% +% end +% +% +% % Convert the grid points to their new locations +% [grid{:}] = s.convert.stretch.old_to_new_points(grid{:}); +% +% M = celltensorconvert(M); +% +% end %%% %If there's a singularity, use arctan scaling on the magnitude of the @@ -152,10 +163,13 @@ % metricellipsefield(s.grid.metric_display{:},celltensorconvert(s.metricfield.metric_display.content.metric),'tissot',{'edgecolor','k’}) - metricellipsefield(grid{:},celltensorconvert(M),'tissot',{'edgecolor','k','parent',ax}); + %metricellipsefield(grid{:},celltensorconvert(M),'tissot-cross',{'edgecolor','k','parent',ax},{'color',Colorset.secondary,'parent',ax}); + metricellipsefield_convert(grid{:},celltensorconvert(M),'tissot-cross',s.convert,plot_info.stretch,{'linewidth',1,'edgecolor',Colorset.spot,'parent',ax},{'color',Colorset.secondary,'parent',ax}); + + box(ax,'on'); - % Make edges if coordinates have changed + % Make edges and create a backing surfaceif coordinates have changed if plot_info.stretch edgeres = 30; @@ -165,10 +179,17 @@ oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; - [x_edge,y_edge] = s.convert.old_to_new_points(oldx_edge,oldy_edge); + [x_edge,y_edge,z_edge] = s.convert.(stretchname).old_to_new_points(oldx_edge,oldy_edge); - l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); %#ok + l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'ZData',z_edge,'Color','k','LineWidth',1); %#ok + if plot_info.stretch == 2 + hold(ax,'on') + [s_x,s_y,s_z] = s.convert.(stretchname).old_to_new_points(s.grid.eval{:}); + s_backing = surf('Parent',ax,'XData',s_x,'YData',s_y,'ZData',s_z,'FaceColor','w','EdgeColor','none'); + hold(ax,'off') + end + end @@ -176,6 +197,9 @@ axis(ax,'equal'); % axis(ax,[min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:))]); axis(ax,[min(x_edge) max(x_edge) min(y_edge) max(y_edge)]) + if plot_info.stretch == 2 + view(ax,3) + end else axis(ax,'equal'); end @@ -192,12 +216,30 @@ %If there's a shape change involved, plot it if ~strcmp(shch,'null') - - overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); - + if n_dim==2 + switch plot_info.stretch + case {0,1} % No stretch or 2-d stretch + switch plot_info.style + case 'contour' + overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); + case 'surface' + overlay_shape_change_3d_surf(ax,p,zdata{function_number,:},plot_info.stretch,s.convert,true); + end + + case 2 % Surface-embedded stretch + + overlay_shape_change_metricsurf(ax,p,s.convert.surface.old_to_new_points,Colorset) + %overlay_shape_change_3d_surf(ax,p,grid_extra,plot_info.stretch,s.convert,false) + %overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); + end + + end + if n_dim>2 + meshhandle.FaceAlpha=0.9; + line('Parent',ax,'XData',p.phi_locus_full{i}.shape(:,1),'YData',p.phi_locus_full{i}.shape(:,2),'ZData',p.phi_locus_full{i}.shape(:,3),'Color',Colorset.spot,'LineWidth',6,'parent',ax); + end end - %%%% %Make clicking on the thumbnail open it larger in a new window @@ -211,7 +253,7 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot - set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); else diff --git a/ProgramFiles/sys_draw_fcns/overlay_shape_change_2d.m b/ProgramFiles/sys_draw_fcns/overlay_shape_change_2d.m index 15bf63d7..cc3861cb 100644 --- a/ProgramFiles/sys_draw_fcns/overlay_shape_change_2d.m +++ b/ProgramFiles/sys_draw_fcns/overlay_shape_change_2d.m @@ -1,4 +1,4 @@ -function overlay_shape_change_2d(ax,p,stretch,convert) +function overlay_shape_change_2d(ax,p,stretch,convert,isomap,s) %overlay shape changes onto the specified axis %Get the configuration file, and extract the Colorpath @@ -10,9 +10,13 @@ function overlay_shape_change_2d(ax,p,stretch,convert) if stretch - - [p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)] ... - = convert.old_to_new_points(p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)); + if stretch==1 + [p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)] ... + = convert.stretch.old_to_new_points(p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)); + else + [p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)] ... + = convert.surface.old_to_new_points(p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)); + end end @@ -23,10 +27,23 @@ function overlay_shape_change_2d(ax,p,stretch,convert) pathpoints{idx} = p.phi_locus_full{i}.shape(:,idx); end - line(pathpoints{:},'Color',Colorset.spot,'LineWidth',5,'Parent',ax); - - %draw the direction arrows on the line - plot_dir_arrows(p.phi_locus_full{i}.shape(:,1),p.phi_locus_full{i}.shape(:,2),p.phi_arrows{i}{1},'Color',Colorset.spot,'LineWidth',4,'Parent',ax); +% if stretch==2 +% +% H_path = griddata(isomap.x_new,isomap.y_new,isomap.HF_isomap,pathpoints{1},pathpoints{2}); +% +% line(pathpoints{:},'ZData',H_path,'Color',Colorset.spot,'LineWidth',5,'Parent',ax); +% +% % %draw the direction arrows on the line +% % plot_dir_arrows(p.phi_locus_full{i}.shape(:,1),p.phi_locus_full{i}.shape(:,2),p.phi_arrows{i}{1},isomap,s,'Color',Colorset.spot,'LineWidth',4,'Parent',ax); +% +% else + + line(pathpoints{:},'Color',Colorset.spot,'LineWidth',5,'Parent',ax); + + %draw the direction arrows on the line + plot_dir_arrows(p.phi_locus_full{i}.shape(:,1),p.phi_locus_full{i}.shape(:,2),p.phi_arrows{i}{1},'Color',Colorset.spot,'LineWidth',4,'Parent',ax); + +% end %draw the start/end markers if ~isempty(p.phi_locus_full{i}.marker.shape) diff --git a/ProgramFiles/sys_draw_fcns/overlay_shape_change_3d_surf.m b/ProgramFiles/sys_draw_fcns/overlay_shape_change_3d_surf.m index 3b4a062d..027fae12 100644 --- a/ProgramFiles/sys_draw_fcns/overlay_shape_change_3d_surf.m +++ b/ProgramFiles/sys_draw_fcns/overlay_shape_change_3d_surf.m @@ -1,6 +1,8 @@ function overlay_shape_change_3d_surf(ax,p,zdata,stretch,convert,scale_when_stretched) %overlay shape changes onto the specified axis + stretchnames = {'stretch'}; + %plot all paths for i = 1:numel(p.phi_locus_full) @@ -8,7 +10,7 @@ function overlay_shape_change_3d_surf(ax,p,zdata,stretch,convert,scale_when_stre % Get the value by which to scale the z data if scale_when_stretched - ascale = arrayfun(@(x,y) 1/det(convert.jacobian(x,y)),p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)); + ascale = arrayfun(@(x,y) 1/det(convert.(stretchnames{stretch}).jacobian(x,y)),p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)); else ascale = 1; end @@ -17,7 +19,7 @@ function overlay_shape_change_3d_surf(ax,p,zdata,stretch,convert,scale_when_stre zdata{i} = ascale.*zdata{i}; [p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)] ... - = convert.old_to_new_points(p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)); + = convert.(stretchnames{stretch}).old_to_new_points(p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)); diff --git a/ProgramFiles/sys_draw_fcns/overlay_shape_change_metricsurf.m b/ProgramFiles/sys_draw_fcns/overlay_shape_change_metricsurf.m new file mode 100644 index 00000000..805aa9f0 --- /dev/null +++ b/ProgramFiles/sys_draw_fcns/overlay_shape_change_metricsurf.m @@ -0,0 +1,14 @@ +function overlay_shape_change_metricsurf(ax,p,conversion,Colorset) + + + %plot all paths + for i = 1:numel(p.phi_locus_full) + + [x,y,z] = conversion(p.phi_locus_full{i}.shape(:,1), p.phi_locus_full{i}.shape(:,2)); + + %draw the path itself + patch('XData',x,'YData',y,'Zdata',z,'EdgeColor',Colorset.spot,'FaceColor','none','LineWidth',6,'Parent',ax); + + end + +end \ No newline at end of file diff --git a/ProgramFiles/sys_draw_fcns/sys_draw_dummy_callback.m b/ProgramFiles/sys_draw_fcns/sys_draw_dummy_callback.m index e4be75b2..34c7a9ab 100644 --- a/ProgramFiles/sys_draw_fcns/sys_draw_dummy_callback.m +++ b/ProgramFiles/sys_draw_fcns/sys_draw_dummy_callback.m @@ -1,4 +1,4 @@ -function sys_draw_dummy_callback(hObject, eventdata, plot_structure,sys,shch) +function sys_draw_dummy_callback(hObject, eventdata, plot_structure,sys,shch,stretch) %strip out extra callback info handles = guidata(hObject); @@ -10,6 +10,6 @@ function sys_draw_dummy_callback(hObject, eventdata, plot_structure,sys,shch) resolution.vector_range = get(handles.vectorresolution,'UserData'); resolution.scalar_range = get(handles.scalarresolution,'UserData'); - sys_draw(plot_structure,sys,shch,handles.progresspanel,0,resolution,handles); + sys_draw(plot_structure,sys,shch,stretch,handles.progresspanel,0,resolution,handles); end \ No newline at end of file diff --git a/ProgramFiles/sys_draw_fcns/vfield_draw.m b/ProgramFiles/sys_draw_fcns/vfield_draw.m index 49a79b4f..1836c358 100755 --- a/ProgramFiles/sys_draw_fcns/vfield_draw.m +++ b/ProgramFiles/sys_draw_fcns/vfield_draw.m @@ -50,13 +50,21 @@ % If the shape coordinates should be transformed, make the conversion % (multiply the vectors by the inverse jacobian) - if plot_info.stretch - - % Calculate the jacobians at the plotting points - Jac = arrayfun(s.convert.jacobian,grid{:},'UniformOutput',false); + if plot_info.stretch && (n_dim == 2) + + if plot_info.stretch==1 + % Calculate the jacobians at the plotting points + Jac = arrayfun(s.convert.stretch.jacobian,grid{:},'UniformOutput',false); + end + + if plot_info.stretch==2 + % Calculate the jacobians at the plotting points + Jac = arrayfun(s.convert.surface.jacobian,grid{:},'UniformOutput',false); + end % Use the jacobians to convert the vectors - + V_converted = repmat({V{1,1}},size(V,1),size(Jac{1},1)); + % Iterate over all connection vector fields present for i = 1:size(V,1) @@ -66,14 +74,25 @@ % Extract all components of the relevant vector tempVin = cellfun(@(x) x(j),V(i,:)); - % Multiply by the inverse Jacobian (because V is a + % Multiply by the inverse transpose Jacobian (because V is a % gradient, not a flow) - tempVout = Jac{j}\tempVin(:); + + % Expand Jacobian if mapping 2 to 3 dimensions. Expansion + % is a vector normal to both columns, length does not + % matter + if size(Jac{j},1) == 3 + J_exp = [Jac{j} cross(Jac{j}(:,1),Jac{j}(:,2))]; + tempVin = [tempVin(:);0]; + else + J_exp = Jac{j}; + end + + tempVout = (J_exp')\tempVin(:); % Replace vector components - for k = 1:size(V,2) + for k = 1:numel(tempVout) - V{i,k}(j) = tempVout(k); + V_converted{i,k}(j) = tempVout(k); end @@ -81,11 +100,19 @@ end - + V = V_converted; % Use the jacobians to convert the coordinate vectors in accordance % with the stretch transformation + % Create a set of vector fields along coordinate directions + V_coord = repmat({zeros(size(V{1}))},size(Jac{1},1)); + for i = 1:numel(grid) + + V_coord{i,i} = ones(size(V{1})); + + end + V_coord_converted = V_coord; % Iterate over all coordinate vector fields present for i = 1:size(V_coord,1) @@ -94,15 +121,15 @@ for j = 1:numel(V_coord{i,1}) % Extract all components of the relevant vector - tempVin = cellfun(@(x) x(j),V_coord(i,:)); + tempVin = cellfun(@(x) x(j),V_coord(i,1:numel(grid))); % Multiply by the Jacobian (because V_coord is a flow) tempVout = Jac{j}*tempVin(:); % Replace vector components - for k = 1:size(V_coord,2) + for k = 1:numel(tempVout) - V_coord{i,k}(j) = tempVout(k); + V_coord_converted{i,k}(j) = tempVout(k); end @@ -110,15 +137,17 @@ end + V_coord = V_coord_converted; + % Rotate the coordinate vectors to get normals V_norm = repmat({zeros(size(V{1}))},numel(grid)); - if numel(grid) == 2 + if size(V_coord,2) == 2 V_norm = {V_coord{2,2} -V_coord{2,1}; -V_coord{1,2} V_coord{1,1}}; - elseif numel(grid) == 3 + elseif size(V_coord,2) == 3 V_norm = {V_coord{3,3} V_coord{3,2} -V_coord{3,1}; % z coord field around y -V_coord{1,2} V_coord{1,1} -V_coord{1,3}; % x around z @@ -130,75 +159,85 @@ end % Convert the grid points to their new locations - [grid{:}] = s.convert.old_to_new_points(grid{:}); + + if plot_info.stretch==1 + [grid{:}] = s.convert.stretch.old_to_new_points(grid{:}); + end + + if plot_info.stretch==2 + [grid{:},grid_extra] = s.convert.surface.old_to_new_points(grid{:}); + grid = [grid;{grid_extra}]; + end + %%%% % Trim any vectors that go outside the boundary + if plot_info.stretch < 2 + % Take the dot product of the connection vector fields and the + % normal vector fields + dprods = repmat({zeros(size(V_norm{1}))},size(V,1),size(V_norm,1)); + for i = 1:size(dprods,1) - % Take the dot product of the connection vector fields and the - % normal vector fields - dprods = repmat({zeros(size(V_norm{1}))},size(V,1),size(V_norm,1)); - for i = 1:size(dprods,1) + for j = 1:size(dprods,2) - for j = 1:size(dprods,2) + elementprods = cellfun(@(x,y) x.*y,V(i,:),V_norm(j,:),'UniformOutput',false); - elementprods = cellfun(@(x,y) x.*y,V(i,:),V_norm(j,:),'UniformOutput',false); + for k = 1:numel(elementprods) - for k = 1:numel(elementprods) + dprods{i,j} = dprods{i,j}+elementprods{k}; - dprods{i,j} = dprods{i,j}+elementprods{k}; + end end end - end - - % Create a cell array to hold the masking term - edgemask = repmat({zeros(size(V{1}))},size(V)); + % Create a cell array to hold the masking term + edgemask = repmat({zeros(size(V{1}))},size(V)); - % Iterate along the rows of V (each row is one field) - for idxA = 1:size(V,1) + % Iterate along the rows of V (each row is one field) + for idxA = 1:size(V,1) - % Iterate along the elements of dotprods in the corresponding row - % (each colum corresponds to the dot product of the current row of - % V with the then nth coordinate field - for idxB = 1:size(dprods,2) + % Iterate along the elements of dotprods in the corresponding row + % (each colum corresponds to the dot product of the current row of + % V with the then nth coordinate field + for idxB = 1:size(dprods,2) - % Identify the index sets that correspond to the first and last - % elements along this direction of the grid - indices_start = [repmat({':'},1,idxB-1), {1}, repmat({':'},1,size(V,2)-idxB)]; - indices_end = [repmat({':'},1,idxB-1), {size(V{1},idxB)}, repmat({':'},1,size(V,2)-idxB)]; + % Identify the index sets that correspond to the first and last + % elements along this direction of the grid + indices_start = [repmat({':'},1,idxB-1), {1}, repmat({':'},1,size(V,2)-idxB)]; + indices_end = [repmat({':'},1,idxB-1), {size(V{1},idxB)}, repmat({':'},1,size(V,2)-idxB)]; - % Find all vectors that point out - V_test_start = dprods{idxA,idxB} < 0; - V_test_end = dprods{idxA,idxB} > 0; + % Find all vectors that point out + V_test_start = dprods{idxA,idxB} < 0; + V_test_end = dprods{idxA,idxB} > 0; - % Take the start and end indices values from the test boolean - edgemask{idxA,idxB}(indices_start{:}) = V_test_start(indices_start{:}); - edgemask{idxA,idxB}(indices_end{:}) = V_test_end(indices_end{:}); + % Take the start and end indices values from the test boolean + edgemask{idxA,idxB}(indices_start{:}) = V_test_start(indices_start{:}); + edgemask{idxA,idxB}(indices_end{:}) = V_test_end(indices_end{:}); - end + end - % Combine all the edgemasks for a field into a single mask + % Combine all the edgemasks for a field into a single mask - edgemask_merged = zeros(size(edgemask{1})); - for idxB = 1:size(V,2) + edgemask_merged = zeros(size(edgemask{1})); + for idxB = 1:size(V,2) - edgemask_merged = edgemask_merged | edgemask{idxA,idxB}; + edgemask_merged = edgemask_merged | edgemask{idxA,idxB}; - end + end - % Apply edgemask_merged to all the fields in this row of V - for idxB = 1:size(V,2) + % Apply edgemask_merged to all the fields in this row of V + for idxB = 1:size(V,2) - V{idxA,idxB}(edgemask_merged) = 0; + V{idxA,idxB}(edgemask_merged) = 0; - end - end - end + end + end + end + end %%% %If there's a singularity, use arctan scaling on the magnitude of the @@ -222,40 +261,67 @@ %plot the vector field arrows if n_dim == 2 - quiver(ax,grid{:},V{field_number,1},V{field_number,2},'k','LineWidth',2) + + if numel(grid) == 2 + q = quiver(ax,grid{:},V{field_number,1},V{field_number,2},'k','LineWidth',2); + else + q = quiver3(ax,grid{:},V{field_number,1},V{field_number,2},V{field_number,3},'k','LineWidth',2); + end + + else idxt=cell(1,n_dim-3); idxt(1,:)={1}; - quiver3(ax,grid{1}(:,:,:,idxt{:}),grid{2}(:,:,:,idxt{:}),grid{3}(:,:,:,idxt{:}),V{field_number,1}(:,:,:,idxt{:}),V{field_number,2}(:,:,:,idxt{:}),V{field_number,3}(:,:,:,idxt{:}),'k','LineWidth',2) + q = quiver3(ax,grid{1}(:,:,:,idxt{:}),grid{2}(:,:,:,idxt{:}),grid{3}(:,:,:,idxt{:}),V{field_number,1}(:,:,:,idxt{:}),V{field_number,2}(:,:,:,idxt{:}),V{field_number,3}(:,:,:,idxt{:}),'k','LineWidth',2); end - % Make edges if coordinates have changed - if plot_info.stretch + % Make edges if coordinates have changed + if plot_info.stretch && (numel(s.grid.eval) == 2) - edgeres = 30; + edgeres = 30; - oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... - s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; - oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... - linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; + oldx_edge = [s.grid_range(1)*ones(edgeres,1);linspace(s.grid_range(1),s.grid_range(2),edgeres)';... + s.grid_range(2)*ones(edgeres,1);linspace(s.grid_range(2),s.grid_range(1),edgeres)']; + oldy_edge = [linspace(s.grid_range(3),s.grid_range(4),edgeres)';s.grid_range(4)*ones(edgeres,1);... + linspace(s.grid_range(4),s.grid_range(3),edgeres)';s.grid_range(3)*ones(edgeres,1)]; - [x_edge,y_edge] = s.convert.old_to_new_points(oldx_edge,oldy_edge); + switch plot_info.stretch + + case 1 % 2-d stretch + + [x_edge,y_edge] = s.convert.stretch.old_to_new_points(oldx_edge,oldy_edge); - l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); %#ok + l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Color','k','LineWidth',1); - end - + case 2 % 3-d surface embedding + + [x_edge,y_edge,z_edge] = s.convert.surface.old_to_new_points(oldx_edge,oldy_edge); + + l_edge = line('Parent',ax,'Xdata',x_edge,'YData',y_edge,'Zdata',z_edge,'Color','k','LineWidth',1); + + hold(ax,'on') + [s_x,s_y,s_z] = s.convert.surface.old_to_new_points(s.grid.eval{:}); + s_backing = surf('XData',s_x,'YData',s_y,'ZData',s_z,'Parent',ax,'FaceColor','w','EdgeColor','none'); + hold(ax,'off') + end + + end if plot_info.stretch axis(ax,'equal'); axis(ax,[min(grid{1}(:)) max(grid{1}(:)) min(grid{2}(:)) max(grid{2}(:))]); + if plot_info.stretch == 2 + view(ax,3) + end else axis(ax,'equal'); end %set the display range if ~plot_info.stretch axis(ax,s.grid_range); - end + end + + box(ax,'on') %Label the axes (two-dimensional) label_shapespace_axes(ax,[],plot_info.stretch); @@ -265,15 +331,24 @@ %If there's a shape change involved, plot it if ~strcmp(shch,'null') - n_dim=length(s.grid_range)/2; if n_dim==2 - overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); - end + switch plot_info.stretch + case {0,1} % No stretch or 2-d stretch + overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); + + case 2 % Surface-embedded stretch + + overlay_shape_change_metricsurf(ax,p,s.convert.surface.old_to_new_points,Colorset) + %overlay_shape_change_3d_surf(ax,p,grid_extra,plot_info.stretch,s.convert,false) + %overlay_shape_change_2d(ax,p,plot_info.stretch,s.convert); + end + + end if n_dim>2 line('Parent',ax,'XData',p.phi_locus_full{i}.shape(:,1),'YData',p.phi_locus_full{i}.shape(:,2),'ZData',p.phi_locus_full{i}.shape(:,3),'Color',Colorset.spot,'LineWidth',6,'parent',ax); end - end + %%%% @@ -289,7 +364,7 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot - set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(plot_info.axes(i),'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); else diff --git a/ProgramFiles/sys_draw_fcns/xy_draw_helper.m b/ProgramFiles/sys_draw_fcns/xy_draw_helper.m index e9f80abc..ab4251fa 100644 --- a/ProgramFiles/sys_draw_fcns/xy_draw_helper.m +++ b/ProgramFiles/sys_draw_fcns/xy_draw_helper.m @@ -196,7 +196,7 @@ %set the button down callback on the plot to be sys_draw with %the argument list for the current plot, and set the button %down callback for the mesh to the same - set(plot_info.axes,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch}); + set(plot_info.axes,'ButtonDownFcn',{@sys_draw_dummy_callback,plot_info_specific,sys,shch,plot_info.stretch_name}); else diff --git a/ProgramFiles/sys_update.m b/ProgramFiles/sys_update.m index 565bd78a..ca93c981 100644 --- a/ProgramFiles/sys_update.m +++ b/ProgramFiles/sys_update.m @@ -1,4 +1,4 @@ -function sys_update(sys,shch,progress,handles) +function sys_update(sys,shch,stretch,progress,handles) %Make sure data in the plot file is up to date % declare the data directory @@ -61,7 +61,7 @@ function sys_update(sys,shch,progress,handles) if update.sys_calc sys_calcsystem('calculate',sys); - end + end if exist('progress','var') %Advance the progress bar diff --git a/ProgramFiles/sysplotter.fig b/ProgramFiles/sysplotter.fig index 39d5ed7f..2bfa13b0 100755 Binary files a/ProgramFiles/sysplotter.fig and b/ProgramFiles/sysplotter.fig differ diff --git a/ProgramFiles/sysplotter.m b/ProgramFiles/sysplotter.m index 789065b2..e02b7002 100755 --- a/ProgramFiles/sysplotter.m +++ b/ProgramFiles/sysplotter.m @@ -22,22 +22,23 @@ % Edit the above text to modify the response to help sysplotter -% Last Modified by GUIDE v2.5 09-Sep-2018 15:01:57 +% Last Modified by GUIDE v2.5 06-May-2021 16:25:08 addpath('./Utilities') %path to gui functions addpath(genpath(GetFullPath('sysplotter_gui_fcns')),... - genpath(GetFullPath('sys_calcpath_fcns')),... - genpath(GetFullPath('sys_calcsystem_fcns')),... - genpath(GetFullPath('sys_draw_fcns')), ... - genpath(GetFullPath('sys_update_fcns')),... - genpath(GetFullPath('sysplotter_config_fcns')),... - genpath(GetFullPath('Utilities')),... - genpath(GetFullPath('Animation')),... - genpath(GetFullPath('Physics')),... - genpath(GetFullPath('Geometry')),... - genpath(GetFullPath('GaitOptimization'))); + genpath(GetFullPath('sys_calcpath_fcns')),... + genpath(GetFullPath('sys_calcsystem_fcns')),... + genpath(GetFullPath('sys_draw_fcns')), ... + genpath(GetFullPath('sys_update_fcns')),... + genpath(GetFullPath('sysplotter_config_fcns')),... + genpath(GetFullPath('Utilities')),... + genpath(GetFullPath('Animation')),... + genpath(GetFullPath('Physics')),... + genpath(GetFullPath('Geometry')),... + genpath(GetFullPath('GaitOptimization')),... + genpath(GetFullPath('CoordinateOptimization'))); %%% % Ensure that system files are properly accessible @@ -99,10 +100,6 @@ mkdir(datapath); end addpath(datapath) - - % Point to the Hodge-Helmholtz and reference-point optimization code - addpath(HHpath) - addpath(Refpointpath) % Begin initialization code - DO NOT EDIT gui_Singleton = 1; diff --git a/ProgramFiles/sysplotter_config.fig b/ProgramFiles/sysplotter_config.fig index bcdda22e..9d0c9539 100644 Binary files a/ProgramFiles/sysplotter_config.fig and b/ProgramFiles/sysplotter_config.fig differ diff --git a/ProgramFiles/sysplotter_config.m b/ProgramFiles/sysplotter_config.m index 42b0d9ef..484b501a 100644 --- a/ProgramFiles/sysplotter_config.m +++ b/ProgramFiles/sysplotter_config.m @@ -68,8 +68,6 @@ function sysplotter_config_OpeningFcn(hObject, eventdata, handles, varargin) %#o % Default values for HH and refpoint paths set(handles.inputpathconfig,'String',GetFullPath('../UserFiles/GenericUser')) - set(handles.HodgeHelmholtzconfig,'String','HodgeHelmholtz') - set(handles.Refpointconfig,'String','RefPointOptimizer') set(handles.Colorconfig,'String','sys_draw_fcns/colorsets/color_Red.m') @@ -78,8 +76,6 @@ function sysplotter_config_OpeningFcn(hObject, eventdata, handles, varargin) %#o if exist(configfile,'file') load(configfile); set(handles.inputpathconfig,'String',inputpath) - set(handles.HodgeHelmholtzconfig,'String',HHpath) - set(handles.Refpointconfig,'String',Refpointpath) set(handles.Colorconfig,'String',Colorpath) end diff --git a/ProgramFiles/sysplotter_config_fcns/HodgeHelmholtzselect_Callback.m b/ProgramFiles/sysplotter_config_fcns/HodgeHelmholtzselect_Callback.m deleted file mode 100644 index 7bdd9391..00000000 --- a/ProgramFiles/sysplotter_config_fcns/HodgeHelmholtzselect_Callback.m +++ /dev/null @@ -1,13 +0,0 @@ -% --- Executes on button press in HodgeHelmholtzselect. -function HodgeHelmholtzselect_Callback(hObject, eventdata, handles) -% hObject handle to HodgeHelmholtzselect (see GCBO) -% eventdata reserved - to be defined in a future version of MATLAB -% handles structure with handles and user data (see GUIDATA) - - % Set the directory string to a user-selected path - targetdir = uigetdir(get(handles.HodgeHelmholtzconfig,'String'),'Select directory of configuration files'); - set(handles.HodgeHelmholtzconfig,'String',targetdir); - - - -end diff --git a/ProgramFiles/sysplotter_config_fcns/OKbutton_Callback.m b/ProgramFiles/sysplotter_config_fcns/OKbutton_Callback.m index 7b5c8480..5ff53558 100644 --- a/ProgramFiles/sysplotter_config_fcns/OKbutton_Callback.m +++ b/ProgramFiles/sysplotter_config_fcns/OKbutton_Callback.m @@ -15,13 +15,6 @@ function OKbutton_Callback(hObject, eventdata, handles) shchpath = fullfile(inputpath,'Shape_Changes'); stretchpath = fullfile(inputpath,'Stretches'); datapath = fullfile(inputpath, '/sysplotter_data/'); - - - % Get the location of the Hodge-Helmholtz function - HHpath = get(handles.HodgeHelmholtzconfig,'String'); - - % Get the location of the Refpoint function - Refpointpath = get(handles.Refpointconfig,'String'); % Get the colors to use in plots [~,colorfunction, ~ ] = fileparts2(get(handles.Colorconfig,'String')); @@ -35,7 +28,7 @@ function OKbutton_Callback(hObject, eventdata, handles) propertyfilepath = fullfile(sysplotterpath,get(handles.displayConfigFile,'String')); % Save the path info to a file for sysplotter to refer to - save('sysplotter_config','inputpath','syspath','shchpath','stretchpath','datapath','HHpath','Refpointpath','Colorset','Colorpath','sysplotterpath','propertyfilepath'); + save('sysplotter_config','inputpath','syspath','shchpath','stretchpath','datapath','Colorset','Colorpath','sysplotterpath','propertyfilepath'); % Update the sysplotter_inputpath variable in the workspace assignin('base','sysplotter_inputpath',inputpath); diff --git a/ProgramFiles/sysplotter_config_fcns/Refpointselect_Callback.m b/ProgramFiles/sysplotter_config_fcns/Refpointselect_Callback.m deleted file mode 100644 index bfe90928..00000000 --- a/ProgramFiles/sysplotter_config_fcns/Refpointselect_Callback.m +++ /dev/null @@ -1,12 +0,0 @@ -% --- Executes on button press in Refpointselect. -function Refpointselect_Callback(hObject, eventdata, handles) -% hObject handle to Refpointselect (see GCBO) -% eventdata reserved - to be defined in a future version of MATLAB -% handles structure with handles and user data (see GUIDATA) - - % Set the directory string to a user-selected path - targetdir = uigetdir(get(handles.Refpointconfig,'String'),'Select directory of configuration files'); - set(handles.Refpointconfig,'String',targetdir); - - -end diff --git a/ProgramFiles/sysplotter_config_fcns/propertyDataLinux.mat b/ProgramFiles/sysplotter_config_fcns/propertyDataLinux.mat index 41042e14..60ebb5fe 100644 Binary files a/ProgramFiles/sysplotter_config_fcns/propertyDataLinux.mat and b/ProgramFiles/sysplotter_config_fcns/propertyDataLinux.mat differ diff --git a/ProgramFiles/sysplotter_config_fcns/propertyDataMacOS.mat b/ProgramFiles/sysplotter_config_fcns/propertyDataMacOS.mat index eb0350f9..5ac4d3f4 100644 Binary files a/ProgramFiles/sysplotter_config_fcns/propertyDataMacOS.mat and b/ProgramFiles/sysplotter_config_fcns/propertyDataMacOS.mat differ diff --git a/ProgramFiles/sysplotter_config_fcns/propertyDataWindows.mat b/ProgramFiles/sysplotter_config_fcns/propertyDataWindows.mat index e412bbf2..3f714c64 100644 Binary files a/ProgramFiles/sysplotter_config_fcns/propertyDataWindows.mat and b/ProgramFiles/sysplotter_config_fcns/propertyDataWindows.mat differ diff --git a/ProgramFiles/sysplotter_gui_fcns/GUI_property_loader.m b/ProgramFiles/sysplotter_gui_fcns/GUI_property_loader.m index ebc41ebd..969d519b 100644 --- a/ProgramFiles/sysplotter_gui_fcns/GUI_property_loader.m +++ b/ProgramFiles/sysplotter_gui_fcns/GUI_property_loader.m @@ -16,6 +16,12 @@ function GUI_property_loader(handles,propertyFile) currentProp = handles.(tagNames{j}).(props{i}); % current object property userProp = propertyList.(props{i}){j}; % user defined property + %Stop refresh button from automatically resizing to cover + %export button + if strcmp(tagNames{j},'refresh_gui') + continue + end + % I really need to make this cleaner. Too many if statements if ~isequal(currentProp,userProp) % don't change the position of the window diff --git a/ProgramFiles/sysplotter_gui_fcns/OptimizeButton_Callback.m b/ProgramFiles/sysplotter_gui_fcns/OptimizeButton_Callback.m index 6f614d88..55d9d32f 100644 --- a/ProgramFiles/sysplotter_gui_fcns/OptimizeButton_Callback.m +++ b/ProgramFiles/sysplotter_gui_fcns/OptimizeButton_Callback.m @@ -4,16 +4,5 @@ function OptimizeButton_Callback(hObject, eventdata, handles) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) -% Get the last plot pushbutton used -if isfield(handles.figure1.UserData,'lastpushbutton') - lastpushbutton = handles.figure1.UserData.lastpushbutton; -else - lastpushbutton = 'plotpushbutton1'; -end - - plot_info = plotpushbutton_Callback_optimize(findall(0,'tag',lastpushbutton), eventdata, handles); - -% Execute the gait_gui_draw command - gait_gui_optimize(plot_info(1).axes(1),hObject, eventdata, handles); - waitbar2a(1,handles.progresspanel,'Finished Plotting') +optimization_gui(handles); end \ No newline at end of file diff --git a/ProgramFiles/sysplotter_gui_fcns/export_sys_callback.m b/ProgramFiles/sysplotter_gui_fcns/export_sys_callback.m new file mode 100644 index 00000000..7af6a249 --- /dev/null +++ b/ProgramFiles/sysplotter_gui_fcns/export_sys_callback.m @@ -0,0 +1,45 @@ +% --- Executes on button press in pushbutton22 +function [s,p] = export_sys_callback(hObject, eventdata, handles) +% hObject handle to pushbutton20 (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + + % Get the setup configuration file + configfile = './sysplotter_config'; + load(configfile,'datapath') + + %Get system name from dropdown + system_index = get(handles.systemmenu,'Value'); + system_names = get(handles.systemmenu,'UserData'); + sys = system_names{system_index}; + + %Get path name from dropdown + shch_index = get(handles.shapechangemenu,'Value'); + shch_names = get(handles.shapechangemenu,'UserData'); + shch = shch_names{shch_index}; + + %Get stretch + stretchstate = get(handles.stretchmenu,'Value'); + stretch_names = get(handles.stretchmenu,'UserData'); + stretch = lower(stretch_names(stretchstate)); + + %Update system if out of date + sys_update(sys,shch,stretch,handles.progresspanel,handles) + + %Update waitbar + waitbar2a(.8,handles.progresspanel,'Loading Data'); + + %merge the system and shape change file names into one + plot_data_file = [sys '__' shch]; + + if strcmpi(sys,'default') || strcmpi(shch,'null') + error('Must specify system and path to export'); + end + + %load the system and path data + load(fullfile(datapath, plot_data_file),'s','p'); + assignin('base','s',s); + assignin('base','p',p); + + waitbar2a(1,handles.progresspanel,'Loaded Data'); +end \ No newline at end of file diff --git a/ProgramFiles/sysplotter_gui_fcns/gait_gui_draw/gait_gui_draw_make_shchf.m b/ProgramFiles/sysplotter_gui_fcns/gait_gui_draw/gait_gui_draw_make_shchf.m index effd042d..1a114171 100644 --- a/ProgramFiles/sysplotter_gui_fcns/gait_gui_draw/gait_gui_draw_make_shchf.m +++ b/ProgramFiles/sysplotter_gui_fcns/gait_gui_draw/gait_gui_draw_make_shchf.m @@ -9,9 +9,14 @@ function gait_gui_draw_make_shchf(paramfilename, displayname,n_dim) elseif n_dim==3 fidi = fopen(fullfile(fileparts(which('gait_gui_draw')),... 'gait_gui_draw_template3.txt')); -else - fidi = fopen(fullfile(fileparts(which('gait_gui_draw')),... +elseif n_dim==4 + fidi = fopen(fullfile(fileparts(which('gait_gui_draw')),... 'gait_gui_draw_template4.txt')); +elseif n_dim==5 + fidi = fopen(fullfile(fileparts(which('gait_gui_draw')),... + 'gait_gui_draw_template5.txt')); +else + error('Trying to make an shchf with an unsupported number of dimensions') end % Create the output file diff --git a/ProgramFiles/sysplotter_gui_fcns/gait_gui_draw/gait_gui_draw_template5.txt b/ProgramFiles/sysplotter_gui_fcns/gait_gui_draw/gait_gui_draw_template5.txt new file mode 100644 index 00000000..a71945a6 --- /dev/null +++ b/ProgramFiles/sysplotter_gui_fcns/gait_gui_draw/gait_gui_draw_template5.txt @@ -0,0 +1,110 @@ +function output = AA_SHCHFILENAME(input_mode,pathnames) + + % Default argument + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + % Name the .mat file with the fourier coefficients + paramfile = 'AA_PARAMSNAME'; + + + switch input_mode + + case 'name' + + output = 'AA_DISPLAYNAME'; + + case 'dependency' + + output.dependency = {fullfile(pathnames.shchpath,[paramfile '.mat'])}; + + case 'initialize' + + %%%% + %% + %Path definitions + + % Load the points that the user clicked and the time vector + load(paramfile) + + % Check if start and end are the same point, and set boundary + % conditions accordingly + if alpha1(1)==alpha1(end) && alpha2(1)==alpha2(end) && alpha3(1)==alpha3(end) && alpha4(1)==alpha4(end) && alpha5(1)==alpha5(end) + splinemode = 'periodic'; + else + splinemode = 'complete'; + end + + % Generate spline structures for the selected points + switch splinemode + + case 'periodic' + + % Fit a periodic spline to the selected points; the endslopes are found + % by averaging the positions of the points before and after the join + endslope1 = (alpha1(2)-alpha1(end-1))/(t(end)-t(end-2)); + endslope2 = (alpha2(2)-alpha2(end-1))/(t(end)-t(end-2)); + endslope3 = (alpha3(2)-alpha3(end-1))/(t(end)-t(end-2)); + endslope4 = (alpha4(2)-alpha4(end-1))/(t(end)-t(end-2)); + endslope5 = (alpha5(2)-alpha5(end-1))/(t(end)-t(end-2)); + spline_alpha1 = spline(t,[endslope1;alpha1(:);endslope1]); + spline_alpha2 = spline(t,[endslope2;alpha2(:);endslope2]); + spline_alpha3 = spline(t,[endslope3;alpha3(:);endslope3]); + spline_alpha4 = spline(t,[endslope4;alpha4(:);endslope4]); + spline_alpha5 = spline(t,[endslope5;alpha5(:);endslope5]); + + case 'complete' + + % Fit a non-periodic spline to the selected points + spline_alpha1 = spline(t,alpha1(:)); + spline_alpha2 = spline(t,alpha2(:)); + spline_alpha3 = spline(t,alpha3(:)); + spline_alpha4 = spline(t,alpha4(:)); + spline_alpha5 = spline(t,alpha5(:)); + + end + + % The gait path is now defined by evaluating the two splines at + % specified times + p.phi_def = @(t) [ppval(spline_alpha1,t(:)),ppval(spline_alpha2,t(:)),ppval(spline_alpha3,t(:)),ppval(spline_alpha4,t(:)),ppval(spline_alpha5,t(:))]; + + + % Speed up execution by defining the gait velocity as well (so + % the evaluator doesn't need to numerically take the derivative + % at every time + spline_dalpha1 = ppdiff(spline_alpha1); + spline_dalpha2 = ppdiff(spline_alpha2); + spline_dalpha3 = ppdiff(spline_alpha3); + spline_dalpha4 = ppdiff(spline_alpha4); + spline_dalpha5 = ppdiff(spline_alpha5); + + % The gait path is now defined by evaluating the two splin + % derivatives at specified times + p.dphi_def = @(t) [ppval(spline_dalpha1,t(:)),ppval(spline_dalpha2,t(:)),ppval(spline_dalpha3,t(:)),ppval(spline_dalpha4,t(:)),ppval(spline_dalpha5,t(:))]; + + + %marker locations + p.phi_marker = []; + + %arrows to plot + p.phi_arrows = 2; + + %time to run path + p.time_def = t([1 end]); %#ok + + + %path resolution + p.phi_res = 50; + + p.cBVI_method = 'simple'; + + + %%%% + %Save the shch properties + output = p; + end + +end \ No newline at end of file diff --git a/ProgramFiles/sysplotter_gui_fcns/initialize_plot_windows.m b/ProgramFiles/sysplotter_gui_fcns/initialize_plot_windows.m index b1e1ec8f..7f2933ef 100644 --- a/ProgramFiles/sysplotter_gui_fcns/initialize_plot_windows.m +++ b/ProgramFiles/sysplotter_gui_fcns/initialize_plot_windows.m @@ -5,6 +5,7 @@ box_active,... plot_style,... stretchstate,... + current_stretch,... plot_types,... plot_subtypes,... plot_coordinates,... @@ -25,7 +26,8 @@ 'style',[],... 'CCFtype',[],... 'stretch',[],... - 'axes',[]); + 'axes',[],... + 'stretch_name',[]); @@ -88,6 +90,7 @@ handles,... CCFtype,... stretchstate,... + current_stretch,... source_number_text); % @@ -154,6 +157,7 @@ handles,... CCFtype,... stretchstate,... + current_stretch,... source_number_text); end @@ -188,6 +192,7 @@ handles,... CCFtype,... stretchstate,... + current_stretch,... source_number_text); plot_subtype_active = box_active{idx}{idx2}{idx3}; @@ -327,6 +332,7 @@ handles,... CCFtype,... stretchstate,... + current_stretch,... source_number_text) %set the plots_to_make structure category for this plot type @@ -349,7 +355,12 @@ error('Unexpected value for CCF appearance toggle') end - + +% if strcmp(current_stretch,'metric_surface') +% +% plots_to_make_i.style = 'pcolor'; +% +% end % Decide if monocolor or a cycling color set should be used for % multi-line plots switch get(handles.(['color' source_number_text]),'Value') @@ -375,5 +386,6 @@ % future, this will pass on higher values into the plotting % function) plots_to_make_i.stretch = stretchstate-1; + plots_to_make_i.stretch_name = current_stretch; end \ No newline at end of file diff --git a/ProgramFiles/sysplotter_gui_fcns/optimization_gui.fig b/ProgramFiles/sysplotter_gui_fcns/optimization_gui.fig new file mode 100644 index 00000000..246866a3 Binary files /dev/null and b/ProgramFiles/sysplotter_gui_fcns/optimization_gui.fig differ diff --git a/ProgramFiles/sysplotter_gui_fcns/optimization_gui.m b/ProgramFiles/sysplotter_gui_fcns/optimization_gui.m new file mode 100644 index 00000000..055dba34 --- /dev/null +++ b/ProgramFiles/sysplotter_gui_fcns/optimization_gui.m @@ -0,0 +1,93 @@ +function varargout = optimization_gui(varargin) +% OPTIMIZATION_GUI MATLAB code for optimization_gui.fig +% OPTIMIZATION_GUI, by itself, creates a new OPTIMIZATION_GUI or raises the existing +% singleton*. +% +% H = OPTIMIZATION_GUI returns the handle to a new OPTIMIZATION_GUI or the handle to +% the existing singleton*. +% +% OPTIMIZATION_GUI('CALLBACK',hObject,eventData,handles,...) calls the local +% function named CALLBACK in OPTIMIZATION_GUI.M with the given input arguments. +% +% OPTIMIZATION_GUI('Property','Value',...) creates a new OPTIMIZATION_GUI or raises the +% existing singleton*. Starting from the left, property value pairs are +% applied to the GUI before optimization_gui_OpeningFcn gets called. An +% unrecognized property name or invalid value makes property application +% stop. All inputs are passed to optimization_gui_OpeningFcn via varargin. +% +% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one +% instance to run (singleton)". +% +% See also: GUIDE, GUIDATA, GUIHANDLES + +% Edit the above text to modify the response to help optimization_gui + +% Last Modified by GUIDE v2.5 28-Jul-2020 11:26:09 + +% Begin initialization code - DO NOT EDIT +gui_Singleton = 1; +gui_State = struct('gui_Name', mfilename, ... + 'gui_Singleton', gui_Singleton, ... + 'gui_OpeningFcn', @optimization_gui_OpeningFcn, ... + 'gui_OutputFcn', @optimization_gui_OutputFcn, ... + 'gui_LayoutFcn', [] , ... + 'gui_Callback', []); +if nargin && ischar(varargin{1}) + gui_State.gui_Callback = str2func(varargin{1}); +end + +if nargout + [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); +else + gui_mainfcn(gui_State, varargin{:}); +end +% End initialization code - DO NOT EDIT + + +% --- Executes just before optimization_gui is made visible. +function optimization_gui_OpeningFcn(hObject, eventdata, handles, varargin) +% This function has no output args, see OutputFcn. +% hObject handle to figure +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +% varargin command line arguments to optimization_gui (see VARARGIN) + +handles.main_gui = varargin{1}; +% Choose default command line output for optimization_gui +handles.output = hObject; + +% Update handles structure +guidata(hObject, handles); + +% UIWAIT makes optimization_gui wait for user response (see UIRESUME) +% uiwait(handles.figure1); + + +% --- Outputs from this function are returned to the command line. +function varargout = optimization_gui_OutputFcn(hObject, eventdata, handles) +% varargout cell array for returning output args (see VARARGOUT); +% hObject handle to figure +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) + +% Get default command line output from handles structure +varargout{1} = handles.output; + + +% --- Executes on button press in optimize_pushbutton. +function optimize_pushbutton_Callback(hObject, eventdata, handles) +% hObject handle to optimize_pushbutton (see GCBO) +% eventdata reserved - to be defined in a future version of MATLAB +% handles structure with handles and user data (see GUIDATA) +% Get the last plot pushbutton used +if isfield(handles.main_gui.figure1.UserData,'lastpushbutton') + lastpushbutton = handles.main_gui.figure1.UserData.lastpushbutton; +else + lastpushbutton = 'plotpushbutton1'; +end + +plot_info = plotpushbutton_Callback_optimize(findall(0,'tag',lastpushbutton), eventdata, handles.main_gui); + +% Execute the gait_gui_optimize command +gait_gui_optimize(plot_info(1).axes(1),hObject, eventdata, handles); +waitbar2a(1,handles.main_gui.progresspanel,'Finished Plotting') diff --git a/ProgramFiles/sysplotter_gui_fcns/plotpushbutton_Callback.m b/ProgramFiles/sysplotter_gui_fcns/plotpushbutton_Callback.m index ee4649c1..45a6571a 100644 --- a/ProgramFiles/sysplotter_gui_fcns/plotpushbutton_Callback.m +++ b/ProgramFiles/sysplotter_gui_fcns/plotpushbutton_Callback.m @@ -43,13 +43,25 @@ % Get the state of the Stretch menu (coordinate conversion to flatten % metric) stretchstate = get(handles.stretchmenu,'Value'); - - +stretch_names = get(handles.stretchmenu,'UserData'); +current_stretch = lower(stretch_names(stretchstate)); + +% if stretchstate==1 +% current_stretch='null'; +% end +% if stretchstate==2 +% current_stretch='metric_stretch'; +% end +% if stretchstate==3 +% current_stretch='metric_surface'; +% end +% % Initialize the plot windows plots_to_make = initialize_plot_windows(box_names,... box_active,... plot_style,... stretchstate,... + current_stretch,... plot_types,... plot_subtypes,... plot_coordinates,... @@ -83,7 +95,7 @@ %Call the draw function % test_plot(plots_to_make,current_system,current_shch) -plot_info = sys_draw(plots_to_make,current_system,current_shch,handles.progresspanel,1,resolution,handles); +plot_info = sys_draw(plots_to_make,current_system,current_shch,current_stretch,handles.progresspanel,1,resolution,handles); %Show full progress bar waitbar2a(1,handles.progresspanel,'Finished plotting') diff --git a/ProgramFiles/sysplotter_gui_fcns/plotpushbutton_Callback_optimize.m b/ProgramFiles/sysplotter_gui_fcns/plotpushbutton_Callback_optimize.m index 5be84634..fe39153b 100644 --- a/ProgramFiles/sysplotter_gui_fcns/plotpushbutton_Callback_optimize.m +++ b/ProgramFiles/sysplotter_gui_fcns/plotpushbutton_Callback_optimize.m @@ -43,6 +43,9 @@ % Get the state of the Stretch menu (coordinate conversion to flatten % metric) stretchstate = get(handles.stretchmenu,'Value'); +stretch_names = get(handles.stretchmenu,'UserData'); +current_stretch = stretch_names(stretchstate); + % Initialize the plot windows @@ -50,6 +53,7 @@ box_active,... plot_style,... stretchstate,... + current_stretch,... plot_types,... plot_subtypes,... plot_coordinates,... @@ -83,7 +87,7 @@ % current_shch='null'; %Call the draw function % test_plot(plots_to_make,current_system,current_shch) -plot_info = sys_draw(plots_to_make,current_system,current_shch,handles.progresspanel,1,resolution,handles); +plot_info = sys_draw(plots_to_make,current_system,current_shch,current_stretch,handles.progresspanel,1,resolution,handles); %Show full progress bar waitbar2a(0.8,handles.progresspanel,'Optimizing') diff --git a/ProgramFiles/sysplotter_gui_fcns/refresh_runinfo_Callback.m b/ProgramFiles/sysplotter_gui_fcns/refresh_runinfo_Callback.m index 761f5fca..b7a8872d 100644 --- a/ProgramFiles/sysplotter_gui_fcns/refresh_runinfo_Callback.m +++ b/ProgramFiles/sysplotter_gui_fcns/refresh_runinfo_Callback.m @@ -23,7 +23,6 @@ shch = shchlist{shchval}; - placeholders = {'default','start_recent','end_recent'}; if any(strcmp(sys,placeholders)) @@ -39,7 +38,7 @@ % Set the dependency and update markers components = fieldnames(dep_update); - for i = 1:length(components), + for i = 1:length(components) if dep_update.(components{i}) set(handles.([components{i} '_dep_indicator']),'BackgroundColor',Colorset.spot) @@ -53,6 +52,6 @@ set(handles.([components{i} '_run_indicator']),'BackgroundColor','w') end - end - + end + end \ No newline at end of file diff --git a/ProgramFiles/sysplotter_gui_fcns/stretchmenu_CreateFcn.m b/ProgramFiles/sysplotter_gui_fcns/stretchmenu_CreateFcn.m index ccf39bd7..b8b93363 100644 --- a/ProgramFiles/sysplotter_gui_fcns/stretchmenu_CreateFcn.m +++ b/ProgramFiles/sysplotter_gui_fcns/stretchmenu_CreateFcn.m @@ -10,7 +10,7 @@ function stretchmenu_CreateFcn(hObject, eventdata, handles) % Set the menu options to be unstretched or metric-stretched - set(hObject,'String',{'No stretch';'Metric stretch'}); + set(hObject,'String',{'No stretch';'Metric stretch';'Metric surface'}); diff --git a/UserFiles/GenericUser/Shape_Changes/shchf_3d_ellipse.m b/UserFiles/GenericUser/Shape_Changes/shchf_3d_ellipse.m new file mode 100644 index 00000000..9d0c2195 --- /dev/null +++ b/UserFiles/GenericUser/Shape_Changes/shchf_3d_ellipse.m @@ -0,0 +1,76 @@ +function output = shchf_3d_ellipse(input_mode,pathnames) + + % Default argument + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + switch input_mode + + case 'name' + + output = '3D Ellipse, Tunable'; + + case 'dependency' + + output.dependency = {}; + + case 'initialize' + + %%%% + % Filename to save to + + %% + %Path definitions + + %path definition + p.phi_def = @strokedef; + + %marker locations + p.phi_marker = []; + + %arrows to plot + p.phi_arrows = 2; + + %time to run path + p.time_def = [0 2*pi]; + + %p.cBVI_method{1}{1} = 'simple'; + + %path resolution + p.phi_res = 100; + + + %%%% + %Output the path properties + output = p; + end + +end + +function [stroke] = strokedef(t) + + t = -t(:); + + yaw = 80 * (pi/180); + pitch = 20 * (pi/180); + roll = 55 * (pi/180); + + x_o = 0; + y_o = 0; + z_o = .2; + + yaw_R = [cos(yaw),sin(yaw),0,0;-sin(yaw),cos(yaw),0,0;0,0,1,0;0,0,0,1]; + pitch_R = [1,0,0,0;0,cos(pitch),sin(pitch),0;0,-sin(pitch),cos(pitch),0;0,0,0,1]; + roll_R = [cos(roll),0,-sin(roll),0;0,1,0,0;sin(roll),0,cos(roll),0;0,0,0,1]; + + a=12; + b=7; + + stroke=[-a*cos(t)+x_o,-b*sin(t)+y_o,zeros(numel(t),1)+z_o,ones(numel(t),1)]*yaw_R*pitch_R*roll_R; + stroke = stroke(:,1:3); + + +end \ No newline at end of file diff --git a/UserFiles/GenericUser/Shape_Changes/shchf_circle_1p0_offset.m b/UserFiles/GenericUser/Shape_Changes/shchf_circle_1p0_offset.m new file mode 100644 index 00000000..ebede2cd --- /dev/null +++ b/UserFiles/GenericUser/Shape_Changes/shchf_circle_1p0_offset.m @@ -0,0 +1,63 @@ +function output = shchf_circle_1p0_offset(input_mode,pathnames) + + % Default argument + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + switch input_mode + + case 'name' + + output = 'Circle Stroke, 1.0 amplitude offset for turning'; + + case 'dependency' + + output.dependency = {}; + + case 'initialize' + + %%%% + % Filename to save to + + %% + %Path definitions + + %path definition + p.phi_def = @strokedef; + + %marker locations + p.phi_marker = []; + + %arrows to plot + p.phi_arrows = 2; + + %time to run path + p.time_def = [0 2*pi]; + + p.cBVI_method = 'simple'; + + %path resolution + p.phi_res = 100; + + + %%%% + %Output the path properties + output = p; + end + +end + +function [stroke] = strokedef(t) + + t = -t(:)'; + + Rot=sqrt(2)/2*[1 -1;1 1]; + a=1.0; + + stroke=(Rot*[-a*cos(t);-a*sin(t)]+[.5;.5])'; + + +end \ No newline at end of file diff --git a/UserFiles/GenericUser/Shape_Changes/shchf_circle_p25.m b/UserFiles/GenericUser/Shape_Changes/shchf_circle_p25.m new file mode 100644 index 00000000..bbaa6fbe --- /dev/null +++ b/UserFiles/GenericUser/Shape_Changes/shchf_circle_p25.m @@ -0,0 +1,63 @@ +function output = shchf_circle_p25(input_mode,pathnames) + + % Default argument + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + switch input_mode + + case 'name' + + output = 'Circle Stroke, 0.25 amplitude'; + + case 'dependency' + + output.dependency = {}; + + case 'initialize' + + %%%% + % Filename to save to + + %% + %Path definitions + + %path definition + p.phi_def = @strokedef; + + %marker locations + p.phi_marker = []; + + %arrows to plot + p.phi_arrows = 2; + + %time to run path + p.time_def = [0 2*pi]; + + p.cBVI_method = 'simple'; + + %path resolution + p.phi_res = 100; + + + %%%% + %Output the path properties + output = p; + end + +end + +function [stroke] = strokedef(t) + + t = -t(:)'; + + Rot=sqrt(2)/2*[1 -1;1 1]; + a=.25; + + stroke=(Rot*[-a*cos(t);-a*sin(t)])'; + + +end \ No newline at end of file diff --git a/UserFiles/GenericUser/Shape_Changes/shchf_circle_p25_offset.m b/UserFiles/GenericUser/Shape_Changes/shchf_circle_p25_offset.m new file mode 100644 index 00000000..dfe50683 --- /dev/null +++ b/UserFiles/GenericUser/Shape_Changes/shchf_circle_p25_offset.m @@ -0,0 +1,63 @@ +function output = shchf_circle_p25_offset(input_mode,pathnames) + + % Default argument + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + switch input_mode + + case 'name' + + output = 'Circle Stroke, 0.25 amplitude offset for turning'; + + case 'dependency' + + output.dependency = {}; + + case 'initialize' + + %%%% + % Filename to save to + + %% + %Path definitions + + %path definition + p.phi_def = @strokedef; + + %marker locations + p.phi_marker = []; + + %arrows to plot + p.phi_arrows = 2; + + %time to run path + p.time_def = [0 2*pi]; + + p.cBVI_method = 'simple'; + + %path resolution + p.phi_res = 100; + + + %%%% + %Output the path properties + output = p; + end + +end + +function [stroke] = strokedef(t) + + t = -t(:)'; + + Rot=sqrt(2)/2*[1 -1;1 1]; + a=.5; + + stroke=(Rot*[-a*cos(t);-a*sin(t)]+[1.5;1.5])'; + + +end \ No newline at end of file diff --git a/UserFiles/GenericUser/Shape_Changes/shchf_diffdrive_example.m b/UserFiles/GenericUser/Shape_Changes/shchf_diffdrive_example.m deleted file mode 100644 index f7b0205d..00000000 --- a/UserFiles/GenericUser/Shape_Changes/shchf_diffdrive_example.m +++ /dev/null @@ -1,81 +0,0 @@ -function output = shchf_diffdrive_example(input_mode,pathnames) - - % Default argument - if ~exist('input_mode','var') - - input_mode = 'initialize'; - - end - - switch input_mode - - case 'name' - - output = 'Diffdrive Example Motion'; - - case 'dependency' - - output.dependency = {}; - - case 'initialize' - - %% - %Path definitions - - % Multiple shape changes (or piecewise definitions of shape changes) can be - % specified in a single file by making a nested - % cell array for phi_def. The outer cell array contains - % individual shape change, and the inner array contains segments of - % the shape change. Here, we have one shape change made up of two - % segments. - % - % Shape change properties can be - % specified for individual shape changes by making the relevant fields - % likewise cell structures, but a single value will be dealt - % out to all the shape changes and segments. - p.phi_def = {{@drive_forward, @turn_in_place}}; - - - %marker locations - p.phi_marker = []; % No marker on this path (can put, e.g. endpoints of path if desired) - - %arrows to plot - p.phi_arrows = {{1,2}}; %put one direction arrow on the first segment, two on the second - - %time to run path - p.time_def = [0 1]; % Duration of each segment - - % With a closed-loop gait, enabling this line takes the area - % integral of the Constraint Curvature Function (note that this - % is currently a slow operation. - %p.cBVI_method{1}{1} = 'simple'; - - %number of points in each path. - p.phi_res = 20; - - - %%%% - %Output the path properties - output = p; - end - -end - -function [alpha] = drive_forward(t) - - t = t(:); - - alpha = [-1+t -1+t]; - - -end - - -function [alpha] = turn_in_place(t) - - t = t(:); - - alpha = [-t t]; - - -end \ No newline at end of file diff --git a/UserFiles/GenericUser/Systems/sysf_constantcurv2_lowRe.m b/UserFiles/GenericUser/Systems/sysf_constantcurv2_lowRe.m index 6482d1b6..6957238d 100644 --- a/UserFiles/GenericUser/Systems/sysf_constantcurv2_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_constantcurv2_lowRe.m @@ -35,12 +35,8 @@ % along the backbone s.geometry.type = 'curvature basis'; - % The specific basis functions are those for a serpenoid curve, - % in which the curvature varies sinusoidally along the length - % of the body. This function is normalized such that the body length - % is taken as one unit long. For this system, we use a - % wavelength equal to the body length. - n_waves = 1; + % The specific basis functions are those for a + % piecewise-constant curvature system s.geometry.function = {@(s)constant_curvature_1(s);@(s)constant_curvature_2(s)}; % Total length of the swimmer, in real units @@ -55,11 +51,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(size(s.geometry.function)); - [s.visual.grid{:}] = ndgrid([-1 -0.5 0 0.5 1]*6); + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 -0.5 0 0.5 1]*5; %%% @@ -99,6 +93,9 @@ s.tic_locs.x = [-1 0 1]*6; s.tic_locs.y = [-1 0 1]*6; %%%% + + % Set system type variable for gait optimization + s.system_type = 'drag'; %Save the system properties diff --git a/UserFiles/GenericUser/Systems/sysf_constantcurv3_lowRe.m b/UserFiles/GenericUser/Systems/sysf_constantcurv3_lowRe.m index af86ba2a..ef6b16e6 100644 --- a/UserFiles/GenericUser/Systems/sysf_constantcurv3_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_constantcurv3_lowRe.m @@ -35,12 +35,8 @@ % along the backbone s.geometry.type = 'curvature basis'; - % The specific basis functions are those for a serpenoid curve, - % in which the curvature varies sinusoidally along the length - % of the body. This function is normalized such that the body length - % is taken as one unit long. For this system, we use a - % wavelength equal to the body length. - n_waves = 1; + % The specific basis functions are those for a + % piecewise-constant curvature system s.geometry.function = {@(s)constant_curvature_3_1(s);@(s)constant_curvature_3_2(s);@(s)constant_curvature_3_3(s)}; % Total length of the swimmer, in real units @@ -55,12 +51,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(size(s.geometry.function)); - [s.visual.grid{:}] = ndgrid([-1 -0.5 0 0.5 1]*6); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 -0.5 0 0.5 1]*5; %%% @@ -89,16 +82,19 @@ s.grid_range = [-1,1,-1,1,-1,1]*6; %densities for various operations - s.density.vector = [21 21 21]; %density to display vector field - s.density.scalar = [21 21 21]; %density to display scalar functions - s.density.eval = [21 21 21]; %density for function evaluations - s.density.metric_eval = [1 1 1]*15; + s.density.vector = [11 11 11]; %density to display vector field + s.density.scalar = [11 11 11]; %density to display scalar functions + s.density.eval = [11 11 21]; %density for function evaluations + s.density.metric_eval = [1 1 1]*11; s.density.finite_element=11; %shape space tic locations s.tic_locs.x = [-1 0 1]*6; s.tic_locs.y = [-1 0 1]*6; %%%% + + % Set system type variable for gait optimization + s.system_type = 'drag'; %Save the system properties diff --git a/UserFiles/GenericUser/Systems/sysf_constantcurv4_lowRe.m b/UserFiles/GenericUser/Systems/sysf_constantcurv4_lowRe.m index cebe4024..3419fa79 100644 --- a/UserFiles/GenericUser/Systems/sysf_constantcurv4_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_constantcurv4_lowRe.m @@ -35,12 +35,8 @@ % along the backbone s.geometry.type = 'curvature basis'; - % The specific basis functions are those for a serpenoid curve, - % in which the curvature varies sinusoidally along the length - % of the body. This function is normalized such that the body length - % is taken as one unit long. For this system, we use a - % wavelength equal to the body length. - n_waves = 1; + % The specific basis functions are those for a + % piecewise-constant curvature system s.geometry.function = {@(s)constant_curvature_4_1(s);@(s)constant_curvature_4_2(s);@(s)constant_curvature_4_3(s);@(s)constant_curvature_4_4(s)}; % Total length of the swimmer, in real units @@ -55,12 +51,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(size(s.geometry.function)); - [s.visual.grid{:}] = ndgrid([-1 -0.5 0 0.5 1]*6); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 -0.5 0 0.5 1]*5; %%% @@ -86,7 +79,7 @@ % Processing details %Range over which to evaluate connection - s.grid_range = [-1,1,-1,1,-1,1,-1,1]*8; + s.grid_range = [-1,1,-1,1,-1,1,-1,1]*6; %densities for various operations s.density.vector = [13 13 13 13]; %density to display vector field @@ -98,6 +91,9 @@ s.tic_locs.x = [-1 0 1]*6; s.tic_locs.y = [-1 0 1]*6; %%%% + + % Set system type variable for gait optimization + s.system_type = 'drag'; %Save the system properties diff --git a/UserFiles/GenericUser/Systems/sysf_diffdrive.m b/UserFiles/GenericUser/Systems/sysf_diffdrive.m index 337b2696..887b4ece 100644 --- a/UserFiles/GenericUser/Systems/sysf_diffdrive.m +++ b/UserFiles/GenericUser/Systems/sysf_diffdrive.m @@ -44,7 +44,6 @@ s.density.eval = [21 21]; %density for function evaluations s.density.finite_element=31; - %%% %Display parameters @@ -56,8 +55,12 @@ %Don't optimize the reference point (turn this off for %non-carlike systems) s.xy_no_opt = 1; + + % Set system type variable for gait optimization + s.system_type = 'inertia'; %%%% + %Output the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_five_link_HighRe.m b/UserFiles/GenericUser/Systems/sysf_five_link_HighRe.m new file mode 100644 index 00000000..6f73a5ae --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_five_link_HighRe.m @@ -0,0 +1,106 @@ +function output = sysf_five_link_HighRe(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe ideal swimmer: 5-link ellipses'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1 1 1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {'ellipse','ellipse','ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.1); + s.geometry.link_shape_parameters = {st,st,st,st,st}; + %s.geometry.modes = [sqrt(2)/2,-sqrt(2)/2;sqrt(2)/2,sqrt(2)/2]; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + + + %Functional Local connection and dissipation metric + + + s.A = @(alpha1,alpha2,alpha3,alpha4) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2,alpha3,alpha4]); % Joint angles + + s.metric = @(alpha1,alpha2,alpha3,alpha4) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2,alpha3,alpha4]); + %s.metric = @(alpha1,alpha2,alpha3,alpha4) eye(4); + +% % TODO: These should probably be calculated as part of a larger +% % wrapping function that's meant to return M and C matrices for +% % a set of points +% % s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% % s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); +% s.M_alpha = @(alpha1,alpha2) mass_matrix(s.geometry,s.physics,[alpha1,alpha2]); +% s.dM_alphadalpha = @(alpha1,alpha2,A_eval,A_grid) shape_partial_mass(s.geometry,s.physics,[alpha1,alpha2],A_eval,A_grid); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1,-1,1,-1,1]*2.5; + + %densities for various operations + sample_density = 8; + sd = sample_density; + s.density.vector = [sd sd sd sd]; %density to display vector field + s.density.scalar = [sd sd sd sd]; %density to display scalar functions + s.density.eval = [sd sd sd sd]; %density for function evaluations + %Bump up this value + s.density.metric_eval = [sd sd sd sd]; %density for metric evaluation +% s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.metric_display = [7 7 7 7]; +% s.density.coriolis_eval = [31 31]; + s.density.finite_element=sd; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_five_link_lowRe.m b/UserFiles/GenericUser/Systems/sysf_five_link_lowRe.m index 1bf3a241..42e22805 100644 --- a/UserFiles/GenericUser/Systems/sysf_five_link_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_five_link_lowRe.m @@ -36,11 +36,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); - [s.visual.grid{:}] = ndgrid([-1 0 1]); + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; %%% @@ -80,6 +78,8 @@ s.tic_locs.x = [-1 0 1]*1; s.tic_locs.y = [-1 0 1]*1; + % Set system type variable for gait optimization + s.system_type = 'drag'; %%%% %Save the system properties diff --git a/UserFiles/GenericUser/Systems/sysf_flapper_highRe.m b/UserFiles/GenericUser/Systems/sysf_flapper_highRe.m new file mode 100644 index 00000000..02fa4314 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_flapper_highRe.m @@ -0,0 +1,103 @@ +function output = sysf_three_link_HighRe(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = '5-Link Flapper with Passive Joints'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1 1 1]; + s.geometry.activelinks = [1 1 1 1 1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {'ellipse','ellipse','ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.1); + s.geometry.link_shape_parameters = {st,st,st,st,st}; + s.geometry.modes = [1,0;0,1;0,1;1,0]; + s.geometry.linkSeparation = .1; + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + s.physics.interaction = 'off'; + + + %Functional Local connection and dissipation metric + + s.A = @(alpha1,alpha2) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + s.metric = @(alpha1,alpha2)eye(2);%@(alpha1,alpha2) LowRE_dissipation_metric(... +% s.geometry,... % Geometry of body +% s.physics,... % Physics properties +% [alpha1,alpha2]); % Joint angles + + % TODO: These should probably be calculated as part of a larger + % wrapping function that's meant to return M and C matrices for + % a set of points +% s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1.2,1.2,-2,2]; + + %densities for various operations + s.density.vector = [21 21]; %density to display vector field + s.density.scalar = [51 51]; %density to display scalar functions + s.density.eval = [31 31]; %density for function evaluations + s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.coriolis_eval = [31 31]; + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + % System type flag + s.system_type = 'inertia'; + + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_flapper_noModes.asv b/UserFiles/GenericUser/Systems/sysf_flapper_noModes.asv new file mode 100644 index 00000000..149a61dd --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_flapper_noModes.asv @@ -0,0 +1,103 @@ +function output = flappe(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = '5-Link Flapper with Passive Joints'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1 1 1]; + s.geometry.activelinks = [1 1 1 1 1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {'ellipse','ellipse','ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.1); + s.geometry.link_shape_parameters = {st,st,st,st,st}; + s.geometry.modes = [1,0;0,1;0,1;1,0]; + s.geometry.linkSeparation = .1; + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + s.physics.interaction = 'off'; + + + %Functional Local connection and dissipation metric + + s.A = @(alpha1,alpha2) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + s.metric = @(alpha1,alpha2)eye(2);%@(alpha1,alpha2) LowRE_dissipation_metric(... +% s.geometry,... % Geometry of body +% s.physics,... % Physics properties +% [alpha1,alpha2]); % Joint angles + + % TODO: These should probably be calculated as part of a larger + % wrapping function that's meant to return M and C matrices for + % a set of points +% s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1.2,1.2,-2,2]; + + %densities for various operations + s.density.vector = [21 21]; %density to display vector field + s.density.scalar = [51 51]; %density to display scalar functions + s.density.eval = [31 31]; %density for function evaluations + s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.coriolis_eval = [31 31]; + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + % System type flag + s.system_type = 'inertia'; + + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_flapper_noModes.m b/UserFiles/GenericUser/Systems/sysf_flapper_noModes.m new file mode 100644 index 00000000..aa9c2173 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_flapper_noModes.m @@ -0,0 +1,103 @@ +function output = sysf_flapper_noModes(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = '5-Link Flapper, No Joint Modes'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1 1 1]; + s.geometry.activelinks = [1 1 1 1 1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {'ellipse','ellipse','ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.1); + s.geometry.link_shape_parameters = {st,st,st,st,st}; + %s.geometry.modes = [1,0;0,1;0,1;1,0]; + s.geometry.linkSeparation = .1; + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + + %Functional Local connection and dissipation metric + + s.A = @(alpha1,alpha2,alpha3,alpha4) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2,alpha3,alpha4]); % Joint angles + + s.metric = @(alpha1,alpha2,alpha3,alpha4)eye(4); + %@(alpha1,alpha2) LowRE_dissipation_metric(... +% s.geometry,... % Geometry of body +% s.physics,... % Physics properties +% [alpha1,alpha2]); % Joint angles + + % TODO: These should probably be calculated as part of a larger + % wrapping function that's meant to return M and C matrices for + % a set of points +% s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-2,2,-2,2,-2,2,-2,2]; + + md = 8; + %densities for various operations + s.density.vector = [md md md md]; %density to display vector field + s.density.scalar = [md md md md]; %density to display scalar functions + s.density.eval = [md md md md]; %density for function evaluations + s.density.metric_eval = [md md md md]; %density for metric evaluation + s.density.mass_eval = [md md md md]; % density for mass matrix evaluation + s.density.coriolis_eval = [md md md md]; + s.density.finite_element=md; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + % System type flag + s.system_type = 'inertia'; + + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_four_link_HighRe.asv b/UserFiles/GenericUser/Systems/sysf_four_link_HighRe.asv new file mode 100644 index 00000000..13183397 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_four_link_HighRe.asv @@ -0,0 +1,104 @@ +function output = sysf_four_link_HighRe(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe ideal swimmer: 3-link ellipses'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1 1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {'ellipse','ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.1); + s.geometry.link_shape_parameters = {st,st,st,st}; + %s.geometry.modes = [sqrt(2)/2,-sqrt(2)/2;sqrt(2)/2,sqrt(2)/2]; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + + + %Functional Local connection and dissipation metric + + + s.A = @(alpha1,alpha2,alpha3) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2,alpha3]); % Joint angles + + %s.metric = @(alpha1,alpha2,alpha3) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2,alpha3]); + s.metric = @(alpha1,alpha2) eye(2); + +% % TODO: These should probably be calculated as part of a larger +% % wrapping function that's meant to return M and C matrices for +% % a set of points +% % s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% % s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); +% s.M_alpha = @(alpha1,alpha2) mass_matrix(s.geometry,s.physics,[alpha1,alpha2]); +% s.dM_alphadalpha = @(alpha1,alpha2,A_eval,A_grid) shape_partial_mass(s.geometry,s.physics,[alpha1,alpha2],A_eval,A_grid); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1]*2.5; + + %densities for various operations + s.density.vector = [21 21]; %density to display vector field + s.density.scalar = [51 51]; %density to display scalar functions + s.density.eval = [31 31]; %density for function evaluations + %Bump up this value + s.density.metric_eval = [31 31]; %density for metric evaluation +% s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.metric_display = [7 7]; +% s.density.coriolis_eval = [31 31]; + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_four_link_HighRe.m b/UserFiles/GenericUser/Systems/sysf_four_link_HighRe.m new file mode 100644 index 00000000..e2c95a36 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_four_link_HighRe.m @@ -0,0 +1,106 @@ +function output = sysf_four_link_HighRe(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe ideal swimmer: 4-link ellipses'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1 1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {'ellipse','ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.1); + s.geometry.link_shape_parameters = {st,st,st,st}; + %s.geometry.modes = [sqrt(2)/2,-sqrt(2)/2;sqrt(2)/2,sqrt(2)/2]; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + + + %Functional Local connection and dissipation metric + + + s.A = @(alpha1,alpha2,alpha3) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2,alpha3]); % Joint angles + + s.metric = @(alpha1,alpha2,alpha3) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2,alpha3]); + %s.metric = @(alpha1,alpha2,alpha3) eye(3); + +% % TODO: These should probably be calculated as part of a larger +% % wrapping function that's meant to return M and C matrices for +% % a set of points +% % s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% % s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); +% s.M_alpha = @(alpha1,alpha2) mass_matrix(s.geometry,s.physics,[alpha1,alpha2]); +% s.dM_alphadalpha = @(alpha1,alpha2,A_eval,A_grid) shape_partial_mass(s.geometry,s.physics,[alpha1,alpha2],A_eval,A_grid); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1,-1,1]*2.5; + + %densities for various operations + sample_density = 12; + sd = sample_density; + s.density.vector = [sd sd sd]; %density to display vector field + s.density.scalar = [sd sd sd]; %density to display scalar functions + s.density.eval = [sd sd sd]; %density for function evaluations + %Bump up this value + s.density.metric_eval = [sd sd sd]; %density for metric evaluation +% s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.metric_display = [7 7 7]; +% s.density.coriolis_eval = [31 31]; + s.density.finite_element=sd; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_four_link_floating_ellipse.m b/UserFiles/GenericUser/Systems/sysf_four_link_floating_ellipse.m new file mode 100644 index 00000000..f720bb76 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_four_link_floating_ellipse.m @@ -0,0 +1,103 @@ +function output = sysf_four_link_floating_ellipse(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'Inertial floating: 4-link ellipses'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1 1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {'ellipse','ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.01); + s.geometry.link_shape_parameters = {st,st,st,st}; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]*1; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 0; + + + %Functional Local connection and dissipation metric + + s.A = @(alpha1,alpha2,alpha3) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2,alpha3]); % Joint angles + + s.metric = @(alpha1,alpha2,alpha3) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2,alpha3]); + %s.metric = @(alpha1,alpha2,alpha3) eye(3); + + % TODO: These should probably be calculated as part of a larger + % wrapping function that's meant to return M and C matrices for + % a set of points +% s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); +% s.M_alpha = @(alpha1,alpha2) mass_matrix(s.geometry,s.physics,[alpha1,alpha2]); +% s.dM_alphadalpha = @(alpha1,alpha2,A_eval,A_grid) shape_partial_mass(s.geometry,s.physics,[alpha1,alpha2],A_eval,A_grid); +% + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1,-1,1]*2*pi/3; + + %densities for various operations + sampleDensity = 12; + sd = sampleDensity; + s.density.vector = [sd sd sd]; %density to display vector field + s.density.scalar = [sd sd sd]; %density to display scalar functions + s.density.eval = [sd sd sd]; %density for function evaluations + s.density.metric_eval = [sd sd sd]; %density for metric evaluation + s.density.mass_eval = [sd sd sd]; % density for mass matrix evaluation + s.density.coriolis_eval = [sd sd sd]; + s.density.metric_display = [sd sd sd]; + s.density.finite_element=sd; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_four_link_lowRe.m b/UserFiles/GenericUser/Systems/sysf_four_link_lowRe.m index ed427112..5a353a3d 100755 --- a/UserFiles/GenericUser/Systems/sysf_four_link_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_four_link_lowRe.m @@ -35,12 +35,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); - [s.visual.grid{:}] = ndgrid([-1 0 1]); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; %%% %%%%%% @@ -79,6 +76,8 @@ s.tic_locs.x = [-1 0 1]*1; s.tic_locs.y = [-1 0 1]*1; + % Set system type variable for gait optimization + s.system_type = 'drag'; %%%% %Save the system properties diff --git a/UserFiles/GenericUser/Systems/sysf_four_link_lowRe_CSmodes.m b/UserFiles/GenericUser/Systems/sysf_four_link_lowRe_CSmodes.m new file mode 100644 index 00000000..bb090f18 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_four_link_lowRe_CSmodes.m @@ -0,0 +1,93 @@ +function output = sysf_four_link_lowRe_CSmodes(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'Viscous Swimmer: 4-link C&S modes'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/LowReynoldsRFT/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1 1]; + s.geometry.modes = [1 1;1 0;1 -1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. The code below uses properties of cell + % arrays to automatically match the dimensionality of the grid + % with the number of shape basis functions in use + s.visual.grid = cell(size(s.geometry.modes,1)-1,1); + [s.visual.grid{:}] = ndgrid([-1 0 1]); + + + %%% + %%%%%% + % Define system physics + s.physics.drag_ratio = 2; + s.physics.drag_coefficient = 1; + + + %Functional Local connection and dissipation metric + + s.A = @(alpha1,alpha2) LowRE_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + s.metric = @(alpha1,alpha2) LowRE_dissipation_metric(... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1]*2.5; + + %densities for various operations + s.density.vector = [21 21 ]; %density to display vector field + s.density.scalar = [51 51 ]; %density to display scalar functions + s.density.eval = [31 31 ]; %density for function evaluations + s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_four_mode_CC.m b/UserFiles/GenericUser/Systems/sysf_four_mode_CC.m new file mode 100644 index 00000000..b6ba4b9e --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_four_mode_CC.m @@ -0,0 +1,110 @@ +function output = sysf_four_mode_CC(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe CC, 4 Mode'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = ones(1,21); + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {}; + st = struct('aspect_ratio',0.1); + for i = 1:21 + s.geometry.link_shape{i} = 'ellipse'; + s.geometry.link_shape_parameters{i} = st; + end + modeSetup = []; + for i = 1:20 + if i <= 5 + modeSetup(i,:) = [1/40,0,0,0]; + elseif i <= 10 + modeSetup(i,:) = [0,1/40,0,0]; + elseif i <= 15 + modeSetup(i,:) = [0,0,1/40,0]; + else + modeSetup(i,:) = [0,0,0,1/40]; + end + s.geometry.modes = modeSetup; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]*15; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + %s.physics.interaction = 'off'; + + %Functional Local connection and dissipation metric + + + s.A = @(alpha1,alpha2,alpha3,alpha4) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2,alpha3,alpha4]); % Joint angles + + s.metric = @(alpha1,alpha2,alpha3,alpha4) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2,alpha3,alpha4]); + %s.metric = @(alpha1,alpha2,alpha3,alpha4) eye(4); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1,-1,1,-1,1]*15; + + sampleDensity = 8; + sd = sampleDensity; + %densities for various operations + s.density.vector = [12 12 12 12]; %density to display vector field + s.density.scalar = [sd sd sd sd]; %density to display scalar functions + s.density.eval = [sd sd sd sd]; %density for function evaluations + s.density.metric_eval = [sd sd sd sd]; %density for metric evaluation + s.density.mass_eval = [sd sd sd sd]; % density for mass matrix evaluation + s.density.coriolis_eval = [sd sd sd sd]; + s.density.finite_element=8; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*15; + s.tic_locs.y = [-1 0 1]*15; + + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_four_mode_serpenoid.m b/UserFiles/GenericUser/Systems/sysf_four_mode_serpenoid.m new file mode 100644 index 00000000..8cf8885e --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_four_mode_serpenoid.m @@ -0,0 +1,104 @@ +function output = sysf_four_mode_serpenoid(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe Serpenoid, Four-Mode'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = ones(1,20); + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {}; + st = struct('aspect_ratio',0.1); + for i = 1:20 + s.geometry.link_shape{i} = 'ellipse'; + s.geometry.link_shape_parameters{i} = st; + end + modeSetup = []; + for i = 1:19 + modeSetup(i,:) = [cos(i/20*2*pi),sin(i/20*2*pi),cos(i/20*4*pi),sin(i/20*4*pi)]; + end + s.geometry.modes = modeSetup; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]*.5; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + %s.physics.interaction = 'off'; + + + %Functional Local connection and dissipation metric + + + s.A = @(alpha1,alpha2,alpha3,alpha4) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2,alpha3,alpha4]); % Joint angles + + s.metric = @(alpha1,alpha2,alpha3,alpha4) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2,alpha3,alpha4]); + %s.metric = @(alpha1,alpha2,alpha3,alpha4) eye(4); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1,-1,1,-1,1]*.5; + + sampleDensity = 8; + sd = sampleDensity; + %densities for various operations + s.density.vector = [sd sd sd sd]; %density to display vector field + s.density.scalar = [sd sd sd sd]; %density to display scalar functions + s.density.eval = [sd sd sd sd]; %density for function evaluations + s.density.metric_eval = [sd sd sd sd]; %density for metric evaluation + s.density.mass_eval = [sd sd sd sd]; % density for mass matrix evaluation + s.density.coriolis_eval = [sd sd sd sd]; + s.density.finite_element=sd; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*.5; + s.tic_locs.y = [-1 0 1]*.5; + + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_nine_link_lowRe_trigmodes.m b/UserFiles/GenericUser/Systems/sysf_nine_link_lowRe_trigmodes.m new file mode 100644 index 00000000..6a873686 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_nine_link_lowRe_trigmodes.m @@ -0,0 +1,93 @@ +function output = sysf_nine_link_lowRe_trigmodes(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'Viscous Swimmer: 9-link Sin and Cos modes'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/LowReynoldsRFT/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = ones(9,1); + n_waves = 1; + s.geometry.modes = [sin(n_waves*2*pi*(1:8)/8)', cos(n_waves*2*pi*(1:8)/8)']; + s.geometry.baseframe = 'tail'; + s.geometry.length = 1; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. The code below uses properties of cell + % arrays to automatically match the dimensionality of the grid + % with the number of shape basis functions in use + s.visual.grid_spacing = {[-1 0 1],[-1 0 1]}; + + + %%% + %%%%%% + % Define system physics + s.physics.drag_ratio = 2; + s.physics.drag_coefficient = 1; + + + %Functional Local connection and dissipation metric + + s.A = @(alpha1,alpha2) LowRE_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + +% s.metric = @(alpha1,alpha2) LowRE_dissipation_metric(... +% s.geometry,... % Geometry of body +% s.physics,... % Physics properties +% [alpha1,alpha2]); % Joint angles + + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1]*2.5; + + %densities for various operations + s.density.vector = [21 21 ]; %density to display vector field + s.density.scalar = [51 51 ]; %density to display scalar functions + s.density.eval = [31 31 ]; %density for function evaluations + s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_serpenoid4_lowRe.m b/UserFiles/GenericUser/Systems/sysf_serpenoid4_lowRe.m index 89d281a0..98e8375b 100644 --- a/UserFiles/GenericUser/Systems/sysf_serpenoid4_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_serpenoid4_lowRe.m @@ -55,12 +55,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(size(s.geometry.function)); - [s.visual.grid{:}] = ndgrid([-1 -0.5 0 0.5 1]*6); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 -0.5 0 0.5 1]*6; %%% @@ -100,6 +97,8 @@ s.tic_locs.y = [-1 0 1]*6; %%%% + % Set system type variable for gait optimization + s.system_type = 'drag'; %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_serpenoid_lowRe.m b/UserFiles/GenericUser/Systems/sysf_serpenoid_lowRe.m index 28524ac6..5e0e7f8c 100644 --- a/UserFiles/GenericUser/Systems/sysf_serpenoid_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_serpenoid_lowRe.m @@ -55,12 +55,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(size(s.geometry.function)); - [s.visual.grid{:}] = ndgrid([-1 -0.5 0 0.5 1]*6); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 -0.5 0 0.5 1]*6; %%% @@ -79,10 +76,9 @@ % Locomotion model derived from viscous drag forces reacting to % local velocities of elements on the body s.metric = @(a1,a2) LowRE_dissipation_metric(s.geometry,s.physics,[a1;a2]); + %%% - - - %%% + % Processing details %Range over which to evaluate connection @@ -98,9 +94,12 @@ %shape space tic locations s.tic_locs.x = [-1 0 1]*6; s.tic_locs.y = [-1 0 1]*6; + + % Set system type variable for gait optimization + s.system_type = 'drag'; + %%%% - %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_three_link_HighRe.m b/UserFiles/GenericUser/Systems/sysf_three_link_HighRe.m old mode 100644 new mode 100755 index c6a0d805..9436258a --- a/UserFiles/GenericUser/Systems/sysf_three_link_HighRe.m +++ b/UserFiles/GenericUser/Systems/sysf_three_link_HighRe.m @@ -32,18 +32,16 @@ s.geometry.link_shape = {'ellipse','ellipse','ellipse'}; st = struct('aspect_ratio',0.1); s.geometry.link_shape_parameters = {st,st,st}; + %s.geometry.modes = [sqrt(2)/2,-sqrt(2)/2;sqrt(2)/2,sqrt(2)/2]; %%% % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); - [s.visual.grid{:}] = ndgrid([-1 0 1]); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; %%% %%%%%% @@ -53,16 +51,22 @@ %Functional Local connection and dissipation metric + s.A = @(alpha1,alpha2) Inertial_local_connection( ... s.geometry,... % Geometry of body s.physics,... % Physics properties [alpha1,alpha2]); % Joint angles - - s.metric = @(alpha1,alpha2)eye(2);%@(alpha1,alpha2) LowRE_dissipation_metric(... -% s.geometry,... % Geometry of body -% s.physics,... % Physics properties -% [alpha1,alpha2]); % Joint angles + s.metric = @(alpha1,alpha2) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2]); + %s.metric = @(alpha1,alpha2) eye(2); + +% % TODO: These should probably be calculated as part of a larger +% % wrapping function that's meant to return M and C matrices for +% % a set of points +% % s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% % s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); +% s.M_alpha = @(alpha1,alpha2) mass_matrix(s.geometry,s.physics,[alpha1,alpha2]); +% s.dM_alphadalpha = @(alpha1,alpha2,A_eval,A_grid) shape_partial_mass(s.geometry,s.physics,[alpha1,alpha2],A_eval,A_grid); %%% %Processing details @@ -71,10 +75,15 @@ s.grid_range = [-1,1,-1,1]*2.5; %densities for various operations - s.density.vector = [21 21 ]; %density to display vector field - s.density.scalar = [51 51 ]; %density to display scalar functions - s.density.eval = [31 31 ]; %density for function evaluations - s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.vector = [21 21]; %density to display vector field + s.density.scalar = [51 51]; %density to display scalar functions + s.density.eval = [31 31]; %density for function evaluations + + %Bump up this value + s.density.metric_eval = [31 31]; %density for metric evaluation +% s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.metric_display = [7 7]; +% s.density.coriolis_eval = [31 31]; s.density.finite_element=31; @@ -83,6 +92,8 @@ s.tic_locs.y = [-1 0 1]*1; + % System type flag + s.system_type = 'inertia'; %%%% %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_three_link_HighRe_panelMethod.m b/UserFiles/GenericUser/Systems/sysf_three_link_HighRe_panelMethod.m new file mode 100644 index 00000000..acaf1804 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_three_link_HighRe_panelMethod.m @@ -0,0 +1,106 @@ +function output = sysf_three_link_HighRe_panelMethod(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe Swimmer, Panel Method: 3-link ellipses'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1]; + s.geometry.activelinks = [1 1 1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {'ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.1); + s.geometry.link_shape_parameters = {st,st,st}; + s.geometry.linkSeparation = .1; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + s.physics.interaction = 'on'; + + + %Functional Local connection and dissipation metric + + s.A = @(alpha1,alpha2) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + s.metric = @(alpha1,alpha2)eye(2);%@(alpha1,alpha2) LowRE_dissipation_metric(... +% s.geometry,... % Geometry of body +% s.physics,... % Physics properties +% [alpha1,alpha2]); % Joint angles + + % TODO: These should probably be calculated as part of a larger + % wrapping function that's meant to return M and C matrices for + % a set of points +% s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); + s.M_alpha = @(alpha1,alpha2) mass_matrix(s.geometry,s.physics,[alpha1,alpha2]); + s.dM_alphadalpha = @(alpha1,alpha2,A_eval,A_grid) shape_partial_mass(s.geometry,s.physics,[alpha1,alpha2],A_eval,A_grid); + + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1]*2.5; + + %densities for various operations + s.density.vector = [21 21]; %density to display vector field + s.density.scalar = [51 51]; %density to display scalar functions + s.density.eval = [31 31]; %density for function evaluations + s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.coriolis_eval = [31 31]; + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + % System type flag + s.system_type = 'inertia'; + + + %%%% + %Save the system properties + output = s; + + + end + +end diff --git a/UserFiles/GenericUser/Systems/sysf_three_link_floating_ellipse.m b/UserFiles/GenericUser/Systems/sysf_three_link_floating_ellipse.m old mode 100644 new mode 100755 index 23741dc6..da5f286b --- a/UserFiles/GenericUser/Systems/sysf_three_link_floating_ellipse.m +++ b/UserFiles/GenericUser/Systems/sysf_three_link_floating_ellipse.m @@ -38,12 +38,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); - [s.visual.grid{:}] = ndgrid([-1 0 1]); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]*2; %%% %%%%%% @@ -58,23 +55,32 @@ s.physics,... % Physics properties [alpha1,alpha2]); % Joint angles - s.metric = @(alpha1,alpha2)eye(2);%@(alpha1,alpha2) LowRE_dissipation_metric(... -% s.geometry,... % Geometry of body -% s.physics,... % Physics properties -% [alpha1,alpha2]); % Joint angles + s.metric = @(alpha1,alpha2) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2]); + %s.metric = @(alpha1,alpha2) eye(2); + + % TODO: These should probably be calculated as part of a larger + % wrapping function that's meant to return M and C matrices for + % a set of points +% s.dJdq = @(alpha1,alpha2) mobile_jacobian_derivative(s.J_full); +% s.dMdq = @(alpha1,alpha2) partial_mass_matrix(s.J,s.dJdq,local_inertias,'mobile'); +% s.M_alpha = @(alpha1,alpha2) mass_matrix(s.geometry,s.physics,[alpha1,alpha2]); +% s.dM_alphadalpha = @(alpha1,alpha2,A_eval,A_grid) shape_partial_mass(s.geometry,s.physics,[alpha1,alpha2],A_eval,A_grid); +% - %%% %Processing details %Range over which to evaluate connection - s.grid_range = [-1,1,-1,1]*2.5; + s.grid_range = [-1,1,-1,1]*2*pi/3; %densities for various operations s.density.vector = [21 21 ]; %density to display vector field s.density.scalar = [51 51 ]; %density to display scalar functions s.density.eval = [31 31 ]; %density for function evaluations - s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.metric_eval = [51 51]; %density for metric evaluation + s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.coriolis_eval = [31 31]; + s.density.metric_display = [7 7]; s.density.finite_element=31; @@ -82,7 +88,8 @@ s.tic_locs.x = [-1 0 1]*1; s.tic_locs.y = [-1 0 1]*1; - + % System type flag + s.system_type = 'inertia'; %%%% %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_three_link_granular.m b/UserFiles/GenericUser/Systems/sysf_three_link_granular.m index 4bf4d0b1..17a99b0f 100644 --- a/UserFiles/GenericUser/Systems/sysf_three_link_granular.m +++ b/UserFiles/GenericUser/Systems/sysf_three_link_granular.m @@ -51,8 +51,8 @@ %Functional representation of local connection s.A_num = @(a1,a2) Conn_num(a1,a2,alpha1,alpha2,Ax1,Ax2,Ay1,Ay2,Atheta1,Atheta2); - %%% + %Processing details %Mark that system has a singularity that needs to be accounted for @@ -78,26 +78,27 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); - [s.visual.grid{:}] = ndgrid([-1 0 1]); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; % power metric %s.metric = eye(2); %@(x,y) Granular_metric_calc(x,y,Metric_Tensor_raw,alpha1,alpha2); %%% + %Display parameters %shape space tic locations s.tic_locs.x = [-1 0 1]*2; s.tic_locs.y = [-1 0 1]*2; - + % Set system type variable for gait optimization + s.system_type = 'drag'; + %%%% + %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_three_link_kinematic.m b/UserFiles/GenericUser/Systems/sysf_three_link_kinematic.m index 45cfdc79..94275e5c 100644 --- a/UserFiles/GenericUser/Systems/sysf_three_link_kinematic.m +++ b/UserFiles/GenericUser/Systems/sysf_three_link_kinematic.m @@ -37,11 +37,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); - [s.visual.grid{:}] = ndgrid([-1 0 1]); + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; % Tell sysplotter to draw this system with wheels on the links s.visual.drawing_function = @wheeled_chain; @@ -79,6 +77,8 @@ s.tic_locs.x = [-1 0 1]; s.tic_locs.y = [-1 0 1]; + % Set system type variable for gait optimization + s.system_type = 'inertia'; %%%% %Save the system properties diff --git a/UserFiles/GenericUser/Systems/sysf_three_link_lowRe.m b/UserFiles/GenericUser/Systems/sysf_three_link_lowRe.m index 02a5ba46..507f3bbb 100644 --- a/UserFiles/GenericUser/Systems/sysf_three_link_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_three_link_lowRe.m @@ -29,26 +29,25 @@ s.geometry.linklengths = [1 1 1]; s.geometry.baseframe = 'center'; s.geometry.length = 1; - + s.geometry.link_shape = {'ellipse','ellipse','ellipse'}; + st = struct('aspect_ratio',0.1); + s.geometry.link_shape_parameters = {st,st,st}; %%% % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); - [s.visual.grid{:}] = ndgrid([-1 0 1]); + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; - %%% %%%%%% % Define system physics s.physics.drag_ratio = 2; s.physics.drag_coefficient = 1; + s.physics.fluid_density = 1; - %Functional Local connection and dissipation metric s.A = @(alpha1,alpha2) LowRE_local_connection( ... @@ -60,9 +59,9 @@ s.geometry,... % Geometry of body s.physics,... % Physics properties [alpha1,alpha2]); % Joint angles - - + %%% + %Processing details %Range over which to evaluate connection @@ -74,13 +73,16 @@ s.density.eval = [31 31 ]; %density for function evaluations s.density.metric_eval = [11 11]; %density for metric evaluation s.density.finite_element=31; - + s.density.coriolis_eval = [31 31]; + s.density.mass_eval = [31 31]; % density for mass matrix evaluation %shape space tic locations s.tic_locs.x = [-1 0 1]*1; s.tic_locs.y = [-1 0 1]*1; - + % Set system type variable for gait optimization + s.system_type = 'drag'; + %%%% %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_three_link_lowRe_CSmodes.m b/UserFiles/GenericUser/Systems/sysf_three_link_lowRe_CSmodes.m new file mode 100644 index 00000000..af73a8cb --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_three_link_lowRe_CSmodes.m @@ -0,0 +1,93 @@ +function output = sysf_three_link_lowRe_CSmodes(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'Viscous Swimmer: 3-link C&S modes'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/LowReynoldsRFT/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = [1 1 1]; + s.geometry.modes = [1 1;1 -1]; + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. The code below uses properties of cell + % arrays to automatically match the dimensionality of the grid + % with the number of shape basis functions in use + s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); + [s.visual.grid{:}] = ndgrid([-1 0 1]); + + + %%% + %%%%%% + % Define system physics + s.physics.drag_ratio = 2; + s.physics.drag_coefficient = 1; + + + %Functional Local connection and dissipation metric + + s.A = @(alpha1,alpha2) LowRE_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + s.metric = @(alpha1,alpha2) LowRE_dissipation_metric(... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1]*2.5; + + %densities for various operations + s.density.vector = [21 21 ]; %density to display vector field + s.density.scalar = [51 51 ]; %density to display scalar functions + s.density.eval = [31 31 ]; %density for function evaluations + s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*1; + s.tic_locs.y = [-1 0 1]*1; + + + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_three_link_lowRe_CoM.m b/UserFiles/GenericUser/Systems/sysf_three_link_lowRe_CoM.m index 14ee8e8f..12e892f5 100644 --- a/UserFiles/GenericUser/Systems/sysf_three_link_lowRe_CoM.m +++ b/UserFiles/GenericUser/Systems/sysf_three_link_lowRe_CoM.m @@ -35,12 +35,12 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(numel(s.geometry.linklengths)-1,1); - [s.visual.grid{:}] = ndgrid([-1 0 1]); - + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]; + + + %%%%%% % Define system physics s.physics.drag_ratio = 2; @@ -80,7 +80,9 @@ s.tic_locs.x = [-1 0 1]*1; s.tic_locs.y = [-1 0 1]*1; - + % Set system type variable for gait optimization + s.system_type = 'drag'; + %%%% %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_three_mode_CC.m b/UserFiles/GenericUser/Systems/sysf_three_mode_CC.m new file mode 100644 index 00000000..b11727c6 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_three_mode_CC.m @@ -0,0 +1,108 @@ +function output = sysf_three_mode_CC(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe CC, 3 Mode'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = ones(1,19); + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {}; + st = struct('aspect_ratio',0.1); + for i = 1:19 + s.geometry.link_shape{i} = 'ellipse'; + s.geometry.link_shape_parameters{i} = st; + end + modeSetup = []; + for i = 1:18 + if i <= 6 + modeSetup(i,:) = [1/40,0,0]; + elseif i <= 12 + modeSetup(i,:) = [0,1/40,0]; + else + modeSetup(i,:) = [0,0,1/40]; + end + s.geometry.modes = modeSetup; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]*15; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + %s.physics.interaction = 'off'; + + %Functional Local connection and dissipation metric + + + s.A = @(alpha1,alpha2,alpha3) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2,alpha3]); % Joint angles + + s.metric = @(alpha1,alpha2,alpha3) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2,alpha3]); + %s.metric = @(alpha1,alpha2,alpha3) eye(3); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1,-1,1]*20; + + sampleDensity = 12; + sd = sampleDensity; + %densities for various operations + s.density.vector = [sd sd sd]; %density to display vector field + s.density.scalar = [sd sd sd]; %density to display scalar functions + s.density.eval = [sd sd sd]; %density for function evaluations + s.density.metric_eval = [sd sd sd]; %density for metric evaluation + s.density.mass_eval = [sd sd sd]; % density for mass matrix evaluation + s.density.coriolis_eval = [sd sd sd]; + s.density.finite_element=sd; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*15; + s.tic_locs.y = [-1 0 1]*15; + + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_triangle_lowRe.m b/UserFiles/GenericUser/Systems/sysf_triangle_lowRe.m index 48b7f3ff..97f21f6e 100644 --- a/UserFiles/GenericUser/Systems/sysf_triangle_lowRe.m +++ b/UserFiles/GenericUser/Systems/sysf_triangle_lowRe.m @@ -136,7 +136,9 @@ s.tic_locs.x = [-1 0 1]*6; s.tic_locs.y = [-1 0 1]*6; - + % Set system type variable for gait optimization + s.system_type = 'drag'; + %%%% %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_triangle_lowRe_two_waves.m b/UserFiles/GenericUser/Systems/sysf_triangle_lowRe_two_waves.m index 1f07694d..bac84873 100644 --- a/UserFiles/GenericUser/Systems/sysf_triangle_lowRe_two_waves.m +++ b/UserFiles/GenericUser/Systems/sysf_triangle_lowRe_two_waves.m @@ -83,11 +83,9 @@ % Define properties for visualizing the system % Make a grid of values at which to visualize the system in - % illustrate_shapespace. The code below uses properties of cell - % arrays to automatically match the dimensionality of the grid - % with the number of shape basis functions in use - s.visual.grid = cell(size(curvdef_parameters)); - [s.visual.grid{:}] = ndgrid([-1 -0.5 0 0.5 1]*6+.001); %.001 is to avoid a singularity at zero + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = ndgrid([-1 -0.5 0 0.5 1]*6+.001); %.001 is to avoid a singularity at zero %%% @@ -125,7 +123,7 @@ %densities for various operations s.density.vector = [1 1]*11; %density to display vector field - s.density.scalar = [1 1]*21; %density to display scalar functions + s.density.scalar = [1 1]*51; %density to display scalar functions s.density.eval = [1 1]*31; %density for function evaluations s.density.metric_eval = [1 1]*11; s.density.finite_element=31; @@ -137,7 +135,9 @@ s.tic_locs.x = [-1 0 1]*6; s.tic_locs.y = [-1 0 1]*6; - + % Set system type variable for gait optimization + s.system_type = 'drag'; + %%%% %Save the system properties output = s; diff --git a/UserFiles/GenericUser/Systems/sysf_twenty_link_serpenoid_HighRe.m b/UserFiles/GenericUser/Systems/sysf_twenty_link_serpenoid_HighRe.m new file mode 100644 index 00000000..04681a6e --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_twenty_link_serpenoid_HighRe.m @@ -0,0 +1,100 @@ +function output = sysf_two_mode_serpenoid(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe Serpenoid, Two-Mode'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = ones(1,20); + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {}; + st = struct('aspect_ratio',0.1); + for i = 1:20 + s.geometry.link_shape{i} = 'ellipse'; + s.geometry.link_shape_parameters{i} = st; + end + modeSetup = []; + for i = 1:19 + modeSetup(i,:) = [cos(i/20*2*pi),sin(i/20*2*pi)]; + end + s.geometry.modes = modeSetup; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]*.5; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + + %Functional Local connection and dissipation metric + + + s.A = @(alpha1,alpha2) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + s.metric = @(alpha1,alpha2) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2]); + %s.metric = @(alpha1,alpha2) eye(2); + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1]*.5; + + %densities for various operations + s.density.vector = [21 21]; %density to display vector field + s.density.scalar = [51 51]; %density to display scalar functions + s.density.eval = [31 31]; %density for function evaluations + s.density.metric_eval = [11 11]; %density for metric evaluation + s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.coriolis_eval = [31 31]; + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*.5; + s.tic_locs.y = [-1 0 1]*.5; + + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end + diff --git a/UserFiles/GenericUser/Systems/sysf_twin_rotor.m b/UserFiles/GenericUser/Systems/sysf_twin_rotor.m new file mode 100644 index 00000000..2a33cc30 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_twin_rotor.m @@ -0,0 +1,64 @@ +function output = sysf_twin_rotor(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'Twin Rotor 1-D Test'; % Display name + + case 'dependency' + + output.dependency = {}; + + case 'initialize' + + %%% + %Local Connection + s.A_num = @(a1, a2) [zeros(size(a1)) zeros(size(a1)); + zeros(size(a1)) zeros(size(a1)); + ones(size(a1)) -ones(size(a1))]; + %%% + % Configuration space + s.conf_space = LieGroups.SO3; + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1]*2.5; + + %densities for various operations + s.density.vector = [10 10]; %density to display vector field + s.density.scalar = [51 51]; %density to display scalar functions + s.density.eval = [31 31]; %density for function evaluations + s.density.finite_element=31; + + %%% + %Display parameters + + %shape space tic locations + s.tic_locs.x = [-2 0 2 4]; + s.tic_locs.y = [-2 0 2 4]; + + %Don't optimize the reference point (turn this off for + %non-carlike systems) + s.xy_no_opt = 1; + + % Set system type variable for gait optimization + s.system_type = 'inertia'; + + %Output the system properties + output = s; + + end + +end \ No newline at end of file diff --git a/UserFiles/GenericUser/Systems/sysf_two_mode_CC.m b/UserFiles/GenericUser/Systems/sysf_two_mode_CC.m new file mode 100644 index 00000000..821c9b55 --- /dev/null +++ b/UserFiles/GenericUser/Systems/sysf_two_mode_CC.m @@ -0,0 +1,106 @@ +function output = sysf_two_mode_CC(input_mode,pathnames) + + % Default arguments + if ~exist('input_mode','var') + + input_mode = 'initialize'; + + end + + %%%%%%% + + switch input_mode + + case 'name' + + output = 'HighRe CC, 2 Mode'; % Display name + + case 'dependency' + + output.dependency = fullfile(pathnames.sysplotterpath,... + {'Geometry/NLinkChain/',... + 'Physics/Inertial/'}); + + case 'initialize' + + %%%%%% + % Define system geometry + s.geometry.type = 'n-link chain'; + s.geometry.linklengths = ones(1,21); + s.geometry.baseframe = 'center'; + s.geometry.length = 1; + s.geometry.link_shape = {}; + st = struct('aspect_ratio',0.1); + for i = 1:21 + s.geometry.link_shape{i} = 'ellipse'; + s.geometry.link_shape_parameters{i} = st; + end + modeSetup = []; + for i = 1:20 + if i <= 10 + modeSetup(i,:) = [1/40,0]; + else + modeSetup(i,:) = [0,1/40]; + end + s.geometry.modes = modeSetup; + + + %%% + % Define properties for visualizing the system + + % Make a grid of values at which to visualize the system in + % illustrate_shapespace. (Use a cell of gridpoints along each + % axis to use different spacings for different axes) + s.visual.grid_spacing = [-1 0 1]*15; + + %%% + %%%%%% + % Define system physics + s.physics.fluid_density = 1; + %s.physics.interaction = 'off'; + + + %Functional Local connection and dissipation metric + + + s.A = @(alpha1,alpha2) Inertial_local_connection( ... + s.geometry,... % Geometry of body + s.physics,... % Physics properties + [alpha1,alpha2]); % Joint angles + + s.metric = @(alpha1,alpha2) Inertial_energy_metric(s.geometry,s.physics,[alpha1,alpha2]); + %s.metric = @(alpha1,alpha2) eye(2); + + + %%% + %Processing details + + %Range over which to evaluate connection + s.grid_range = [-1,1,-1,1]*15; + + %densities for various operations + s.density.vector = [21 21]; %density to display vector field + s.density.scalar = [51 51]; %density to display scalar functions + s.density.eval = [31 31]; %density for function evaluations + s.density.metric_eval = [21 21]; %density for metric evaluation + s.density.mass_eval = [31 31]; % density for mass matrix evaluation + s.density.coriolis_eval = [31 31]; + s.density.finite_element=31; + + + %shape space tic locations + s.tic_locs.x = [-1 0 1]*15; + s.tic_locs.y = [-1 0 1]*15; + + + % System type flag + s.system_type = 'inertia'; + %%%% + %Save the system properties + output = s; + + + end + +end +