Project Presentation

Final Project

 

%prepare the planner for a new graph
laser "resetplanner"
laser "scanget"
laser "resetlocalizer"
%----------------------------------------------
%add nodes to the graph
laser "addpoint pno=1 x=0.5 y=0.5"
laser "addpoint pno=2 x=-0.5 y=1.5"
laser "addpoint pno=3 x=0.5 y=1.5"
laser "addpoint pno=4 x=3.5 y=1.5"
laser "addpoint pno=5 x=4.5 y=1.5"
laser "addpoint pno=6 x=-0.5 y=3.5"
laser "addpoint pno=7 x=0.5 y=3.5"
laser "addpoint pno=8 x=3.6 y=3.5"
laser "addpoint pno=9 x=4.5 y=3.5"
laser "addpoint pno=10 x=0.5 y=4.6"
laser "addpoint pno=11 x=3.6 y=4.6"
laser "addpoint pno=12 x=1.85 y=3.4"
laser "addpoint pno=13 x=2.25 y=3.4"
laser "addpoint pno=14 x=-0.5 y=2.5"
laser "addpoint pno=15 x=0.5 y=2.2"
laser "addpoint pno=16 x=3.6 y=2.5"
laser "addpoint pno=17 x=4.5 y=1.9"
laser "addpoint pno=18 x=4.5 y=3"
laser "addpoint pno=19 x=2 y=2.5"
laser "addpoint pno=20 x=2 y=3"
%laser "addpoint pno=21 x=2.05 y=3.3"
laser "addpoint pno=22 x=1.3 y=3.45"
laser "addpoint pno=23 x=2.7 y=3.45"
laser "addpoint pno=24 x=1.3 y=4"
laser "addpoint pno=25 x=2.7 y=4"
laser "addpoint pno=26 x=1.7 y=2.2"
laser "addpoint pno=27 x=2.2 y=2.2"
laser "addpoint pno=28 x=1.5 y=2.2"
laser "addpoint pno=29 x=2.5 y=2.2"
laser "addpoint pno=30 x=1.5 y=0.5"
laser "addpoint pno=31 x=2.5 y=0.65"

%----------------------------------------------
%add connections between points
laser "addcon pno1=1 pno2=3"
laser "addcon pno1=3 pno2=1"

laser "addcon pno1=3 pno2=15"
laser "addcon pno1=15 pno2=3"

laser "addcon pno1=2 pno2=14"
laser "addcon pno1=14 pno2=2"

laser "addcon pno1=6 pno2=14"
laser "addcon pno1=14 pno2=6"

laser "addcon pno1=15 pno2=14"
laser "addcon pno1=14 pno2=15"

laser "addcon pno1=7 pno2=15"
laser "addcon pno1=15 pno2=7"

laser "addcon pno1=7 pno2=10"
laser "addcon pno1=10 pno2=7"

laser "addcon pno1=15 pno2=28"
laser "addcon pno1=28 pno2=15"

laser "addcon pno1=4 pno2=16"
laser "addcon pno1=16 pno2=4"

laser "addcon pno1=5 pno2=17"
laser "addcon pno1=17 pno2=5"

laser "addcon pno1=16 pno2=17"
laser "addcon pno1=17 pno2=16"

laser "addcon pno1=16 pno2=29"
laser "addcon pno1=29 pno2=16"

laser "addcon pno1=8 pno2=16"
laser "addcon pno1=16 pno2=8"

laser "addcon pno1=9 pno2=18"
laser "addcon pno1=18 pno2=9"

laser "addcon pno1=16 pno2=18"
laser "addcon pno1=18 pno2=16"

laser "addcon pno1=8 pno2=11"
laser "addcon pno1=11 pno2=8"

laser "addcon pno1=19 pno2=20"
laser "addcon pno1=20 pno2=19"

laser "addcon pno1=10 pno2=11"
laser "addcon pno1=11 pno2=10"

laser "addcon pno1=20 pno2=12"
laser "addcon pno1=12 pno2=20"

laser "addcon pno1=20 pno2=13"
laser "addcon pno1=13 pno2=20"

laser "addcon pno1=12 pno2=13"
laser "addcon pno1=13 pno2=12"

laser "addcon pno1=12 pno2=22"
laser "addcon pno1=22 pno2=12"

laser "addcon pno1=22 pno2=24"
laser "addcon pno1=24 pno2=22"

laser "addcon pno1=13 pno2=23"
laser "addcon pno1=23 pno2=13"

laser "addcon pno1=23 pno2=25"
laser "addcon pno1=25 pno2=23"

laser "addcon pno1=19 pno2=26"
laser "addcon pno1=26 pno2=19"

laser "addcon pno1=19 pno2=27"
laser "addcon pno1=27 pno2=19"

laser "addcon pno1=27 pno2=26"
laser "addcon pno1=26 pno2=27"

laser "addcon pno1=28 pno2=26"
laser "addcon pno1=26 pno2=28"

laser "addcon pno1=27 pno2=29"
laser "addcon pno1=29 pno2=27"

laser "addcon pno1=1 pno2=30"
laser "addcon pno1=30 pno2=1"

laser "addcon pno1=30 pno2=31"
laser "addcon pno1=31 pno2=30"

%----------------------------------------------
%cost calculation
laser "calculatecost"
%----------------------------------------------
%add walls

laser "addline startx=0 starty=0 endx=0 endy=1.8 name='WD'"
laser "addline startx=0 starty=0 endx=1.8 endy=0 name='SL'"
laser "addline startx=4 starty=0 endx=2.2 endy=0 name='SR'"
laser "addline startx=4 starty=0 endx=4 endy=1.8 name='ED'"
laser "addline startx=0 starty=5 endx=0 endy=3.2 name='WU'"
laser "addline startx=0 starty=5 endx=1.8 endy=5 name='NL'"
laser "addline startx=4 starty=5 endx=2.2 endy=5 name='NR'"
laser "addline startx=4 starty=5 endx=4 endy=3.2 name='EU'"
laser "addline startx=1.7 starty=2.5 endx=1.7 endy=3.1 name='M1'"
laser "addline startx=1.7 starty=3.1 endx=0.9 endy=3.1 name='M2'"
laser "addline startx=0.9 starty=3.1 endx=0.9 endy=4.3 name='M3'"
laser "addline startx=0.9 starty=4.3 endx=3.1 endy=4.3 name='M4'"
laser "addline startx=2 starty=3.7 endx=2 endy=4.3 name='M5'"
laser "addline startx=1.5 starty=3.7 endx=2.5 endy=3.7 name='M6'"
laser "addline startx=3.1 starty=4.3 endx=3.1 endy=3.1 name='M7'"
laser "addline startx=3.1 starty=3.1 endx=2.3 endy=3.1 name='M8'"
laser "addline startx=2.3 starty=3.1 endx=2.3 endy=2.5 name='M9'"
laser "addline startx=4.2 starty=2.4 endx=4.2 endy=2.6 name='T1'"
laser "addline startx=4.2 starty=2.6 endx=4.3 endy=2.6 name='T2'"
laser "addline startx=4.3 starty=2.6 endx=4.3 endy=2.4 name='T3'"
laser "addline startx=4.3 starty=2.4 endx=4.2 endy=2.4 name='T4'"

%----------------------------------------------------------------
%set initial position
laser "setinitpose x=0.5 y=0.5 th=0"
laser "setinitcov Cx=0.1 Cy=0.1 Cth=0.1"
set "$odox" 0.5
set "$odoy" 0.5
set "$odoth" 0

laser "scanset def=urg"
laser "scanset logopen"
laser "scanset log=1"
laser "odopose log=true"
%----------------------------------------------------------------
%set localizer period
laser "push t='0.5' cmd='localize'"
%laser "localize"

%----------------------------------------------------------------
%guidemark reading point coordinates and angles
array "gmx" 23
array "gmy" 23
array "gmth" 23
pi=3.1416
gmx[1]=-0.5
gmx[2]=0.5
gmx[3]=3.5
gmx[4]=4.5
gmx[5]=-0.5
gmx[6]=0.5
gmx[7]=3.5
gmx[8]=4.5
gmx[9]=0.5
gmx[10]=3.5
gmx[11]=0.5
gmx[12]=3.5
gmx[13]=1.3
gmx[14]=2.9
gmx[15]=3.5
gmx[16]=3.5
gmx[17]=2.5
gmx[18]=1.5
gmx[19]=0.5
gmx[20]=1.5
gmx[21]=2.5
gmx[22]=0.5

gmy[1]=1.5
gmy[2]=1.5
gmy[3]=1.5
gmy[4]=1.5
gmy[5]=3.5
gmy[6]=3.5
gmy[7]=3.5
gmy[8]=3.5
gmy[9]=4.6
gmy[10]=4.6
gmy[11]=4.6
gmy[12]=4.6
gmy[13]=4
gmy[14]=4
gmy[15]=1.5
gmy[16]=1.5
gmy[17]=2.2
gmy[18]=2.2
gmy[19]=1.5
gmy[20]=0.5
gmy[21]=0.65
gmy[22]=0.5


gmth[1]=0
gmth[2]=pi
gmth[3]=0
gmth[4]=pi
gmth[5]=0
gmth[6]=pi
gmth[7]=0
gmth[8]=pi
gmth[9]=pi
gmth[10]=0
gmth[11]=pi/2
gmth[12]=pi/2
gmth[13]=0
gmth[14]=pi
gmth[15]=-pi/2-pi/12
gmth[16]=pi
gmth[17]=-pi/2
gmth[18]=-pi/2
gmth[19]=0
gmth[20]=pi/2
gmth[21]=pi/2
gmth[22]=0


%----------------------------------------------
%send to predefined guidemark
gm=2
%----------------------------------------------
laser "localize getonly"
xp=$odox
yp=$odoy
thp=$odoth
eval xp
eval yp
eval thp
k=65
gmnotfound=0

label "main"
call "findroute"
call "gmark"
if (gmnotfound==1 & gm<15) "failgmk"
if (gm!=99) "main"
label "failgmk"
gm=15
speak "Going to count the shiny round thingies"
call "findroute"

%---------------------
%ballfinding algorithm
%---------------------
vision "imageget"
wait 1
vision "imageget"
wait 1
vision "ball red smrcl"
wait 1
eval $vis4
tointstring $vis4
stringcat "I see " "$str" " red balls"
speak "$string"
vision "ball blue smrcl"
wait 1
eval $vis4
tointstring $vis4
stringcat "and " "$str" " blue balls"
speak "$string"
wait 1

%---------------------
%box finding algorithm
%---------------------
done=15
speak "Searching for tricky square object"
label "findbox"
done=done+1
gm=done
if (gm==22) "finish"
call "findroute"
call "gmark"
if ($gmkx > 2.3 | $guidemarkok<1) "findbox"
invtrans $l0 $l1 $l2 $odox $odoy $odoth
wait 0.1
rx=$res0
ry=$res1
rth=$res2
invtrans $gmkx $gmky $gmkkappa rx ry rth
wait 0.1
angdeg=$res2*180/pi
angdeg=normalizeangledeg(angdeg)
stringcat "Found box at coordinates x equals " $res0 " meters, y equals " $res1 " meters"
speak "$string"
wait 2
goto "finish"

%------------------------------------------------
%route finding
%------------------------------------------------
label "findroute"

targetx=gmx[gm]
targety=gmy[gm]
targetth=gmth[gm]

wait 1
stringcat "findroute startx="xp" starty="yp" endx="targetx" endy="targety" id="k""
laser "$string"
wait 1 :($l9 == k)
eval $l4
Nmax=$l4
k=k+1
%Start sequence
nxtp=Nmax
if (Nmax == 1) "skip"
nxtp = Nmax-1
label "skip"
stringcat "getpoint p=" nxtp " id=" nxtp""
laser "$string"
wait 1:($l9 == nxtp)
angle = atan2(y1,x1)-$odoth+$l2
angle = normalizeanglerad(angle)
ignoreobstacles
turn angle "rad" @v0.2
idle

label "Drive"
stringcat "getpoint p=" Nmax " id=" Nmax""
laser "$string"
wait 1:($l9 == Nmax)
eval $l5
eval $l6
eval $l7
ignoreobstacles
drivew $l5 $l6 $l7 "rad" @v 0.3 :($targetdist < 0.05)
stop
Nmax = Nmax-1
if (Nmax > -1) "Drive"
xp = targetx
yp = targety
%End sequence
laser "localize getonly"
eval $l2
eval $odoth
angle=targetth-$odoth+$l2
angle=normalizeanglerad(angle)
ignoreobstacles
turn angle "rad" @v 0.1
idle
return

%------------------------------------------------
label "gmark"
%------------------------------------------------
%Try reading guidemark
call "tryreading"
if (code > 0) "goodreading"
turn 10 @v0.1
idle
wait 0.2
call "tryreading"
if (code > 0) "goodreading"
turn -20 @v0.1
idle
wait 0.2
call "tryreading"
if (code > 0) "goodreading"
if (gm>14 | gm==99) "bad"
speak "I fail"
goto "bad"

label "goodreading"
if ($gmkx > 2.3) "jmp"
tointstring code
stringcat "Guidemark number  " "$str"
speak "$string"
label "jmp"
wait 0.2
gm=code
return
label "bad"
gmnotfound=1
return

%-----------------------------------------------
%function for reading guidemark
%-----------------------------------------------
label "tryreading"
n=0
label "again"
get "guidemark"
wait 2 : ($guidemarkok)
eval $guidemarkok
code=$guidemarkok
n=n+1
if (n<2) "again"
return

%-----------------------------------------
label "finish"
if (gm!=22) "boxfound"
speak "Could not find box. I am a sad robot"
label "boxfound"
gm=22
speak "I am going home"
call "findroute"
turn 45 @v0.1
fwd -0.2
turn -45 @v0.1
speak "bazinga"
laser "scanset logclose"
laser "odopose log=false"
stop

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)