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.

Exercise 12, Task 1

To add or extract elements from a queue, the functions below were used:

function q = insert(cell,queue)
i=size(queue,1)+1;
queue(i,:)=cell;
q = queue;
end


function [val,q] = retrieve(queue)
val = queue(1,:);
q = queue(2:(size(queue,1)),:);
end

Exercise 9, Problem 1.2

Nabla H is calculated as stated in the previous exercise, but taking into account the displacement of the laser from the robot. We then obtain:

Exercise 9, Problem 1.3

The line matching script and obtained results are shown below:

Exercise 10, Problem 2

Problem 2 Generation of target poses for driving in a box.

The box has 5 points: the 4 corners and the starting point (pose) in mainloop.m file:

places = [  [0; 0; 0] [0.5; 0.5; pi] [-0.5; 0.5; -pi/2] [-0.5; -0.5; 0] [0.5; -0.5; pi/2] [0.5; 0.5; pi] ];

The sequence of the points with exception handling in mainloop.m file:

if exist('nextPose','var') == 0 %
  nextPose = 1;
elseif nextPose >= length(places(1,:))
  nextPose = 1;
else
  nextPose = nextPose + 1;
end

Constants setup in file constants.m:

simulation = true;
noOfIter = 16; %The number of simulation iterations

Exercise 10, Problem 2.1

Simulate Boxdriving. Run the framework and see that the boxdrive works in simulation.
The result of the simulation reveals the errors in odometry:

Exercise 10, Problem 1.1

Calculation of targetpose in odometry coordinates.

function out = trans(transform,targetPose)
% out <-> odoTargetPose (notation)
% odoTargetPose = TRANS(transform,targetPose)
% Transform a given point in world coordinates (targetPose) to odometry
% coordinates, using the origo of the odometry coordinates in world
% coordinates (transform).
%calculation of targetpose in ordinary coordinates
    t=transform; %for shorter name
    tMatrix=[cos(t(3)) -sin(t(3))  0; sin(t(3))  cos(t(3))  0; 0 0 1];
    temp = tMatrix*targetPose ;
    out = temp + t;
    out(3) = normalizeAngle(out(3));
end

Exercise 10, Problem 1

Calculation of transformation from world coordinates to odometry coordinates.

function transform = findTransform(odoPose, pose)
% transform = FINDTRANSFORM(odoPose,pose)
% Find the transformation from the world coordinates to the odometry
% coordinates given a pose in the odometry coordinates (odoPose) and the
% same point in the world coordinates (pose). The output (transform) is
% simply the origo of the odometry coordinates in the world coordinates
  theta =  normalizeAngle(odoPose(3)-pose(3));
  tMatrix = [cos(theta) -sin(theta) 0;sin(theta) cos(theta) 0;0 0 1];
  transform = -tMatrix*pose + odoPose;
  transform(3) = normalizeAngle(transform(3));
end

Where we have used the angle normalization:

function outAngle = normalizeAngle(inAngle)
    inAngle  = inAngle + 2*pi;
    outAngle = mod(inAngle,2*pi);
    outAngle = outAngle -2*pi;
end

Exercise 6

The script for ball navigation:

pi=3.14159265
ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label11"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label12"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label13"
turn 45
wait 1
if (ad1>ad) "label14"
x=x1
y=y1
th=th1
label "label14"
if (ad2>ad) "label15"
x=x2
y=y2
th=th2
label "label15"
if (ad3>ad) "label16"
x=x3
y=y3
th=th3
label "label16"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label17"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label18"
label "label17"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label18"
fwd 0.5


ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label19"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label110"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label111"
turn 45
wait 1
if (ad1>ad) "label112"
x=x1
y=y1
th=th1
label "label112"
if (ad2>ad) "label113"
x=x2
y=y2
th=th2
label "label113"
if (ad3>ad) "label114"
x=x3
y=y3
th=th3
label "label114"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label115"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label116"
label "label115"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label116"
fwd 0.5

ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label117"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label118"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label119"
turn 45
wait 1
if (ad1>ad) "label120"
x=x1
y=y1
th=th1
label "label120"
if (ad2>ad) "label121"
x=x2
y=y2
th=th2
label "label121"
if (ad3>ad) "label122"
x=x3
y=y3
th=th3
label "label122"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label123"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label124"
label "label123"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label124"
fwd 1.5

turn 180

ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label21"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label22"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label23"
turn 45
wait 1
if (ad1>ad) "label24"
x=x1
y=y1
th=th1
label "label24"
if (ad2>ad) "label25"
x=x2
y=y2
th=th2
label "label25"
if (ad3>ad) "label26"
x=x3
y=y3
th=th3
label "label26"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label27"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label28"
label "label27"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label28"
fwd 0.5


ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label29"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label210"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label211"
turn 45
wait 1
if (ad1>ad) "label212"
x=x1
y=y1
th=th1
label "label212"
if (ad2>ad) "label213"
x=x2
y=y2
th=th2
label "label213"
if (ad3>ad) "label214"
x=x3
y=y3
th=th3
label "label214"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label215"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label216"
label "label215"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label216"
fwd 0.5

ah=0
ad=100
turn 45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad1=1000
if ($vis1<0.5)>
ad1=sqrt($vis3*$vis3+$vis2*$vis2)
x1=$vis2
y1=$vis3
th1=pi/4
label "label217"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad2=1000
if ($vis1<0.5)>
ad2=sqrt($vis3*$vis3+$vis2*$vis2)
x2=$vis2
y2=$vis3
th2=0
label "label218"
turn -45
wait 1
vision "ball2 debug=true blue=false smrcl"
wait 1
eval $vis1
ad3=1000
if ($vis1<0.5)>
ad3=sqrt($vis3*$vis3+$vis2*$vis2)
x3=$vis2
y3=$vis3
th3=-pi/4
label "label219"
turn 45
wait 1
if (ad1>ad) "label220"
x=x1
y=y1
th=th1
label "label220"
if (ad2>ad) "label221"
x=x2
y=y2
th=th2
label "label221"
if (ad3>ad) "label222"
x=x3
y=y3
th=th3
label "label222"
ahc=atan2(y,x)
ahc=ahc+th
eval ahc
if (ahc>0) "label223"
y=y+0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=0.7*(sqrt(y*y+x*x))
turn ah
eval ad
fwd ad@v0.5
aht=-ah
turn aht
goto "label224"
label "label223"
x=x-0.5
ah=atan2(y,x)
eval th
ah=ah+th
ah=180*(ah/pi)
eval ah
ad=sqrt(y*y+x*x)
turn ah
eval ad
fwd ad@v0.5
aht=90-ah
turn aht
turnr 0.5 -90
label "label224"
fwd 1

turn 180




Results
The results are highly dependent of the camera calibration. The success rate is high for using only one ball but for more the robot fails in getting around the balls because of accumulated odometry errors.

Exercise 9, Problem 1.1

As stated in the book, the Jacobian matrix of the world to robot frame transformation is:

This is valid for the case when the robot and laser positions can be superimposed. In our case, due to laser displacement, the third term in the second row will be non-zero, as shown in the following problem.

Problem 8, Exercise 2.2

By use and extension of the previously created function, we obtain:


function [ projectedLine lineCov ] = projectToLaser( worldLine,poseIn, covIn)
%[projectedLine, lineCov] = PROJECTTOLASER(worldLine,poseIn,covIn)
%Project a word line to the laser scanner frame given the
%world line, the robot pose and robot pose covariance. Note that the laser
%scanner pose in the robot frame is read globally
%   worldLine: The line in world coordinates
%   poseIn: The robot pose
%   covIn: The robot pose covariance
%
%   projectedLine: The line parameters in the laser scanner frame
%   lineCov: The covariance of the line parameters

%% Constants
global lsrRelPose % The laser scanner pose in the robot frame is read globally



%% Calculation

[h(1), h(2)]=world2robot(poseIn, worldLine);
[projectedLine(1), projectedLine(2)]=world2robot(lsrRelPose, h);


lineCov = zeros(2,2)+covIn;
end

The results are shown below. We will note that the last three pictures also show the extracted lines from the data and are thus also a solution for the last problem in this exercise.

Results with 0 uncertainty:
Line track:

Circular track:

Box track:

After adding some uncertainties we notice that the extracted lines do not perfectly fit the world:

Line track:

Circle track:

Box Track: