Exercise 11, Problem 4

Cleaning the box.
Make a program that 'cleans' the box by going from side to side and back in a pattern that covers the hold box. Aulocalize should be used to assure correct position in the box.

N=10

laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser "push t='1.0' cmd='localize'"

turn 45
fwd 0.7
turn 135
drivew -0.5 0.5 -180:($targetdist<0.05)
drivew 0.5 0.45 0:($targetdist<0.05)
drivew -0.5 0.4 -180:($targetdist<0.05)
drivew 0.5 0.35 0:($targetdist<0.05)
drivew -0.5 0.3 -180:($targetdist<0.05)
drivew 0.5 0.25 0:($targetdist<0.05)
drivew -0.5 0.2 -180:($targetdist<0.05)
drivew 0.5 0.15 0:($targetdist<0.05)
drivew -0.5 0.1 -180:($targetdist<0.05)
drivew 0.5 0.05 0:($targetdist<0.05)
drivew -0.5 0.0 -180:($targetdist<0.05)
drivew 0.5 -0.05 0:($targetdist<0.05)
drivew -0.5 -0.1 -180:($targetdist<0.05)
drivew 0.5 -0.15 0:($targetdist<0.05)
drivew -0.5 -0.2 -180:($targetdist<0.05)
drivew 0.5 -0.25 0:($targetdist<0.05)
drivew -0.5 -0.3 -180:($targetdist<0.05)
drivew 0.5 -0.35 0:($targetdist<0.05)
drivew -0.5 -0.4 -180:($targetdist<0.05)
drivew 0.5 -0.45 0:($targetdist<0.05)
drivew -0.5 -0.5 -180:($targetdist<0.05)
drivew 0.5 0.5 90:($targetdist<0.05)

Exercise 11, Problem 3

Boxdriving with Kalman Localization.

Change   the   code   from  problem  two   so   it   uses   kalman   updates   from  the localizer plugin.

periodic Kalman filtering

N=10

laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser "push t='1.0' cmd='localize'"

label "run"
stop
ignoreobstacles
drivew 0.4 0.4 90:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 0.4 -180:($targetdist<0.05)
stop
ignoreobstacles
drivew -0.4 -0.4 -90:($targetdist<0.05)
stop
ignoreobstacles
drivew 0.4 -0.4 0:($targetdist<0.05)
N=N-1
if(N>0) "run"

each corner Kalman filtering

N=10

laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser "localize"

label "run"
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 0.5 90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 0.5 -180:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew -0.5 -0.5 -90:($targetdist<0.05)
stop
wait 0.5
laser "localize"
ignoreobstacles
drivew 0.5 -0.5 0:($targetdist<0.05)
N=N-1
if(N>0) "run"

Exercise 11, Problem 2

Generation of Boxdriving in SMRCL

Make smrcl  code  that  makes an smr  follow  the  targetposes starting  in  the middle   of   the   box  then   going   to   the   corners and   then   continueing N times around in the square defined by the cornerpoints.

laser "addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser "addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser "addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser "addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser "setinitpose x=0 y=0 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser "localize"

ignoreobstacles
drive 0.4 0.4 90:($targetdist<0.05)
ignoreobstacles
drive -0.4 0.4 -180:($targetdist<0.05)
ignoreobstacles
drive -0.4 -0.4 -90:($targetdist<0.05)
ignoreobstacles
drive 0.4 -0.4 0:($targetdist<0.05)
ignoreobstacles
drive 0.4 0.4 90:($targetdist<0.05)

Exercise 11, Problem 1

 Initialization of the localiser.

laser cmd="addline startx=-0.9 starty=0.9 endx=0.9 endy=0.9 name='N'"
laser cmd="addline startx=-0.9 starty=-0.9 endx=0.9 endy=-0.9 name='S'"
laser cmd="addline startx=-0.9 starty=-0.9 endx=-0.9 endy=0.9 name='V'"
laser cmd="addline startx=0.9 starty=-0.9 endx=0.9 endy=0.9 name='E'"

laser cmd="setinitpose x=0 y=0 th=0"
laser cmd="setinitcov Cx=0.1 Cy=0.1 Cth=0.1"

laser cmd="localize"

 The localizer is know ready. It will make a new pose estimate each time the command  'localize'  is given.

Exercise 8, Problem 2.2

After fervently fighting for hours with correct matrix sizing, we can see that the system is now more accurate.

Exercise 12, Task 6

The function below creates a file that contains instructions for the robot depending on the resulted list of cells.



function SMRfile(q)
fid = fopen('SMRcommand.txt','w');
for i=1:(size(q,1)-1)
    x= 0.05*(q(i+1,1)-q(i,1));
    y= 0.05*(q(i+1,2)-q(i,2));
    
    if(y==0 && x>0)
        th=0;
    elseif(y==0 && x<0)
        th=180;
    elseif(x==0 && y>0)
        th=90;
    elseif(x==0 && y<0)
        th=-90;
    elseif(x>0 && y>0)
        th=45;
    elseif(x>0 && y<0)
        th=-45;
    elseif(y>0)
        th=135;
    else
        th=-135;
    end
    
    fprintf(fid,'drive %f %f %f:($targetdist<0)\n',x,y,th);
end
fprintf(fid,'stop');
fclose(fid)
end

Exercise 12, Task 5

The results for the given case are:


distmap =


3.41   3.00      0      0      0      0      0       0      0   0
3.00   2.00      0  14.82  15.23  15.64  16.05 16.46  16.87   0
   0   3.00      0  13.82  14.23  14.64  15.05   15.46  16.46   0
   0   4.00      0  12.82  13.23  13.64  14.05   15.05  16.05   0
   0   5.00      0  11.82  12.23  12.64  13.64   14.64  15.64   0
   0   6.00      0  10.82  11.23  12.23  13.23   14.23  15.23   0
   0   7.00      0   9.82  10.82  11.82  12.82   13.82  14.82   0
   0   8.00   8.41   9.41  10.41  11.41  12.41   13.41  14.41   0
   0   9.00   9.41   9.82  10.82  11.82  12.82   13.82  14.82   0
   0  10.00  10.41  10.82  11.23  12.23  13.23   14.23      0   0


route =


     9  9
     8  8
     8  7
     8  6
     8  5
     8  4
     8  3
     7  2
     6  2
     5  2
     4  2
     3  2
     2  2



map =


     0  0  1  0  0  0  0  0  0  0
     0  2  1  0  0  0  0  0  0  0
     1  2  1  0  0  0  0  0  0  0
     1  2  1  0  0  0  0  0  0  0
     1  2  1  0  0  0  0  0  0  0
     1  2  1  0  0  0  0  0  0  0
     1  2  1  0  0  0  0  0  0  0
     1  0  2  2  2  2  2  2  0  0
     1  0  0  0  0  0  0  0  2  0
     1  0  0  0  0  0  0  0  0  0

Exercise 12, Task 4

After computing the distance map with the previous function, the following one finds the minimum distance route from the start point to the final one.


function route = findroute(startcell,finalcell,distmap)
global map;
q = startcell;
cell = startcell;
d = 100000;


while(cell(1)~=finalcell(1) || cell(2)~=finalcell(2))
    n = neighbours(cell);
    for i=1:size(n,1)
        if(map(n(i,1),n(i,2))==0 && distmap(n(i,1),n(i,2))~=0 && distmap(n(i,1),n(i,2))<d)
            d = distmap(n(i,1),n(i,2));
            next = n(i,:)
        end
    end
    q = insert(next,q);
    cell = next;
end


for i=1:size(q,1)
    map(q(i,1),q(i,2))=2;
end 
map
route = q
end


The function returns the list of cells that form the route as well as the map which shows the route (represented by values of 2)

Exercise 12, Task 3

The function starts in the final point and calculates the minimum distances of the cells until it reaches the start point.

function matr = makewave(startcell,finalcell)

global map;

distmap = zeros(size(map,1),size(map,2));

distmap(finalcell(1),finalcell(2))=2;

queue = finalcell;

reached = 0;

while(size(queue,1) && reached==0)

    [crt,queue] = retrieve(queue);

    n = neighbours(crt);

    for i=1:size(n,1)

        if(map(n(i,1),n(i,2))==0)

           d = distmap(crt(1),crt(2)) + celldist(crt,n(i,:));

           if(distmap(n(i,1),n(i,2))==0) 

                distmap(n(i,1),n(i,2))= d;

                queue = insert(n(i,:),queue);

           elseif(distmap(n(i,1),n(i,2)) > d) 

                distmap(n(i,1),n(i,2)) = d;

           end

        end

        if(n(i,:)==startcell)

            reached = 1;

        end    

    end

end    

if(reached==0)

    fprintf('The wave did not reach the starting point.')

end

matr = distmap;

end

Exercise 12, Task 2

The function to find the neighbours of a cell is:

function list = neighbours(cell)
global map;
x = cell(1);
y = cell(2);


if(x==1 && y==1)
    list = [ x   y+1;
            x+1  y+1;
            x+1   y];
elseif(x==1 && y==size(map,2))
    list = [x+1   y;
             x   y-1;
            x+1  y-1];
elseif(x==1)
    list = [ x   y+1;
            x+1  y+1;
            x+1   y;
             x   y-1;
            x+1  y-1];
elseif(y==1 && x==size(map,1))
    list = [x-1  y+1;
             x   y+1;
            x-1   y];
elseif(y==1)
    list = [x-1  y+1;
             x   y+1;
            x+1  y+1;
            x-1   y;
            x+1   y];
elseif(x==size(map,1) && y==size(map,2))
    list = [x-1   y;
            x-1  y-1;
             x   y-1];
elseif(x==size(map,1))
    list = [x-1  y+1;
             x   y+1;
            x-1   y;
            x-1  y-1;
             x   y-1];
elseif(y==size(map,2))
    list = [x-1   y;
            x+1   y;
            x-1  y-1;
             x   y-1;
            x+1  y-1];
else 
    list = [x-1  y+1;
             x   y+1;
            x+1  y+1;
            x-1   y;
            x+1   y;
            x-1  y-1;
             x   y-1;
            x+1  y-1];   
end    
end


Note: Only the valid neighbours of the cell are taken, i.e. the ones that do not exceed the borders of the matrix.