Code covered by the BSD License  

Highlights from
Massive Uniform Manipulation: Control Large Populations of Simple Robots with a Common Input Signal

image thumbnail
from Massive Uniform Manipulation: Control Large Populations of Simple Robots with a Common Input Signal by Aaron Becker
rearranges n robots controlled to move in unison {up, down, left, right} using 1 obstacle

PosControl_universal
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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

Contact us