legSpring

0001 function [ndf, ndd] = legSpring( res, pfx )
0002   if nargin<2
0003     pfx=[];
0004   end
0005   
0006   [xAll,xIdx,bAll] = resInterpl(res);   
0007   % Centre of mass position along body axis
0008   COM = [0.3];
0009   % Obtain pixel to cm scale
0010   sclXY = 11.4/abs((res.l2 - res.l1)*[1;i;0]);
0011   % Get period estimate
0012   period = median( res.preLeg(:,1) );
0013   % Obtain time scale
0014   sclT = 1 / period;
0015   % Get length of body axis in cm
0016   bLen = sclXY * norm(diff(res.bodyAx(:,1:2)));
0017   % Convert to dimensionless lengths
0018   xAll = xAll ./ bLen;
0019   % Position in units of body length
0020   ang = fillNans(res.ang(xIdx)).';
0021   x = [ real(xAll(7,:));...
0022         imag(xAll(7,:));...
0023         ang];
0024   idx0 = 50:size(x,2)-50;
0025   idx = idx0(2:end-1);
0026   xe0 = Kal( x, [1 1] * 0.01, idx0, sclT );
0027 
0028   % Index offsets into xe array
0029   n=3;
0030   vOfs=n;
0031   aOfs=2*n;
0032   xOfs=1;
0033   yOfs=2;
0034   angOfs = 3;
0035   
0036 %  figure(1);
0037 %  showXVA( x(:,idx), xe0, {'x','y','\theta'}, sclT, 'Ass Motion in Belt Frame' );
0038 
0039   % Rotation from world to body coordinates
0040   rot = exp(-i * ang(idx));
0041   % World Velocity of cockroach ass
0042   vw0 = xe0(vOfs+xOfs,:) + i * xe0(vOfs+yOfs,:);
0043   % Angular velocity
0044   angDot = xe0(vOfs+angOfs,:);
0045   % Body Velocity of ass
0046   vb0 = rot .* vw0;
0047   % Centre of Rotation location satisfies that at that
0048   %   location the rotation induced velocity is orthogonal to
0049   %   lateral body velocity. This gives the following distance
0050   %   (in units of body-length)
0051   d = -imag(vb0) * angDot.' ./ norm(angDot).^2;
0052   % Centre of Rotation position (unfiltered)
0053   ctrRot = xAll(7,:) + d * exp(i * ang);
0054   % Kalman filter the centre of rotation
0055   x2 = [ real(ctrRot); imag(ctrRot); ang ];
0056   xe = Kal( x2, [0.2 0.2] ./ period, idx0, sclT );
0057   
0058   % Display BodyMotion diagram
0059   figure(2);
0060   msg = sprintf('Centre of Rotation\n%.0f%% of body; ',d*100);
0061   showXVA( x(:,idx), xe, {'x','y','\theta'}, sclT, [msg 'in Belt Frame'] );
0062   condPng( pfx,'BodyMotion' );
0063 
0064   % Velocity in World frame of COR
0065   vwc = xe(vOfs+1,:) + i * xe(vOfs+2,:);
0066   % Accelleration in World frame of COR
0067   awc = xe(aOfs+1,:) + i * xe(aOfs+2,:);
0068   % Velocity in Body frame of COR
0069   vbc = rot .* vwc;
0070   % Accelleration in Body frame of COR
0071   abc = rot .* awc;
0072   
0073   % Plot COR in Body Frame
0074   bxe = [zeros(2,size(vbc,2));...
0075          real(vbc); imag(vbc);...
0076          real(abc); imag(abc); ];
0077   figure(8);
0078   showXVA( [],bxe, {'x','y'}, sclT, [ msg 'in Body Frame'] );
0079 
0080   %% Use Kalman Filter to estimate tarsus motion
0081   % Tarsus Position in units of body length
0082   tx = real(bAll) ./ bLen;
0083   txe = Kal(tx,[0.1 0.01] * 50/period, idx0, sclT);
0084 
0085   % Show body centered tarsus positions
0086   figure(3); showXVA( tx(:,idx), txe, ...
0087     {'FRbx','MRbx','HRbx','HLbx','MLbx','FLbx'}, sclT, 'Along Body Axis' ); 
0088   condPng( pfx,'ForeAftMotion' );
0089 
0090   % Gait is true for swing, false for stance.
0091   gait = txe(6+[1:6],:) > 0;
0092 
0093   % Tarsus Positions in World=Belt frame, in units of body length
0094   tp = [ real(xAll(1:6,:)); imag(xAll(1:6,:)) ];
0095   tpe = Kal( tp, [.5 1] ./ period, idx0, sclT );
0096  
0097   figure(4); showXVA( tp(1:6,idx), tpe([1:6,12+[1:6],24+[1:6]],:), ...
0098     {'FRx','MRx','HRx','HLx','MLx','FLx'}, sclT, 'Tarsi, Belt Frame X' ); 
0099   condPng( pfx,'BeltX' );
0100   figure(5); showXVA( tp(7:12,idx), tpe([6+[1:6],18+[1:6],30+[1:6]],:),  ...
0101     {'FRy','MRy','HRy','HLy','MLy','FLy'}, sclT, 'Tarsi, Belt Frame Y' ); 
0102   condPng( pfx,'BeltY' );
0103 
0104   % scale accellerations and trim the top 10%
0105   aScl = prctile(abs(awc),90);
0106   fixMe = find(abs(awc)>aScl);
0107   awc_ = awc; awc_(fixMe) = awc_(fixMe) ./ abs(awc_(fixMe)) * aScl;
0108   
0109   % Pad to simplify next computation
0110   falsePad = false(size(xAll,1)-6,length(idx));
0111   % X of Gait Forward (has nan for stance)
0112   xgf = xAll(:,idx); xgf([gait;falsePad]) = nan;
0113   % X of Gait Backward (has nan for swing)
0114   xgb = xAll(:,idx); xgb([~gait;falsePad]) = nan;
0115   
0116   % Show motion Owverview chart
0117   figure(6);
0118   clf; hold on;
0119   plot(xgf(7:8,:),'k-');
0120   plot(repmat(ctrRot(idx),2,1)+[zeros(size(awc_));awc_./aScl],'-b');
0121   plot(ctrRot(idx)+awc_./aScl,'-b');
0122   plot(xgf(1:6,:).','.-g');
0123   plot(xgb(1:6,:).','-r');
0124   xlabel('cm'); ylabel('cm');
0125   title('Accelerations');
0126   axis equal;
0127   condPng( pfx,'Overview' );
0128   
0129   % Show Gait Correlation
0130   figure(7); showGaitCorr( xe, gait, sclT );
0131   condPng( pfx,'GaitCorr' );
0132   
0133   % "Left Tripods" (with one error allowed)
0134   lt = sum( gait == repmat([0;1;0;1;0;1],1,size(gait,2))) > 4;
0135   % "Right Tripods" (with one error allowed)
0136   rt = sum( gait == repmat([1;0;1;0;1;0],1,size(gait,2))) > 4;
0137   % Other states
0138   other = ~(lt | rt);
0139   figure(9);
0140   msg = sprintf('Acceleration by Tripod\n in Body Frame (means LT %.2g,%.2g RT %.2g,%.2g )', ...
0141     real(mean(awc(lt))),imag(mean(awc(lt))),...
0142     real(mean(awc(rt))),imag(mean(awc(rt))) );
0143   subplot(2,1,1);
0144   plot( [1:length(lt)] * sclT,...
0145         trimOutliers([abs(abc.*other);abs(abc.*lt);abs(abc.*rt)].',0.01) );
0146   xlabel('cyc'); ylabel('bodylengths/cyc^2');
0147   legend({'other','LT','RT'},-1);
0148   axis tight;
0149   title(msg);
0150   subplot(2,1,2);
0151   aang_ = [angle(abc.*other);angle(abc.*lt);angle(abc.*rt)].';
0152 %  aang_([false(1,3);abs(diff(aang_))>1.5*pi])=nan;
0153   plot( [1:length(lt)] * sclT,aang_ );
0154   xlabel('cyc'); ylabel('radians');
0155   legend({'other','LT','RT'},-1);
0156   axis([1,length(lt)* sclT,-pi,pi]);
0157   condPng( pfx,'TripAcc' );
0158 return
0159   if nargout==0
0160     figure(1); clf; plot(xAll(7:8,idx).'); axis equal; ax=axis;
0161     for kk=1:length(idx)
0162       figure(1); clf; hold on;
0163       plot(xAll(7:8,idx).','c');
0164       plot(xgf(1:6,kk),'hg');
0165       plot(xgb(1:6,kk),'.r');
0166       set(plot(xAll(7:8,idx(kk)),'k-p'),'LineWidth',2);
0167       plan = cumsum([0;... %ndd_(:,k).'.*dScl;...
0168                      awc_(kk).*aScl]);
0169       set(plot(ctrRot(idx(kk))+plan(:,1),'-b'),'LineWidth',2);
0170       title(sprintf('%d',idx(kk) * 1000 / res.fps ));
0171       axis(ax);
0172       drawnow;
0173       pause(0.05);
0174     end
0175   end
0176 return
0177 
0178 function showXVA( x, xe, lbl, tScl, cap )
0179   n= length(lbl);
0180   vOfs = n;
0181   aOfs = 2 * n;
0182   t = [1:size(xe,2)] .* tScl;
0183   out = 0.02;
0184   clf;
0185   if isempty(x)
0186     ng = 2;
0187   else
0188     ng = 3;
0189     subplot(ng,1,1); 
0190     set(plot(t, trimOutliers(xe(1:n,:).',out) ),'LineWidth',2); hold on;
0191     plot(t, trimOutliers(x([1:n],:).',out) ,'k');
0192     legend(lbl,-1); axis tight;
0193     ylabel('bodylengths');
0194     xlabel('cycles');
0195     title(cap);
0196   end
0197   
0198   subplot(ng,1,ng-1); 
0199   plot(t, trimOutliers(xe(vOfs+[1:n],:).',out) );
0200   legend(lbl, -1); axis tight; 
0201   ylabel('bodylengths/cycle');
0202   xlabel('cycles');
0203   if isempty(x)
0204     title(cap);
0205   end    
0206   
0207   subplot(ng,1,ng); 
0208   plot(t, trimOutliers(xe(aOfs+[1:n],:).',out) );
0209   legend(lbl, -1); axis tight;
0210   ylabel('bodylengths/cycle^2');
0211   xlabel('cycles');
0212 return
0213 
0214 function rng = quadRng( data )
0215   X = repmat( 1:size(data,2), size(data,1), 1 );
0216  [P,S] = polyfit( X, data, 2 );
0217  [Y,DELTA] = polyval(P,X,S);
0218  rng = max(DELTA,[],2) - min(DELTA,[],2);
0219 return
0220 
0221 function [est2] = Kal( x, scl, idx, tScl )
0222   R = quadRng( x ).' / 100;
0223   Q = kron( scl, R );
0224   if length(scl)==2
0225     ksp = newKalmanConstV(x(:,1),[],Q,R,tScl);
0226   else
0227     ksp = newKalmanConstA(x(:,1),[],[],Q,R,tScl);
0228   end
0229   [est,V,VV] = kalman_smooth(x,ksp);
0230 
0231   rows = 1:size(x,1);
0232   est2 = [ est(rows,idx(2:end-1)); ...
0233            diff(est(rows,idx(1:end-1)),1,2) ./ tScl; ...
0234            diff(est(rows,idx),2,2) ./ tScl.^2 ];
0235 return
0236 
0237 function showGaitCorr(xe,gait,sclT)
0238   clf;
0239   legIdx= [1 5 3 4 2 6];
0240   leg = {'FR','MR','HR','HL','ML','FL'};
0241   g = repmat(~gait(legIdx,:),[1 1 3]);
0242   cm = colormap('jet');
0243   
0244   subplot(2,1,1);
0245   v = repmat(xe(6,:),6,1);
0246   v = ceil(size(cm,1)*(v - min(v(:))+eps)/(eps+max(v(:))-min(v(:))));
0247   v(v>size(cm,1)) = size(cm,1);
0248   rgb = reshape(cat(3,cm(v,1),cm(v,2),cm(v,3)),[6,size(xe,2),3]).* g;
0249   image(rgb);
0250   
0251   set(gca,'XTick',1:1/sclT:size(xe,2),...
0252        'XTickLabel',0:size(xe,2) * sclT,...
0253        'YTick',[1:6],'YTickLabel',leg(legIdx) );
0254   title('\omega by contact state');
0255   subplot(2,1,2);
0256 
0257   v = repmat(xe(9,:),6,1);
0258   v = ceil(size(cm,1)*(v - min(v(:))+eps)/(eps+max(v(:))-min(v(:))));
0259   rgb = reshape(cat(3,cm(v,1),cm(v,2),cm(v,3)),[6,size(xe,2),3]).* g;
0260   image(rgb);
0261   
0262   set(gca,'XTick',1:1/sclT:size(xe,2),...
0263        'XTickLabel',0:size(xe,2) * sclT,...
0264        'YTick',[1:6],'YTickLabel',leg(legIdx) );
0265   title('\alpha by contact state');
0266 %  Vbias =  sign(xe(6,:)) * ~gait.' ./ size(xe,2)
0267 %  Abias =  sign(xe(9,:)) * ~gait.' ./ size(xe,2)
0268 return
0269 
0270 function condPng( pfx, sfx )
0271   if ~isempty(pfx)
0272     drawnow;
0273     print('-r150','-dpng',[pfx,sfx,'.png']);
0274   end
0275 return
0276 
0277 function dat = trimOutliers( dat, out )
0278   b = repmat( prctile( dat, 100 - out * 100 ), size(dat,1), 1 );
0279   bi = dat>b;
0280   dat(bi) = b(bi);
0281   b = repmat( -prctile( -dat, 100 - out * 100 ), size(dat,1), 1 );
0282   bi = dat<b;
0283   dat(bi) = b(bi);
0284 return
0285