Quadratic Bezier Curves + Genetic Algorithm. w/ Code.
UltraLazer
Posts: 30
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.
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.
Comments
On a side note: Do you know what type of sensors are most commonly used for robot detection? I was planing to have my robot hiding most of the time in the safest parts of the map, always facing the opponent with a surface engineered to absorb sonar and IR. I wondered about a flag that could extend out from the side of the robot like in a bull fight. The flag would be extra 'bright'.
I'm not sure how to hide from a vision based robot... maybe some really powerful lights... If all of these fail the plan is to run away quickly. Wait for the buzzer and don't get smashed.
'**************************Fitness Function**************************** pat := pathBezier(robot_x, robot_y, t1x, t1y, t2x, t2y, enemy_x, enemy_y, 8) 'evaluate the angle of the phenotype fitness := (||(theta - pat)) ' - (lenght*someScale) ' get the difference between the optimum angle and the phenotype angleThe smaller the number the better the fitness. If the (lenght * someScale) is subtracted the GA won't converge. I'm hoping to avoid floats in here... Any ideas on a good way to weight the lenght vs. angle values?
Can anyone recommend a good and friendly comp-sci forum where I might try?
Also, have you seen the sumo bots from japan? lightning fast 4 wheelers
that use long straight lines for ramming speed.
* I'm not sure what type of sensors most people are using.
* I do have to agree that omniwheels may not be the best solution since traction will be a big decider in the match.
* The bright flag idea is a pretty good one. That could work well.
* Related to the flag idea, I saw a pretty interesting robot once that started out folded up. When the match started, it would race over to the edge and unfold. The inside of the robot was the same color as the ring's floor and the robot itself was very short. This essentially made a ramp that the other robot could drive over. If the other robot went in search of this robot, it would drive over the robot and fall off the edge because it never saw a white line.
* The Japanese robots are *extremely* fast. The matches are usually over within two or three seconds. Make sure that your robot is very fast as well if you want to avoid them.