PDA

View Full Version : Quadratic Bezier Curves + Genetic Algorithm. w/ Code.



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.

Jack Buffington
11-15-2011, 05:09 PM
Consider making your robot as heavy as possible to give yourself as much inertia as possible. The more advanced robots use vacuum systems to stick themselves to the floor with quite a lot of force.

UltraLazer
11-15-2011, 06:19 PM
Very interesting Jack, good idea. Do they multiplex the vacuum / motors? The weight might help with the omni wheels. They are quite smooth. This is even more reason to have the length of the path in the fitness function to make sure there is enough time to accelerate the mass.

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 angle


The 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?

UltraLazer
11-16-2011, 02:49 PM
Am I asking the wrong kind of question here? I realize this is not a spin specific problem but was hoping the community would be able suggest an approach... Maybe in trade for the included code or function that derives the bezier curve points. Wrote most of this based on descriptions of the different aspects of genetic algorithms but I have no formal computer theory and so I'm stuck here.

Can anyone recommend a good and friendly comp-sci forum where I might try?

Capt. Quirk
11-16-2011, 03:46 PM
I would like to suggest you add sumo bot to your title.

Also, have you seen the sumo bots from japan? lightning fast 4 wheelers
that use long straight lines for ramming speed.

erco
11-16-2011, 03:55 PM
I gotta go with Jack & CapQ on this one. You can't fight physics. Momentum, speed and TRACTION will most likely trump the delicate elegance of holonomic wheels. Omniwheels are graceful neat, but those little rollers on the outside can never generate the same traction as big beefy tires, which can also double as shock absorbers.

Jack Buffington
11-16-2011, 04:55 PM
Random thoughts:
* 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.