%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function PosControl_universal
% program for position control to an arbitrary shape using 1 obstacle.
% Requires posControlShapes.m. This function automatically applies a
% routine to reshape the robots into a desired arrangement using uniform
% control inputs.
%
% Authors: Aaron Becker and Golnaz Habibi, Rice University 2013
%
% Code used for Fig. 3 in "Massive Uniform Manipulation:
% Controlling Large Populations of Simple Robots with a Common Input Signal"
% by Aaron Becker, Golnaz Habibi, Justin Werfel, Michael Rubenstein, and
% James McLurkin, IEEE/RSJ International Conference on Intelligent Robots
% and Systems (IROS), Nov 3-7 2013.
%
% TODO: add comments
% TODO: ensure workspace is 16:9 screen size
% TODO: this is still very slow for large arrays -- profile it and speed
% itup
% TODO: the pop-off operation should go around the left side, not the right
%side.
close all; clc;
clear all
format compact
%global G MAKE_MOVIE FrameCount
FrameCount = 0;
G.ShowDrawing = 1;
MAKE_MOVIE = 0; % if 1, records png images to make into a movie
%%% PICK the desired shape, which are in posControlShapes.m
Shape = 'Smile'; % 2.715 s with globals 2.689 s, 1.052124 without global
%Shape = 'MRSL'; %23.34s, 22.081948 without return statements, 7.140782 without global
%Shape = 'MonaLisa10'; % small BW image of Mona Lisa %730.219704 s
%Shape = 'MonaLisa3'; % very large BW image of Mona Lisa
%Shape = 'A'; %simple letter 'A'
%Shape = 'I'; %simple letter 'I'
G.MOVIE_NAME = ['PosControlData',Shape];
%ffmpeg -r 30 -f image2 -i img/PosControlI%06d.png -b 8000k -r 30 MassivePositionControlI.mp4
G.BW = posControlShapes(Shape);
px= size(find(G.BW>0),1);
G.m1 = floor(px^.5) + 1 ;
G.n1 = floor(px/G.m1) ;
G.lastrow = px -G.m1*G.n1;
if G.lastrow ~=0
G.m1 = G.m1+1;
end
% G.YDist = size(BW,1)+1;
% G.XDist = size(BW,2);
G.m= 2* size(G.BW,1) + 2* G.m1 +2 ; %
G.n= 2*size(G.BW,2) + 2*G.n1 + 1 ;
G.EMPTY = 0;
G.OBST = 1;
G.ROBOT1 = 2;
G.INTARGET1 = 3; %Gray
G.TARGET = 8;
G.colormap = [1,1,1; %Empty --- white
1,0,0; %obstacle --- red
%.8,1,.8; % goal position;% robot at goal ----- light green
.9,.9,1; % starting robot position -- light blue
0,0,1; %robots --- blue
0,0,0 ];
% pos_target
startgame();
% game is over when figure closes
for countTemp = 1:10
updateDrawing
end
function startgame()
tic
%global G MAKE_MOVIE
G.fig = figure(1);
G.DistanceCommanded = 0;
G.DistanceTotal = 0;
if MAKE_MOVIE
set(G.fig ,'Name','Massive Control','color',[1,1,1]);G.fig = figure(1);
end
G.numMoves = 0;
G.selected=[];
G.borderRight=[];
G.borderBottom=[];
G.obstacle=[];
set(G.fig ,'Name','Massive Control');
moverobots()
posStart = G.S; %#ok<NASGU>
posFinal = G.F; %#ok<NASGU>
posObs = G.obstacle_pos; %#ok<NASGU>
save([G.MOVIE_NAME,'.mat'], 'posStart','posFinal','posObs');
select()
updateDrawing()
for i=1:size(G.Target,1)
move_out()
push_to_goal(i)
select()
end
%align robots and target
[~,c] =find(G.robot_pos>0);
while max(c) > G.obstacle(2)
updateTarget('l')
[~,c] =find(G.robot_pos>0);
end
DistanceTotal = G.DistanceTotal;
DistanceCommanded = G.DistanceCommanded;
numMoves = sum(sum(G.S))*12;
toc
display([G.MOVIE_NAME,'.mat'])
display(DistanceTotal)
display(DistanceCommanded)
display(numMoves)
save([G.MOVIE_NAME,'.mat'], 'DistanceTotal','DistanceCommanded','numMoves', '-append');
end
function select()
%function
%global G
[y,x]=find(G.robot_pos==G.ROBOT1);
top_robot = max(y);
left_robot = min(x(y == top_robot));
G.borderRight = [max(y) max(x)];
G.borderBottom = [min(y) min(x)];
G.selected = [top_robot left_robot];
end
function updateTarget(direction)
%global G
switch direction
case 'l'
G.Target_pos = zeros(G.m,G.n);
G.Target(:,2) = G.Target(:,2) - 1;
case 'r'
G.Target_pos = zeros(G.m,G.n);
G.Target(:,2) = G.Target(:,2) + 1;
case 'u'
G.Target_pos = zeros(G.m,G.n);
G.Target(:,1) = G.Target(:,1) + 1;
case 'd'
G.Target_pos = zeros(G.m,G.n);
G.Target(:,1) = G.Target(:,1) - 1;
end
moveto(direction);
if G.ShowDrawing || (FrameCount<200 && mod(G.DistanceCommanded,100) == 0) || mod(G.DistanceCommanded,500) == 0
updateDrawing();
end
end
function move_out()
%function moves a single robot out of the 'starting block'
CostX= pushing_cost('x');
CostY= pushing_cost('y');
if CostX>= CostY %the selected robot is inside the surrounding box
push('y')
else
push('y')
end
end
function cost = pushing_cost(direction)
%global G
switch direction
case 'x'
cost = abs(G.selected(1)-G.borderRight(1))+abs(G.selected(2)-G.borderRight(2));
case 'y'
cost = abs(G.selected(1)-G.borderBottom(1))+abs(G.selected(2)-G.borderBottom(2));
end
end
function push(direction)
% function pops a single robot out from the starting configuration
%global G
switch direction
case 'x'
% do nothing, already popped. This seems to never happen
case 'y'
imax = abs(G.obstacle(1)-G.borderBottom(1))+1;
for i=1:imax
updateTarget('u')
select()
end
imax = abs(G.obstacle(2)-G.borderBottom(2));
for i = 1:imax
updateTarget('l')
select()
end
while isinbox(1,'y')==1 % 1 = G.selected
updateTarget('d')
select()
end
go_to_push('l')
end
end
function go_to_push(direction)
%global G
switch direction
case 'l'
while isinbox(2)==1 % 2 = G.obstacle
updateTarget('u')
select()
end
while G.obstacle(2)<= G.borderRight(2)
updateTarget('l')
select()
end
while G.obstacle(1)< G.borderRight(1)
updateTarget('d')
select()
end
while G.obstacle(2)> G.selected(2)+1
updateTarget('r')
select()
end
end
end
function push_to_goal(i)
%global G
[ydist,xdist] = dist_to_goal(i);
while xdist ~=0
updateTarget('r')
[ydist,xdist] = dist_to_goal(i);
end
updateTarget('u')
G.selected(1) = G.selected(1) + 1;
updateTarget('r')
G.selected(2) = G.selected(2) + 1;
while ydist ~=0
updateTarget('d')
[ydist,xdist] = dist_to_goal(i); %#ok<NASGU>
end
G.robot_pos(G.Target(i,1), G.Target(i,2))=G.INTARGET1; % Grey
end
function [ydist,xdist] = dist_to_goal(i)
%global G
target=G.Target(i,:);
ypicked = G.selected(1) ; xpicked = G.selected(2);
xdist = target(1,2) - xpicked;
ydist = target(1,1)- ypicked;
end
function out = isinbox(a, direction)
%global G
[y,x]=find(G.robot_pos==G.ROBOT1);
%top_robot = max(y);
%left_robot = min(x(y == top_robot));
G.borderRight = [max(y) max(x)];
G.borderBottom = [min(y) min(x)];
switch(a)
case 1,
if (direction=='y') && size(find(y == max(y)),1) == 1 %the selected robot is outside of the surronding box (on the top)
out = 0;
elseif (direction=='x') && size(find(x == max(x)),1) == 1 %the selected robot is outside of the surronding box (on the top)
out = 0;
else
out = 1;
end
case 2,
if G.obstacle(1)< G.borderBottom(1) || G.obstacle(2)> G.borderBottom(2)
out = 0;
else
out = 1;
end
end
end
function tryMove(stx,sty,endx,endy)
%function: tries to move robot from st(x,y) to end(x,y)
%global G
if G.robot_pos(endx,endy)==0 && G.obstacle_pos(endx,endy)==0 %TODO: make a function called "collisionTest"
G.DistanceTotal = G.DistanceTotal+1;
robotValnew = G.robot_pos(stx,sty);
G.robot_pos(endx,endy)=robotValnew;
G.robot_pos(stx,sty)=0;
end
end
function moveto(t)
%function: move the array of robots in direction t
%global G
if t=='l' || t=='r' || t=='u' ||t=='d'
G.DistanceCommanded = G.DistanceCommanded +1;
end
%We only touch the robots
%[rows(small at bottom), cols( small left)]
[iY,iX] = find(G.robot_pos>0);
ind = [iX,iY];
% rearrange the indices so that we move the robots in the correct order
switch t
case 'l'
ind = sortrows(ind,1);
case 'r'
ind = sortrows(ind,-1);
case 'u'
ind = sortrows(ind,-2);
case 'd'
ind = sortrows(ind,2);
end
% move the robots
for c = 1:numel(iX)
j = ind(c,1);
i = ind(c,2);
if t == 'l' && j>1
tryMove(i,j,i,j-1);
elseif t == 'r' && j<G.n-1
tryMove(i,j,i,j+1);
elseif t == 'u' && i < G.m-1
tryMove(i,j,i+1,j);
elseif t == 'd' && i>1
tryMove(i,j,i-1,j);
end
end
end
function moverobots()
%global G
%m1=2; %height
%n1=4; %width
G.robot_pos=G.EMPTY*ones(G.m,G.n); %initialize grid to be empty
%place the obstacle
G.obstacle_pos = zeros(size(G.robot_pos));
G.obstacle=[G.m1+size(G.BW,1)+1,G.n1+size(G.BW,2)+1];
G.obstacle_pos(G.obstacle(1),G.obstacle(2)) = G.OBST;
%%% END SINGLE OBSTACLE
%%%%%%%%%%%% START Sorting array:
%two boxes of robots
if (G.lastrow) ~=0
G.robot_pos((G.obstacle(1))+(-G.m1:-2),G.obstacle(2)+(1:G.n1))=G.ROBOT1*ones([G.m1-1,G.n1]);
G.robot_pos((G.obstacle(1))-1,G.obstacle(2)+(1:G.lastrow))=G.ROBOT1*ones([1,G.lastrow]);
else
G.robot_pos((G.obstacle(1))+(-m1:-1),G.obstacle(2)+(1:G.n1))=G.ROBOT1*ones([G.m1,G.n1]);
end
YDist = size(G.BW,1)+1;
XDist = size(G.BW,2)+1;
[y,x]=find(G.robot_pos==G.ROBOT1);
G.S = G.robot_pos; % starting position of the robots
G.F = zeros(size(G.robot_pos));
top_robot = max(y);
left_robot = min(x(y == top_robot));
G.borderRight = [max(y) max(x)];
G.borderBottom = [min(y) min(x)];
G.selected = [top_robot left_robot];
[ytarget, xtarget] = find (G.BW>0);
%G.robot_pos(ytarget, xtarget)= G.ROBOT2 ;
G.borderRight = [max(y) max(x)];
G.borderBottom = [min(y) min(x)];
G.Target = repmat(G.selected + [YDist -XDist],size(ytarget,1),1)-[ytarget, -xtarget];
G.Target = sortrows(G.Target,-1);
for i=1:size(G.Target,1)
G.F(G.Target(i,1)+1,G.Target(i,2))=G.TARGET;
end
colormap(G.colormap(1:4,:))
%G.axis=pcolor(G.robot_pos + G.obstacle_pos );
%G.axis=imagesc(G.robot_pos + G.obstacle_pos);
G.axis=imagesc(G.obstacle_pos + max(3*(G.robot_pos>0),2*(G.S>0)+2*(G.F>0)));
set(gca,'box','off','xTick',[],'ytick',[],'ydir','normal','Visible','off');
%set(G.axis,'edgealpha',.08)
axis equal
axis tight
G.title = title(['Total moves = ', num2str(G.numMoves)]);
end
function updateDrawing
%global G MAKE_MOVIE FrameCount
colormap(G.colormap(1:4,:))
%set(G.axis,'CData', G.robot_pos+ G.obstacle_pos);
set(G.axis,'CData', G.obstacle_pos + max(3*(G.robot_pos>0),2*(G.S>0)+2*(G.F>0)));
set(G.title,'String',['Total commands = ', num2str(G.DistanceCommanded),', total moves = ', num2str(G.DistanceTotal)]);
%drawnow expose
%set(G.fig, 'position',[-98 57 1774 965]);
drawnow
if(MAKE_MOVIE)
FrameCount=FrameCount+1;
F = getframe;
fname = sprintf('img/%s%06d.png',G.MOVIE_NAME,FrameCount);
imwrite(F.cdata, fname,'png');
while(FrameCount < 10)
updateDrawing
end
clear F
end
end
end