UltraLazer
11-15-2011, 02:58 PM
Here is an experiment I have been working on for a sumo-type autonomous battle robot. The competition I hope to enter has sub 13" diameter autonomous robots trying to push one another outside of a 4ft ring. I decided to go with a 3 wheel holonomic design. One of the drawbacks of this motor configuration is that is has less (1/3?) over all torque/power for pushing. The idea then is to build a fast and nimble robot that can take advantage of its momentum to hit the opponent. More like a wrecking ball and less like a bulldozer. Let the opponent drive itself close to the edge and tap it out.
Since the sumo robot ring is circular the best angle to hit the opponent is the angle between the enemy robot and the center of the circle. Along this trajectory the enemy robot is closest to the edge. Right now the included code evolves a quadratic bezier curve that starts on our robot, ends on the enemy robot and has 2 control points so that it intersects with the enemy robot at the optimum angle.
The genetic algorithm (GA) approximates the final high res bezier-path in a 16x16 world. It generates an answer within a definable range of optimum. This can take 1-10 generations. If the GA converges on a solution with too much error there is an extinction and a new population gives it a shot. Because the search space is only 16x16 there are robot/enemy x,y configurations that are more difficult. Also the way in which the initial random population is generated greatly impacts the convergence time. I have not verified this but it seems that slightly 'less' random values work better, (less evenly distributed?)
The next trick will be including the beziers length in the fitness function. Seems simple enough but I'm unsure how to weight the angle vs length of the line properly. It would be great to evolve beziers that allow the robot to accelerate to a velocity in relation to the opponents distance from the edge. Or even the longest bezier so that the bot can reach top speed while on the path. Does any one have any advice/ideas on how I could go about this? fuzzy degree of membership stuff maybe?
Set the Parallax serial terminal font size to 6 in the prefs if you run this.
CON
'***********Processor Settings**********
_clkfreq = 80_000_000
_clkmode = xtal1 + pll16x
'***************************************
_maxPoints = 64
_F1 = 1.0
_F2 = 2.0
_F3 = 3.0
'GA constants
_Convergence = 45 'this is how self-similar a population msut be to have 'converged' on a solution
_Population = 22 'initial population, larger populations have more genetic potential but converge slower
_MutationRate = 1 'number loops where a gene other than a parent has a 50/50 of mutating by 1 bit. Bigger numbers slow convergence or make it impossible
_okErrorLevel = 10 'GA output must be within '_okErrorLevel' degrees of optimum
OBJ
Q : "SPIN_TrigPack"
F32 : "F32"
FPS : "FloatString"
PST : "Parallax Serial Terminal"
VAR
'Bezier path vars
LONG fBx[_maxPoints], fBy[_maxPoints] 'bezier float. point arrays
LONG pathAngl[_maxPoints], pathDist[_maxPoints] 'bezier line to polar path arrays
'GA vars
LONG BezierChromosome[_Population]
LONG Convergence[_Population]
LONG lenght
'Global vars
BYTE robot_x, robot_y, enemy_x, enemy_y
LONG pathlenght
BYTE qStr[32]
PUB Start
PST.Start(115200)
F32.start
Q.Start_Driver
robot_x := 4
robot_y := 5
enemy_x := 20
enemy_y := 20
repeat
PST.clear
PST.Position(robot_x, robot_y)
PST.char("R")
PST.Position(enemy_x, enemy_y)
PST.char("E")
PST.Position(8, 8)
PST.char("+")'center point
geneticBezier
PST.Position(robot_x, robot_y)
PST.char("R")
PST.Position(enemy_x, enemy_y)
PST.char("E")
PST.Position(8, 8)
PST.char("+")'center point
PST.CharIn
enemy_x := randomRange(15,0)
enemy_Y := randomRange(15,0)
PUB fitnessBezier(P1x, P1y, P2x, P2y, P3x, P3y, P4x, P4y, Resolution) : theta | i, n, t, a, b, c, d, x, y
'P1 - Start point, P2 - Control point 1, P3 - Control point 2, P4 - End Point
'Only derives the final point of the bezier, returns the angle between the last bezier point and P4x, P4y
i := Resolution-2
n := F32.FFloat(Resolution-1)
P1x := F32.FFloat(P1x)
P1y := F32.FFloat(P1y)
P2x := F32.FFloat(P2x)
P2y := F32.FFloat(P2y)
P3x := F32.FFloat(P3x)
P3y := F32.FFloat(P3y)
P4x := F32.FFloat(P4x)
P4y := F32.FFloat(P4y)
'********* Terms ********
t := F32.FDiv(F32.FFloat(i),n)
a := F32.Pow(F32.FSub(_F1, t), _F3)
b := F32.FMul(F32.FMul(_F3,t),F32.Pow(F32.FSub(_F1, t), _F2))
c := F32.FMul(F32.FMul(_F3, F32.Pow(t, _F2)), F32.FSub(_F1, t))
d := F32.Pow(t, _F3)
'********* Bezier Point Components ********
x := F32.FAdd(F32.FAdd(F32.FAdd(F32.FMul(a, P1x), F32.FMul(b, P2x)),F32.FMul(c, P3x)), F32.FMul(d, P4x))
y := F32.FAdd(F32.FAdd(F32.FAdd(F32.FMul(a, P1y), F32.FMul(b, P2y)),F32.FMul(c, P3y)), F32.FMul(d, P4y))
PST.Position(F32.FRound(x), F32.FRound(y))
PST.Dec(I)
theta := vectorToDegree(F32.FRound(x), F32.FRound(y), F32.FRound(p4x), F32.FRound(p4y))
PUB pathBezier(P1x, P1y, P2x, P2y, P3x, P3y, P4x, P4y, Resolution) : l | i, n, t, a, b, c, d, mx, my
'P1 - Start point, P2 - Control point 1, P3 - Control point 2, P4 - End Point
i := 1
n := F32.FFloat(Resolution--)
P1x := F32.FFloat(P1x)
P1y := F32.FFloat(P1y)
P2x := F32.FFloat(P2x)
P2y := F32.FFloat(P2y)
P3x := F32.FFloat(P3x)
P3y := F32.FFloat(P3y)
P4x := F32.FFloat(P4x)
P4y := F32.FFloat(P4y)
REPEAT Resolution
'********* Terms ********
t := F32.FDiv(F32.FFloat(i),n)
a := F32.Pow(F32.FSub(_F1, t), _F3)
b := F32.FMul(F32.FMul(_F3,t),F32.Pow(F32.FSub(_F1, t), _F2))
c := F32.FMul(F32.FMul(_F3, F32.Pow(t, _F2)), F32.FSub(_F1, t))
d := F32.Pow(t, _F3)
'********* Bezier Point Components ********
fBx[i] := F32.FAdd(F32.FAdd(F32.FAdd(F32.FMul(a, P1x), F32.FMul(b, P2x)),F32.FMul(c, P3x)), F32.FMul(d, P4x))
fBy[i] := F32.FAdd(F32.FAdd(F32.FAdd(F32.FMul(a, P1y), F32.FMul(b, P2y)),F32.FMul(c, P3y)), F32.FMul(d, P4y))
'tabulate lenght
'mx := F32.FSub(fBx[i], fBx[i-1])
'my := F32.FSub(fBy[i], fBy[i-1])
'l := F32.FAdd(F32.FSqr(F32.FAdd(F32.FMul(mx, mx), F32.FMul(my, my))), l)
'Generate Polar path
'pathAngl[i] := vectorToDegree(F32.FRound(fBx[i]), F32.FRound(fBy[i]), F32.FRound(fBy[i-1]), F32.FRound(fBy[i-1]))
'pathDist[i] := F32.FRound(F32.FSqr(F32.FAdd(F32.FMul(mx, mx), F32.FMul(my, my))))
PST.Position(F32.FRound(fBx[i]), F32.FRound(fBy[i]))
PST.Dec(i)
i++
pathlenght := F32.FRound(l) '
l := vectorToDegree(F32.FRound(fBx[Resolution-1]), F32.FRound(fBy[Resolution-1]), F32.FRound(p4x), F32.FRound(p4y))
PUB geneticBezier| POP, pat, theta, i, d, GC, chromosome_NUM, genotype, t1x, t1y, t2x, t2y, parentN_1, parentN_2, unfitN_1, unfitN_2, unfitN_3, parentF_1, parentF_2, unfitF_1, unfitF_2, unfitF_3, fitness, crossoverMask
POP := 0
REPEAT
POP += 1
chromosome_NUM := 0
LONGFILL(@BezierChromosome[0], 0, _Population-1)
''/\/\/\/\/\/\/\/\/\/\/\/\/\/\/GENERATE INITIAL RANDOM POPULATION\/\/\/\/\/\/\/\/\/\/\/\/\/\/\
REPEAT UNTIL chromosome_NUM == _Population-1
genotype := ?genotype * ?cnt
IF LOOKDOWN(genotype.BYTE[0] & %0000_1111 : robot_x, enemy_x) == 0 AND LOOKDOWN((genotype.BYTE[0] & %1111_0000) >> 4 : robot_y, enemy_y) == 0
IF LOOKDOWN(genotype.BYTE[1] & %0000_1111 : robot_x, enemy_x) == 0 AND LOOKDOWN((genotype.BYTE[1] & %1111_0000) >> 4 : robot_y, enemy_y) == 0
BezierChromosome[++chromosome_NUM] := genotype
BezierChromosome[8] := %0000_0000_0000_0000_1000_1000_1000_1000
theta := vectorToDegree(8, 8, enemy_x, enemy_y) 'optimum fittness requirement - put this or refrence to global var in world loop later
''/\/\/\/\/\/\/\/\/\/\/\/\/\/\/EVOLUTIONARY WORLD LOOP\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\
REPEAT i FROM 1 TO 10000 'i number of generations
parentF_1 := parentF_2 := 100 'initialize the parents, the 2 chromosomes with the lowest fittnes # number will be made parents
unfitF_1 := unfitF_2 := unfitF_3 := 0 'initialize the scum, the 2 chromosomes with the highest fittnes # will be killed off
GC := 0
IF i == 10
quit
'***************************Artificial Selection******************************
REPEAT chromosome_Num FROM 1 TO _Population - 1 'loop to evaluate each chromosome in the population
fitness := 0 'initialize new fitness number
'****************Genotype to Phenotype******************
t1x := BezierChromosome[chromosome_NUM] & %0000_0000_0000_1111
t1y := (BezierChromosome[chromosome_NUM] & %0000_0000_1111_0000) >> 4
t2x := (BezierChromosome[chromosome_NUM] & %0000_1111_0000_0000) >> 8
t2y := (BezierChromosome[chromosome_NUM] & %1111_0000_0000_0000) >> 12
'***********Get Generation i Convergence 1of2***********
IF BezierChromosome[chromosome_Num] <> 0
Convergence[chromosome_Num] := countBits((BezierChromosome[chromosome_Num] & BezierChromosome[chromosome_Num - 1]))
Convergence[chromosome_Num] += countBits((!BezierChromosome[chromosome_Num] & !BezierChromosome[chromosome_Num - 1]))
ELSE ' chromosome_Num zero case
Convergence[chromosome_Num] := countBits((BezierChromosome[chromosome_Num] & BezierChromosome[chromosome_Num + 1]))
Convergence[chromosome_Num] += countBits((!BezierChromosome[chromosome_Num] & !BezierChromosome[chromosome_Num + 1]))
'**************************Fitness Function****************************
pat := fitnessBezier(robot_x, robot_y, t1x, t1y, t2x, t2y, enemy_x, enemy_y, 8)
fitness := (||(theta - pat))' - (lenght)
IF fitness < parentF_1 'Remember the 2 most fit chromosomes
parentN_1 := chromosome_Num
parentF_1 := fitness
NEXT
IF fitness =< parentF_2
parentN_2 := chromosome_Num
parentF_2 := fitness
NEXT 'Remember the 2 most unfit chromosomes
IF fitness > unfitF_1
unfitN_1 := chromosome_Num
unfitF_1 := fitness
NEXT
IF fitness > unfitF_2
unfitN_2 := chromosome_Num
unfitF_2 := fitness
NEXT
''/\/\/\/\/\/\/\/\/\/\/\/\/\/\/ RECOMBINATION /\/\/\/\/\/\/\/\/\/\/\/\/\/\/
crossoverMask := 0
REPEAT randomRange(14,1) 'get the random crossover point, 15 and 0 are excluded to avoid cloning
crossoverMask := crossoverMask << 1 + 1 'generate the crossover mask
BezierChromosome[unfitN_1] := ((BezierChromosome[parentN_2] & crossoverMask)+(BezierChromosome[parentN_1] & !crossoverMask))
BezierChromosome[unfitN_2] := ((BezierChromosome[parentN_1] & crossoverMask)+(BezierChromosome[parentN_2] & !crossoverMask))
''/\/\/\/\/\/\/\/\/\/\/\/\/\/\/ MUTATION /\/\/\/\/\/\/\/\/\/\/\/\/\/\/
'************Single Alele Mutation***********
REPEAT _MutationRate
RESULT := randomRange(_Population-1,0)
IF RESULT <> parentN_1 AND RESULT <> parentN_2 'Eletism keeps the best genes intact and free of mutation.
ToggleBitByte(@BezierChromosome[RESULT], randomRange(15,0))
'***********Get Generation i Convergence 2of2***********
d := 0
REPEAT _Population-1 'sum the realitive similarity (0..8) of each adajacent chromosome in the population
GC += Convergence[d++]
GC := GC * 10
GC := GC / (_Population) 'divide the final sum by the chromosome populatin to get average
'***********Test Convergence ***********
IF GC => _Convergence'45 ' I ave found too much confidence is not nessisary on a 16x16 map
pst.clear
IF fitness >= _okErrorLevel '10 'If the GA converged on a solution with too much error start again
quit
'****************Genotype to Phenotype******************
t1x := BezierChromosome[parentN_1] & %0000_0000_0000_1111
t1y := (BezierChromosome[parentN_1] & %0000_0000_1111_0000) >> 4
t2x := (BezierChromosome[parentN_1] & %0000_1111_0000_0000) >> 8
t2y := (BezierChromosome[parentN_1] & %1111_0000_0000_0000) >> 12
'---debugging---------------------------
pathBezier(robot_x, robot_y, t1x, t1y, t2x, t2y, enemy_x, enemy_y, 8)
repeat 20
PST.str(STRING(PST#NL))
PST.Str(string("Converged on Pop./Gen. "))
PST.dec(POP)
PST.Str(string("/"))
PST.dec(i)
PST.str(STRING(PST#NL))
PST.str(STRING("Optimum approach: "))
PST.Dec(theta)
PST.str(STRING("^d"))
PST.str(STRING(PST#NL))
PST.str(STRING("Evolved approach: "))
PST.Dec(pat)
PST.str(STRING("^d"))
PST.str(STRING(PST#NL, PST#NL))
PST.Str(string("Any key: Enemy(?x,?y) Run GA"))
PST.Position(enemy_x*4+25, enemy_y*4)
PST.char("E")
'--/debugging---------------------------
'Build the path bezier using the best control points
pathBezier(robot_x+25, robot_y, t1x*4+25, t1y*4, t2x*4+25, t2y*4, enemy_x*4+25, enemy_y*4, 20) 'scaled funny to debug better
RETURN
DAT ''BIT MANIPULATION ROUTINES
'forget who wrote these, would like to credit
PUB ToggleBitByte(variableAddr,index)
IF (getBitbyte(variableAddr,index) == 0)
setBitbyte(variableAddr,index)
ELSE
clrBitbyte(variableAddr,index)
PUB GetBitByte(variableAddr,index) | localcopy
localcopy := BYTE[variableAddr]
RETURN ((localcopy & (1<<index)) >> index)
PUB SetBitByte(variableAddr,index)
BYTE[variableAddr] := BYTE[variableAddr] | (1<<index)
PUB ClrBitByte(variableAddr,index)
BYTE[variableAddr] := BYTE[variableAddr] & (!(1<<index))
PUB countBits(variableAddr) : count
''Elegant snippet from Parallax Fourm User Mike Green. Thanks!
REPEAT 8
count += variableAddr & 1
variableAddr := variableAddr >> 1
DAT '' MATH ROUTINES
PUB vectorToDegree(Vx1,Vy1,Vx2,Vy2) : theta | qX, qY, i, d
Vx1 := Q.Qval(Vx1)
Vy1 := Q.Qval(Vy1)
Vx2 := Q.Qval(Vx2)
Vy2 := Q.Qval(Vy2)
qX := Vx2-Vx1
qY := Vy2-Vy1
theta := Q.Deg_ATAN2(qX,qY)
theta := qStrToInt(Q.QvalToStr(theta))
PUB qStrToInt(qAddr) : int |i, d, n, r
'' Q string to Int. Maybe works for float strings too. this needs work. Returns rounded interger
'max 999
n := 1
r := i := 0
REPEAT STRSIZE(qAddr)
qStr[i] := BYTE[qAddr++]
IF qStr[i] == "."
IF qStr[i+1] >= 5
r := 1
QUIT
i++
IF qStr[0] == "-"
BYTEMOVE(@qStr[0],@qStr[1],i)
n := -1
i--
CASE i
3: int += qStr[2]-48+r
int += (qStr[1]-48)*10
int += (qStr[0]-48)*100
2: int := qStr[1]-48+r
int += (qStr[0]-48)*10
1: int := qStr[0]-48+r
int := int*n
PUB randomRange(maxValue, minValue)
'' Return a psudo-random value between maxValue and minValue (inclusive).
'forget who wrote this, would like to credit.
IF (minValue > maxValue)
RETURN false
IF (minValue == maxValue)
RETURN maxValue
RETURN (?cnt >> 1 // ((maxValue - minValue) + 1)) + minValue
The next next trick will be to use another GA to optimize the parameters for the path GA including population, mutation rate and convergence. This will take a long long time to run.
Maybe my approach is too convoluted. In my research I read that GA's are best at solving non-deterministic polynomial-time hard problems; which I'm not sure this is. Is there a way to achieve these results w/ pure geometry?
Thanks for any help.
Since the sumo robot ring is circular the best angle to hit the opponent is the angle between the enemy robot and the center of the circle. Along this trajectory the enemy robot is closest to the edge. Right now the included code evolves a quadratic bezier curve that starts on our robot, ends on the enemy robot and has 2 control points so that it intersects with the enemy robot at the optimum angle.
The genetic algorithm (GA) approximates the final high res bezier-path in a 16x16 world. It generates an answer within a definable range of optimum. This can take 1-10 generations. If the GA converges on a solution with too much error there is an extinction and a new population gives it a shot. Because the search space is only 16x16 there are robot/enemy x,y configurations that are more difficult. Also the way in which the initial random population is generated greatly impacts the convergence time. I have not verified this but it seems that slightly 'less' random values work better, (less evenly distributed?)
The next trick will be including the beziers length in the fitness function. Seems simple enough but I'm unsure how to weight the angle vs length of the line properly. It would be great to evolve beziers that allow the robot to accelerate to a velocity in relation to the opponents distance from the edge. Or even the longest bezier so that the bot can reach top speed while on the path. Does any one have any advice/ideas on how I could go about this? fuzzy degree of membership stuff maybe?
Set the Parallax serial terminal font size to 6 in the prefs if you run this.
CON
'***********Processor Settings**********
_clkfreq = 80_000_000
_clkmode = xtal1 + pll16x
'***************************************
_maxPoints = 64
_F1 = 1.0
_F2 = 2.0
_F3 = 3.0
'GA constants
_Convergence = 45 'this is how self-similar a population msut be to have 'converged' on a solution
_Population = 22 'initial population, larger populations have more genetic potential but converge slower
_MutationRate = 1 'number loops where a gene other than a parent has a 50/50 of mutating by 1 bit. Bigger numbers slow convergence or make it impossible
_okErrorLevel = 10 'GA output must be within '_okErrorLevel' degrees of optimum
OBJ
Q : "SPIN_TrigPack"
F32 : "F32"
FPS : "FloatString"
PST : "Parallax Serial Terminal"
VAR
'Bezier path vars
LONG fBx[_maxPoints], fBy[_maxPoints] 'bezier float. point arrays
LONG pathAngl[_maxPoints], pathDist[_maxPoints] 'bezier line to polar path arrays
'GA vars
LONG BezierChromosome[_Population]
LONG Convergence[_Population]
LONG lenght
'Global vars
BYTE robot_x, robot_y, enemy_x, enemy_y
LONG pathlenght
BYTE qStr[32]
PUB Start
PST.Start(115200)
F32.start
Q.Start_Driver
robot_x := 4
robot_y := 5
enemy_x := 20
enemy_y := 20
repeat
PST.clear
PST.Position(robot_x, robot_y)
PST.char("R")
PST.Position(enemy_x, enemy_y)
PST.char("E")
PST.Position(8, 8)
PST.char("+")'center point
geneticBezier
PST.Position(robot_x, robot_y)
PST.char("R")
PST.Position(enemy_x, enemy_y)
PST.char("E")
PST.Position(8, 8)
PST.char("+")'center point
PST.CharIn
enemy_x := randomRange(15,0)
enemy_Y := randomRange(15,0)
PUB fitnessBezier(P1x, P1y, P2x, P2y, P3x, P3y, P4x, P4y, Resolution) : theta | i, n, t, a, b, c, d, x, y
'P1 - Start point, P2 - Control point 1, P3 - Control point 2, P4 - End Point
'Only derives the final point of the bezier, returns the angle between the last bezier point and P4x, P4y
i := Resolution-2
n := F32.FFloat(Resolution-1)
P1x := F32.FFloat(P1x)
P1y := F32.FFloat(P1y)
P2x := F32.FFloat(P2x)
P2y := F32.FFloat(P2y)
P3x := F32.FFloat(P3x)
P3y := F32.FFloat(P3y)
P4x := F32.FFloat(P4x)
P4y := F32.FFloat(P4y)
'********* Terms ********
t := F32.FDiv(F32.FFloat(i),n)
a := F32.Pow(F32.FSub(_F1, t), _F3)
b := F32.FMul(F32.FMul(_F3,t),F32.Pow(F32.FSub(_F1, t), _F2))
c := F32.FMul(F32.FMul(_F3, F32.Pow(t, _F2)), F32.FSub(_F1, t))
d := F32.Pow(t, _F3)
'********* Bezier Point Components ********
x := F32.FAdd(F32.FAdd(F32.FAdd(F32.FMul(a, P1x), F32.FMul(b, P2x)),F32.FMul(c, P3x)), F32.FMul(d, P4x))
y := F32.FAdd(F32.FAdd(F32.FAdd(F32.FMul(a, P1y), F32.FMul(b, P2y)),F32.FMul(c, P3y)), F32.FMul(d, P4y))
PST.Position(F32.FRound(x), F32.FRound(y))
PST.Dec(I)
theta := vectorToDegree(F32.FRound(x), F32.FRound(y), F32.FRound(p4x), F32.FRound(p4y))
PUB pathBezier(P1x, P1y, P2x, P2y, P3x, P3y, P4x, P4y, Resolution) : l | i, n, t, a, b, c, d, mx, my
'P1 - Start point, P2 - Control point 1, P3 - Control point 2, P4 - End Point
i := 1
n := F32.FFloat(Resolution--)
P1x := F32.FFloat(P1x)
P1y := F32.FFloat(P1y)
P2x := F32.FFloat(P2x)
P2y := F32.FFloat(P2y)
P3x := F32.FFloat(P3x)
P3y := F32.FFloat(P3y)
P4x := F32.FFloat(P4x)
P4y := F32.FFloat(P4y)
REPEAT Resolution
'********* Terms ********
t := F32.FDiv(F32.FFloat(i),n)
a := F32.Pow(F32.FSub(_F1, t), _F3)
b := F32.FMul(F32.FMul(_F3,t),F32.Pow(F32.FSub(_F1, t), _F2))
c := F32.FMul(F32.FMul(_F3, F32.Pow(t, _F2)), F32.FSub(_F1, t))
d := F32.Pow(t, _F3)
'********* Bezier Point Components ********
fBx[i] := F32.FAdd(F32.FAdd(F32.FAdd(F32.FMul(a, P1x), F32.FMul(b, P2x)),F32.FMul(c, P3x)), F32.FMul(d, P4x))
fBy[i] := F32.FAdd(F32.FAdd(F32.FAdd(F32.FMul(a, P1y), F32.FMul(b, P2y)),F32.FMul(c, P3y)), F32.FMul(d, P4y))
'tabulate lenght
'mx := F32.FSub(fBx[i], fBx[i-1])
'my := F32.FSub(fBy[i], fBy[i-1])
'l := F32.FAdd(F32.FSqr(F32.FAdd(F32.FMul(mx, mx), F32.FMul(my, my))), l)
'Generate Polar path
'pathAngl[i] := vectorToDegree(F32.FRound(fBx[i]), F32.FRound(fBy[i]), F32.FRound(fBy[i-1]), F32.FRound(fBy[i-1]))
'pathDist[i] := F32.FRound(F32.FSqr(F32.FAdd(F32.FMul(mx, mx), F32.FMul(my, my))))
PST.Position(F32.FRound(fBx[i]), F32.FRound(fBy[i]))
PST.Dec(i)
i++
pathlenght := F32.FRound(l) '
l := vectorToDegree(F32.FRound(fBx[Resolution-1]), F32.FRound(fBy[Resolution-1]), F32.FRound(p4x), F32.FRound(p4y))
PUB geneticBezier| POP, pat, theta, i, d, GC, chromosome_NUM, genotype, t1x, t1y, t2x, t2y, parentN_1, parentN_2, unfitN_1, unfitN_2, unfitN_3, parentF_1, parentF_2, unfitF_1, unfitF_2, unfitF_3, fitness, crossoverMask
POP := 0
REPEAT
POP += 1
chromosome_NUM := 0
LONGFILL(@BezierChromosome[0], 0, _Population-1)
''/\/\/\/\/\/\/\/\/\/\/\/\/\/\/GENERATE INITIAL RANDOM POPULATION\/\/\/\/\/\/\/\/\/\/\/\/\/\/\
REPEAT UNTIL chromosome_NUM == _Population-1
genotype := ?genotype * ?cnt
IF LOOKDOWN(genotype.BYTE[0] & %0000_1111 : robot_x, enemy_x) == 0 AND LOOKDOWN((genotype.BYTE[0] & %1111_0000) >> 4 : robot_y, enemy_y) == 0
IF LOOKDOWN(genotype.BYTE[1] & %0000_1111 : robot_x, enemy_x) == 0 AND LOOKDOWN((genotype.BYTE[1] & %1111_0000) >> 4 : robot_y, enemy_y) == 0
BezierChromosome[++chromosome_NUM] := genotype
BezierChromosome[8] := %0000_0000_0000_0000_1000_1000_1000_1000
theta := vectorToDegree(8, 8, enemy_x, enemy_y) 'optimum fittness requirement - put this or refrence to global var in world loop later
''/\/\/\/\/\/\/\/\/\/\/\/\/\/\/EVOLUTIONARY WORLD LOOP\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\
REPEAT i FROM 1 TO 10000 'i number of generations
parentF_1 := parentF_2 := 100 'initialize the parents, the 2 chromosomes with the lowest fittnes # number will be made parents
unfitF_1 := unfitF_2 := unfitF_3 := 0 'initialize the scum, the 2 chromosomes with the highest fittnes # will be killed off
GC := 0
IF i == 10
quit
'***************************Artificial Selection******************************
REPEAT chromosome_Num FROM 1 TO _Population - 1 'loop to evaluate each chromosome in the population
fitness := 0 'initialize new fitness number
'****************Genotype to Phenotype******************
t1x := BezierChromosome[chromosome_NUM] & %0000_0000_0000_1111
t1y := (BezierChromosome[chromosome_NUM] & %0000_0000_1111_0000) >> 4
t2x := (BezierChromosome[chromosome_NUM] & %0000_1111_0000_0000) >> 8
t2y := (BezierChromosome[chromosome_NUM] & %1111_0000_0000_0000) >> 12
'***********Get Generation i Convergence 1of2***********
IF BezierChromosome[chromosome_Num] <> 0
Convergence[chromosome_Num] := countBits((BezierChromosome[chromosome_Num] & BezierChromosome[chromosome_Num - 1]))
Convergence[chromosome_Num] += countBits((!BezierChromosome[chromosome_Num] & !BezierChromosome[chromosome_Num - 1]))
ELSE ' chromosome_Num zero case
Convergence[chromosome_Num] := countBits((BezierChromosome[chromosome_Num] & BezierChromosome[chromosome_Num + 1]))
Convergence[chromosome_Num] += countBits((!BezierChromosome[chromosome_Num] & !BezierChromosome[chromosome_Num + 1]))
'**************************Fitness Function****************************
pat := fitnessBezier(robot_x, robot_y, t1x, t1y, t2x, t2y, enemy_x, enemy_y, 8)
fitness := (||(theta - pat))' - (lenght)
IF fitness < parentF_1 'Remember the 2 most fit chromosomes
parentN_1 := chromosome_Num
parentF_1 := fitness
NEXT
IF fitness =< parentF_2
parentN_2 := chromosome_Num
parentF_2 := fitness
NEXT 'Remember the 2 most unfit chromosomes
IF fitness > unfitF_1
unfitN_1 := chromosome_Num
unfitF_1 := fitness
NEXT
IF fitness > unfitF_2
unfitN_2 := chromosome_Num
unfitF_2 := fitness
NEXT
''/\/\/\/\/\/\/\/\/\/\/\/\/\/\/ RECOMBINATION /\/\/\/\/\/\/\/\/\/\/\/\/\/\/
crossoverMask := 0
REPEAT randomRange(14,1) 'get the random crossover point, 15 and 0 are excluded to avoid cloning
crossoverMask := crossoverMask << 1 + 1 'generate the crossover mask
BezierChromosome[unfitN_1] := ((BezierChromosome[parentN_2] & crossoverMask)+(BezierChromosome[parentN_1] & !crossoverMask))
BezierChromosome[unfitN_2] := ((BezierChromosome[parentN_1] & crossoverMask)+(BezierChromosome[parentN_2] & !crossoverMask))
''/\/\/\/\/\/\/\/\/\/\/\/\/\/\/ MUTATION /\/\/\/\/\/\/\/\/\/\/\/\/\/\/
'************Single Alele Mutation***********
REPEAT _MutationRate
RESULT := randomRange(_Population-1,0)
IF RESULT <> parentN_1 AND RESULT <> parentN_2 'Eletism keeps the best genes intact and free of mutation.
ToggleBitByte(@BezierChromosome[RESULT], randomRange(15,0))
'***********Get Generation i Convergence 2of2***********
d := 0
REPEAT _Population-1 'sum the realitive similarity (0..8) of each adajacent chromosome in the population
GC += Convergence[d++]
GC := GC * 10
GC := GC / (_Population) 'divide the final sum by the chromosome populatin to get average
'***********Test Convergence ***********
IF GC => _Convergence'45 ' I ave found too much confidence is not nessisary on a 16x16 map
pst.clear
IF fitness >= _okErrorLevel '10 'If the GA converged on a solution with too much error start again
quit
'****************Genotype to Phenotype******************
t1x := BezierChromosome[parentN_1] & %0000_0000_0000_1111
t1y := (BezierChromosome[parentN_1] & %0000_0000_1111_0000) >> 4
t2x := (BezierChromosome[parentN_1] & %0000_1111_0000_0000) >> 8
t2y := (BezierChromosome[parentN_1] & %1111_0000_0000_0000) >> 12
'---debugging---------------------------
pathBezier(robot_x, robot_y, t1x, t1y, t2x, t2y, enemy_x, enemy_y, 8)
repeat 20
PST.str(STRING(PST#NL))
PST.Str(string("Converged on Pop./Gen. "))
PST.dec(POP)
PST.Str(string("/"))
PST.dec(i)
PST.str(STRING(PST#NL))
PST.str(STRING("Optimum approach: "))
PST.Dec(theta)
PST.str(STRING("^d"))
PST.str(STRING(PST#NL))
PST.str(STRING("Evolved approach: "))
PST.Dec(pat)
PST.str(STRING("^d"))
PST.str(STRING(PST#NL, PST#NL))
PST.Str(string("Any key: Enemy(?x,?y) Run GA"))
PST.Position(enemy_x*4+25, enemy_y*4)
PST.char("E")
'--/debugging---------------------------
'Build the path bezier using the best control points
pathBezier(robot_x+25, robot_y, t1x*4+25, t1y*4, t2x*4+25, t2y*4, enemy_x*4+25, enemy_y*4, 20) 'scaled funny to debug better
RETURN
DAT ''BIT MANIPULATION ROUTINES
'forget who wrote these, would like to credit
PUB ToggleBitByte(variableAddr,index)
IF (getBitbyte(variableAddr,index) == 0)
setBitbyte(variableAddr,index)
ELSE
clrBitbyte(variableAddr,index)
PUB GetBitByte(variableAddr,index) | localcopy
localcopy := BYTE[variableAddr]
RETURN ((localcopy & (1<<index)) >> index)
PUB SetBitByte(variableAddr,index)
BYTE[variableAddr] := BYTE[variableAddr] | (1<<index)
PUB ClrBitByte(variableAddr,index)
BYTE[variableAddr] := BYTE[variableAddr] & (!(1<<index))
PUB countBits(variableAddr) : count
''Elegant snippet from Parallax Fourm User Mike Green. Thanks!
REPEAT 8
count += variableAddr & 1
variableAddr := variableAddr >> 1
DAT '' MATH ROUTINES
PUB vectorToDegree(Vx1,Vy1,Vx2,Vy2) : theta | qX, qY, i, d
Vx1 := Q.Qval(Vx1)
Vy1 := Q.Qval(Vy1)
Vx2 := Q.Qval(Vx2)
Vy2 := Q.Qval(Vy2)
qX := Vx2-Vx1
qY := Vy2-Vy1
theta := Q.Deg_ATAN2(qX,qY)
theta := qStrToInt(Q.QvalToStr(theta))
PUB qStrToInt(qAddr) : int |i, d, n, r
'' Q string to Int. Maybe works for float strings too. this needs work. Returns rounded interger
'max 999
n := 1
r := i := 0
REPEAT STRSIZE(qAddr)
qStr[i] := BYTE[qAddr++]
IF qStr[i] == "."
IF qStr[i+1] >= 5
r := 1
QUIT
i++
IF qStr[0] == "-"
BYTEMOVE(@qStr[0],@qStr[1],i)
n := -1
i--
CASE i
3: int += qStr[2]-48+r
int += (qStr[1]-48)*10
int += (qStr[0]-48)*100
2: int := qStr[1]-48+r
int += (qStr[0]-48)*10
1: int := qStr[0]-48+r
int := int*n
PUB randomRange(maxValue, minValue)
'' Return a psudo-random value between maxValue and minValue (inclusive).
'forget who wrote this, would like to credit.
IF (minValue > maxValue)
RETURN false
IF (minValue == maxValue)
RETURN maxValue
RETURN (?cnt >> 1 // ((maxValue - minValue) + 1)) + minValue
The next next trick will be to use another GA to optimize the parameters for the path GA including population, mutation rate and convergence. This will take a long long time to run.
Maybe my approach is too convoluted. In my research I read that GA's are best at solving non-deterministic polynomial-time hard problems; which I'm not sure this is. Is there a way to achieve these results w/ pure geometry?
Thanks for any help.