Monday, March 28, 2011

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

Thursday, March 24, 2011

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.