fixed scaling handling for non-unit body lengths; cleaned up some dependencies

Author: Ross Hatton

ProgramFiles/Geometry/ContinuousBackbone/GeneralCurvature/backbone_from_general_curvature.m

@@ -59,7 +59,7 @@
     jacobian_sol = ode_multistart(@ode45,@(s,J) J_helper(s,J,dcurvdef,h_norm(s)),all_limits,0,zeros(2*length(dcurvdef(0)),1));
 
     % Concatenate xy and theta jacobians.
-    J = @(s) cat(1,reshape(L*jacobian_sol(toz(s/L)),2,[],length(s)),permute(J_theta_fun(s/L),[3,2,1]));
+    J = @(s) cat(1,reshape(L*jacobian_sol(toz(s)),2,[],length(s)),permute(J_theta_fun(s),[3,2,1]));
 
 end
 

ProgramFiles/Geometry/ContinuousBackbone/ModalCurvature/backbone_from_curvature_bases.m

@@ -155,7 +155,7 @@
     jacobian_sol = ode_multistart(@ode45,@(s,J) J_helper(s,J,kappa_basis,h_norm(s)),all_limits,0,zeros(2*length(kappa_basis),1));
 
     % Concatenate xy and theta jacobians.
-    J = @(s) cat(1,reshape(L*jacobian_sol(toz(s/L)),2,[],length(s)),J_theta_fun(s/L));
+    J = @(s) cat(1,reshape(L*jacobian_sol(toz(s)),2,[],length(s)),J_theta_fun(s));
 
 end
 

ProgramFiles/Geometry/ContinuousBackbone/backbone.m

@@ -28,10 +28,10 @@
 %                                       sysf_ file. Argument should be the name of
 %                                       a system in the current UserFiles folder
 %
-%   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.
+%       length (optional): Total length of the backbone. 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
+%           backbone will not be scaled.
 %
 %   shapeparams: A vector of the  input parameters taken by the curvature
 %       function
@@ -132,14 +132,11 @@
 %%%%%%
 % Calculate the transformation from the original base frame to the new base
 % frame
+
 if calc_J
-    [frame_zero,J_zero] = backbone_conversion_factors(h,J,shapeparams,baseframe);   
+    [frame_zero,J_zero] = backbone_conversion_factors(h,J,shapeparams,baseframe,geometry.length);   
 else
-    frame_zero = backbone_conversion_factors(h,[],shapeparams,baseframe);
-    if strncmp(baseframe,'sysf_',5)
-        frame_zero(1,3)=geometry.length*frame_zero(1,3);
-        frame_zero(2,3)=geometry.length*frame_zero(2,3);
-    end
+    frame_zero = backbone_conversion_factors(h,[],shapeparams,baseframe,geometry.length);
 end
 
 %%%%%%

ProgramFiles/Geometry/ContinuousBackbone/backbone_conversion_factors.m

@@ -1,7 +1,8 @@
 function [frame_zero,J_zero] = backbone_conversion_factors(h,...
                                                            J,...
                                                            shapeparams,...
-                                                           baseframe)
+                                                           baseframe,...
+                                                           L)
 %%%%%%%
 % This is a helper-function for backbone. 
 %
@@ -234,6 +235,12 @@
                         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
@@ -246,6 +253,14 @@
                                 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
 
@@ -278,10 +293,10 @@
                     % Get conversion factors for the original baseframe
                     if calc_J
                         [frame_original,J_original] =...
-                            backbone_conversion_factors(h,J,shapeparams,baseframe_original);
+                            backbone_conversion_factors(h,J,shapeparams,baseframe_original,L);
                     else
                         frame_original =...
-                            backbone_conversion_factors(h,J,shapeparams,baseframe_original);                        
+                            backbone_conversion_factors(h,J,shapeparams,baseframe_original,L);                        
                     end
 
                     % Combine original conversion with minimum-perturbation

ProgramFiles/Geometry/NLinkChain/N_link_conversion_factors.m

@@ -45,6 +45,9 @@
 % 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);
+
 %%%%%%%%%%%%%%%%%%%
 
 
@@ -166,9 +169,6 @@
     % of the links, using link-lengths as weighting terms
     case 'com-mean'
 
-        % Total length for use in taking weighted averages
-        L = sum(linklengths);
-        
         % Convert link positions to row form
         chain = mat_to_vec_SE2(chain_m);
 
@@ -329,7 +329,13 @@
                         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)
 
@@ -341,11 +347,19 @@
                                                     s.B_optimized.eval.gradBeta{idx,idx2},...    % Components of the transfomation to m-p coordinates
                                                     jointangles_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
-                    frame_mp(1)=sum(linklengths)*frame_mp(1);
-                    frame_mp(2)=sum(linklengths)*frame_mp(2);
+                    
+                                            
+
                     % 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);

ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_connection_continuous.m

@@ -19,10 +19,10 @@
 [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];
+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),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),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,19 +35,20 @@
 end
 
 
-function dOmega = LowRE_Pfaffian_infinitesimal(s,h,J,c,drag_ratio) %#ok<INUSL>
+function dOmega = LowRE_Pfaffian_infinitesimal(s,h,J,lambda,c,drag_ratio) %#ok<INUSL>
 % 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
+	% torsional drag, multiplied by drag coefficient and local scaled
+	% differential length
 	gcirc_local_to_F_local = ...
         [-1     0       0;
         0   -drag_ratio 0;
-        0       0       0]*c;
+        0       0       0]*c*lambda;
 
     % Transfer force to midpoint-tangent frame by transpose of the
     % adjoint-inverse action

ProgramFiles/Physics/LowReynoldsRFT/Continuous_backbone/LowRE_metric_continuous.m

@@ -11,7 +11,7 @@
 	[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),J(s),drag_matrix),int_limit,zeros(length(shapeparams)));%zeros(length(shapeparams)^2,1));
+	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));
 
     % Form the components of the metric into the standard NxN symmetric matrix form
 	Mp = reshape(deval(Mp_sol,int_limit(end)),length(shapeparams),[]);

ProgramFiles/sys_calcpath.m

@@ -45,7 +45,7 @@
 
 				%Vectorize the shape change equation if it's an inline function
 				p = vectorize_shch(p); %#ok<NODEF>
-                save(infile2,'p')
+                
 
 				%get shape change loci
 				p = find_loci(s,p);

ProgramFiles/sys_calcpath_fcns/se2_integrator_all_terms.m

@@ -7,11 +7,7 @@
 	shape = phi_fun(t);
 	shapelist = num2cell(shape);
 	dshape = dphi_fun(t);
-    dshape1=dshape;
-    clear dshape
-    for i=1:length(dshape);
-        dshape(i,1)=dshape1(i);
-    end    
+    dshape1=dshape(:);
 	n_dim=length(s.vecfield.eval.content.Avec_optimized(1,:));
 
     if length(shape)<n_dim