Code covered by the BSD License  

Highlights from
Simulate Control of Magnetized Tetrahymena Pyriformis Cells

image thumbnail
from Simulate Control of Magnetized Tetrahymena Pyriformis Cells by Aaron Becker
With uniform magnetic field uses control-Lypunov function to steer all cells to orbit goal positions

ODEsimPyriformisPhaseLag
function ODEsimPyriformisPhaseLag
% Simulates control using F-threshold method, a feedback controller for n
% cells.
%
% This simulation takes n robots, leanrs their models by rottating the
% magnetic field at a constant rate, and then uses a control-Lyapunov
% method to drive the cells to orbit their goal positions.

SHOW_MOVIE = true; %animation showing the magnetic field
close all
clc
global  freq vel a running goal calibCenter FrameCount MOVIE_NAME MAKE_MOVIE

MAKE_MOVIE = 1;
FrameCount = 0;
MOVIE_NAME = 'sim4CellsToGoal';
running = 1;  %if you close the figure with the movie, this cleanly shuts down the program
set(0,'defaultaxesfontsize',14);
set(0,'defaulttextfontsize',14);
format compact

N = 4; %number of robots
a = linspace(5,8,N)'; % a values, should be between 3 and 10

freq = 15;  % as the frequency increases, the lines become straighter, but the differentiation decreases)
vel = 0.7; %mm/s (Estimate by Yan Ou) 700 \mum/s

tCal = 15;
tF = 200;  %final time

%initial conditions for cells (x,t,theta)
x = 10*ones(N,1);
y = 5+ones(N,1).*5/N.*(0:N-1)';  %spaces cells vertically
th = 0*ones(N,1); % performance is very dependent on initial orientation for large amplitude
phase = 0; % phase of magnetic frequency
xc = zeros(N,1);  %x value, center of rotation
yc = zeros(N,1);  %y value, center of rotation 
options = odeset('RelTol',1e-4,'AbsTol',1e-4 );
%first find the center of rotation
calibCenter = true;

Dt = 1/30;

[T0,Y0] = ode45(@simPyriformis,[0:Dt:0.1],[x;y;th;phase;xc;yc],options);  %start rotating
[T1,Y1] = ode45(@simPyriformis,[0.1:Dt:tCal],Y0(end,:)',options);         %calibrate
%first find the center of rotation
calibCenter = false;
x1 = Y1(3:end,1+0*N:1*N);
y1 = Y1(3:end,1+1*N:2*N);
th1 = Y1(3:end,1+2*N:3*N);
phase1 = Y1(3:end,3*N+1);
xcenter = (max(x1)+min(x1))/2;
ycenter = (max(y1)+min(y1))/2;

%set the center locations for initial data so Lyapunov looks right
Y0(:,3*N+1 + (1:N)) = repmat(xcenter,size(Y0(:,3*N+1 + (1:N)),1),1);
Y0(:,4*N+1 + (1:N)) =  repmat(ycenter,size(Y0(:,3*N+1 + (1:N)),1),1);
Y1(:,3*N+1 + (1:N)) =  repmat(xcenter,size(Y1(:,3*N+1 + (1:N)),1),1);
Y1(:,4*N+1 + (1:N)) =  repmat(ycenter,size(Y1(:,3*N+1 + (1:N)),1),1);

[T2,Y2] = ode45(@simPyriformis,[tCal:Dt:tF],[x1(end,:)';y1(end,:)';th1(end,:)';phase1(end,:);xcenter';ycenter'],options);

T = [T0;T1;T2];
Y = [Y0;Y1;Y2];
%%%%%%%%%%%%%%%%%%%%%%% PLOTTING %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%draw path, cells
figure(1)
clf
%c = ['r','g','m','y','k','c','b'];
c = distinguishable_colors(N+1);


for i = 1:N
    hold on
    plot(Y(:,i),Y(:,i+N),'-','color',c(i,:))
    text(Y(1,i),Y(1,i+N)+1,['\it a \rm = ',num2str(a(i))])
end
axis equal
title({[num2str(N),' cells simulated for ',num2str(tF),' s'];});%['\theta_M = ',num2str(Mag),'sin(',num2str(freq),'t)']});
xlabel('mm')
ylabel('mm')
for i = 1:N
    drawCell(Y(end,i),Y(end,i+N),Y(end,i+2*N),a(i),0.05);
end
ax = axis;
hold off


figure(3) %plot sum squared error 
r = vel/(freq); % radius of circle the robot is turning around
  %(xc,yc) is the center of the circle the robot is following
th = Y(:,1+2*N:3*N);
%xc = Y(:,1+0*N:1*N)  -r*cos(th-pi/2);
%yc = Y(:,1+1*N:2*N)  -r*sin(th-pi/2);
%sum((Y(:,1:nRob).^2 + Y(:,nRob+1:2*nRob).^2).^.5,2)./nRob;
xc = Y(:,3*N+1 + (1:N));
yc = Y(:,4*N+1 + (1:N));
for i = 1:N
    plot( T, ((xc(:,i)-goal(1)).^2+(yc(:,i)-goal(1)).^2).^1,'linewidth',1,'color',c(i,:));
    hold on
end
plot( T, sum(((xc-goal(1)).^2+(yc-goal(1)).^2).^1,2),'linewidth',2,'color',c(N+1,:))
%plot( T, sum(((xc).^2+(yc).^2).^.5,2))
xlabel('time')
ylabel('sum squared error')
title('Lyapunov Function')
axLyp = axis;
crec = rectangle('Position',[0,0,tCal,2000],'facecolor',[.8,.8,.8]);
uistack(crec,'bottom')
axis(axLyp);
text(tCal/2,axLyp(4)/2,'Calibration','Rotation',90,'VerticalAlignment','middle','HorizontalAlignment','center')
% figure(4)
% plot( T, Y(:,end),'.')
% xlabel('time')
% ylabel('phase integrator')

% figure(5)
% 
% %F = -sum( vel.*(xerr.*cos(th)+yerr.*sin(th)));  %gradient of the distance
% % if F < -1/2*sum(vel.*(xerr.^2+yerr.^2).^.5,2) -->  M=0; %sum of the distances F>0 is bad!
% 
% plot( T, -sum(vel.*((xc-goal(1)).*cos(th)+(yc-goal(1)).*sin(th)),2),'linewidth',2,'color',c(N+1,:))
% hold on
% plot( T, -1/2*sum(vel.*(((xc-goal(1)).^2+(yc-goal(1)).^2).^.5),2),'linewidth',1,'color','b')
% xlabel('time')
% legend('F','thresh')
% ylabel('Lyapunov Derivative')
% title('Lyapunov Derivative')
% hold off
if ~SHOW_MOVIE
    return
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%% MAKE A MOVIE! %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%% Setup figures %%%%%%%%%
figure(2);
set(gcf,'CloseRequestFcn',@F2Close);

clf
hpath = zeros(N,1);
hpathOld = zeros(N,1);
hCell  = cell(N,1);
for i = 1:N
    %start pos
    plot(Y(1,i),Y(1,i+N),'x','color', c(i,:))
    hold on
     
    hpath(i) = plot(Y(1,i),Y(1,i+N),'-','color', c(i,:),'linewidth',1);
    hpathOld(i) = plot(Y(1,i),Y(1,i+N),':','color', c(i,:),'linewidth',1);
    ht =text(Y(1,i),Y(1,i+N)+1,['\it a \rm = ',num2str(a(i))]);
    uistack(ht,'top')
    plot(goal(i,1),goal(i,2),'+','color','w','linewidth',3);
     plot(goal(i,1),goal(i,2),'+','color', c(i,:),'linewidth',2);
end
for i = 1:N
    %draw cells
    hCell{i} = drawCell(Y(1,i),Y(1,i+N),Y(1,i+2*N),a(i),0.05);
end
hTitle = title({[num2str(N),' cells simulated for ',num2str(tF),' s']});%;['\theta_M = ',num2str(Mag),'sin(',num2str(freq),'t), T = ',num2str(T(1)),' s']});


Magsc = 1;
MagRad = max(ax(2)-ax(1),ax(4)-ax(3))/2;
MagOrigin = [ (ax(2)+ax(1))/2, (ax(4)+ax(3))/2];
%thetaM = Mag*sin(freq*T(1));
%hMag = drawMagnet(thetaM,MagRad,Magsc,MagOrigin);
axis equal
ax(1) = ax(1)-2*Magsc;
ax(2) = ax(2)+2*Magsc;
ax(3) = ax(3)-2*Magsc;
ax(4) = ax(4)+2*Magsc;
axis(ax);
%hMagLine = zeros(6*N,1);
%hMagLineAr = zeros(6*N,1);
%magcolor = [.5,.5,1];
% for i = 1:numel(hMagLine)
%     %draw lines
%     hMagLine(i) = plot( MagOrigin(1)+[-3*MagRad,3*MagRad],...
%         MagOrigin(2)+(i-numel(hMagLine)/2)*3*MagRad/numel(hMagLine)*[1,1],'Color',magcolor);
%     uistack(hMagLine(i),'bottom')
%     hMagLineAr(i) = patch( MagOrigin(1)+[0,0,Magsc],...
%         MagOrigin(2)+(i-numel(hMagLine)/2)*3*MagRad/numel(hMagLine)+[-Magsc/4,Magsc/4,0],magcolor);
%     set(hMagLineAr(i),'edgecolor',magcolor);
%     uistack(hMagLineAr(i),'bottom')
% end

MagredX = [0,5,0];
MagredY = [-1,0,1]/4;
MagwhtX = [0,-5,0];
MagwhtY = [-1,0,1]/4;
magO = [-5,10];
hTime = text(-5,-4,{'\it t \rm=',num2str(T(1),'%3.0f'),' s'});
text(magO(1)+2,magO(2)+2,{'Magnetic';'Orientation'})

hMagR = patch(magO(1)+MagredX,magO(2)+MagredY,'r','linewidth',2);
hMagW = patch(magO(1)+MagwhtX,magO(2)+MagwhtY,'w','linewidth',2);
text(magO(1)+2,magO(2)+2,{'Magnetic';'Orientation'})

%%%% show movie frames %%%%%%%%%%%%%%%%%%%
for j = 2:numel(T)
    if ~running
        break
    end
    set(hTitle,'string',{[num2str(N),' cells, \it t \rm=',num2str(T(j),'%3.0f'),' s']});%['\theta_M = ',num2str(Mag),'sin(',num2str(freq),'t), T = ',num2str(T(j), '% 5.0f'),' s']});
    
    for i = 1:N
        set(hpath(i),'Xdata',Y(max(1,j-50):j,i),'Ydata',Y(max(1,j-50):j,i+N));
        set(hpathOld(i),'Xdata',Y(1:j,i),'Ydata',Y(1:j,i+N));
        updateCell(hCell{i},Y(j,i),Y(j,i+N),Y(j,i+2*N));
    end
    %thetaM = Mag*sin(freq*T(j));
    thetaM = Y(j,3*N+1);
set(hTime,'String',['\it t \rm=',num2str(T(j),'%3.0f'),' s']);
    set(hMagR,'Xdata',magO(1)+MagredX*cos(thetaM)-MagredY*sin(thetaM),...
              'Ydata',magO(2)+MagredX*sin(thetaM)+MagredY*cos(thetaM));
    set(hMagW,'Xdata',magO(1)+MagwhtX*cos(thetaM)-MagwhtY*sin(thetaM),...
              'Ydata',magO(2)+MagwhtX*sin(thetaM)+MagwhtY*cos(thetaM));
%     %updateMagnet(hMag, thetaM);
%     for i = 1:numel(hMagLine)
%         if( Y(j,3*N+1) == Y(j-1,3*N+1))
%             mcol = [1,1,1];
%         else
%             mcol = magcolor;
%         end
%         %draw lines
%         xmag =  [-3*MagRad,3*MagRad];
%         ymag = (i-numel(hMagLine)/2)*3*MagRad/numel(hMagLine)*[1,1];
%         set(hMagLine(i),'Color',mcol,'Xdata',MagOrigin(1)+xmag*cos(thetaM)-ymag*sin(thetaM), 'Ydata',MagOrigin(2)+xmag*sin(thetaM)+ymag*cos(thetaM));
%         
%         xar=1+[0,0,Magsc];
%         yar=(i-numel(hMagLine)/2)*3*MagRad/numel(hMagLine)+[-Magsc/4,Magsc/4,0];
%         set(hMagLineAr(i),'faceColor',mcol,'edgecolor',mcol,'Xdata',MagOrigin(1)+xar*cos(thetaM)-yar*sin(thetaM), 'Ydata',MagOrigin(2)+xar*sin(thetaM)+yar*cos(thetaM));
%         
%     end
    
    axis(ax);
   updateDrawing;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%   END MOVIE!  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end


function F2Close(~,~)
global running
running = 0;
delete(gcf);
end

function dq = simPyriformis(~,q)
% simulates a differential equation model for the cells
% q = [x_coordinates; y_coordinates; theta_coordinates,
% magnetic_field_orientation]
global  freq vel a goal calibCenter 
%thetaM = Mag*sin(freq*t);
M = 1;  %magnetic field, 1= on, 0 = off
goal = [0,0;
        0,5;
        5,0;
        5,5];  %desired point we want cells to circle around


dq = zeros(size(q));    % a column vector for the change in state
N = (numel(q)-1)/5;  %number of robots

%r = vel/freq; % radius of circle the robot is turning around
th = q(2*N+1:3*N); %current orientations of the cells


%sum((Y(:,1:nRob).^2 + Y(:,nRob+1:2*nRob).^2).^.5,2)./nRob;
 % trans = -sum(eps*(x*cos(theta) + y*sin(theta)))/unicycles, where x and
 % y are the distance to each robot's goal
xc = q(3*N+1 + (1:N));%-r*cos(th-pi/2);  %(xc,yc) is the center of the circle the robot is following
yc = q(4*N+1 + (1:N));%-r*sin(th-pi/2);

%goal = [mean(xc),mean(yc)]; %to collect all robots together (clumping/rendezvous)
xerr  = goal(:,1)-xc; %error between goal position and current circle center
yerr  = goal(:,2)-yc;

F = -sum( vel.*(xerr.*cos(th)+yerr.*sin(th)));  %gradient of the distance

% TODO:  what is the best threshold? do a test and iterate to find the best
% value
 if (calibCenter == false) && (F < -1/4*sum(vel.*((xerr.^2+yerr.^2).^.5),1)) %sum of the distances F>0 is bad!
     M=0; %go straight if the gradient is large (and negative)
 end
 
thetaM = q(3*N+1);  % magnetic field direction
 
dq(1:N)       = vel*cos(th);
dq(N+1:2*N)   = vel*sin(th);


angDif = thetaM-th;
dq(2*N+1:3*N) = M*a.*sin(angDif);

dq(3*N+1) = freq*M; %only update the magnetic field orientation if the magnetic field is on.
if M==0
    dq(3*N+1 + (1:N))   = vel*cos(th);
    dq(4*N+1 + (1:N))   = vel*sin(th);
end

end


function updateDrawing
    global FrameCount MOVIE_NAME MAKE_MOVIE
    set(gcf, 'position',[-98          57        1774         965]);
    drawnow
    if(MAKE_MOVIE)
        FrameCount=FrameCount+1;
        F = getframe(gca);
        fname = sprintf('img/%s%06d.png',MOVIE_NAME,FrameCount);
        imwrite(F.cdata, fname,'png'); 
        while(FrameCount < 10)
            updateDrawing
        end
    end
end

Contact us