Genetic Algorithm Implementation for Self Balancing Robot

Question

I have made a Self Balancing Robot in Webots that learns to balance itself with the help of a Genetic Algorithm. The setup works well enough but I'm not really sure how much of a good Genetic Algorithm it is and because I did this on my own I'm not feeling sure about this. Also note some terms I use here may not be the standard ones, please do let me know if I'm using any terms incorrectly.

Program Flow

(1)Creating Population

I am using an LQR controller to balance the Robot. It has four parameters and hence each individual in my population is a list with four randomly generated parameters. However to reduce the domain of the search space I have added some bounds to the parameters, which I got a rough Idea about by trying to balance the robot manually.

Code

def population_create(p_size,geno_size,bounds):
    
    p = [ random_param(geno_size,bounds) for i in range(p_size)]
    return p
    
def random_param(g,b):
    return [random.uniform(b[i][0],b[i][1]) for i in range(g)]

(2)Send Parameters To Robot, Evaluate Fitness

I then pass these random parameters to the robot and see how well it balances for around 60 seconds. The fitness is calculated by summing up the angle of the robot and measuring the displacement of the robot and its load from the initial position.

def getPerformanceData():
    global init_translation,init_rotation,load_init_translation,load_init_rotation
    emitter.send("return_fitness".encode('utf-8'))
    while superv.step(timestep) != -1: 
 
        if reciever.getQueueLength()>0:
            message = reciever.getData().decode('utf-8')
            reciever.nextPacket()
            angle_fitness = float(message)
 
            load_translation = load.getField("translation").getSFVec3f()
            load_rotation = load.getField("rotation").getSFRotation()
            load_t_cost = sum([(i1-i2)**2 for i1,i2 in zip(load_translation,load_init_translation)])
            load_r_cost = sum([(i1-i2)**2 for i1,i2 in zip(load_rotation,load_init_rotation)])
            
 
            sbr_translation = sbr.getField("translation").getSFVec3f()
            sbr_rotation = sbr.getField("rotation").getSFRotation()
            sbr_t_cost = sum([(i1-i2)**2 for i1,i2 in zip(sbr_translation,init_translation)])
            sbr_r_cost = sum([(i1-i2)**2 for i1,i2 in zip(sbr_rotation,init_rotation)])
            #print("Angle Fitness - ",angle_fitness)
            #print("Load Fitness - ",(load_r_cost+load_t_cost))
            #print("Robot T Fitness ",(sbr_r_cost+sbr_t_cost))
            return angle_fitness+((load_r_cost+load_t_cost)+(sbr_r_cost+sbr_t_cost))*30
   


def evaluate_genotype(genotype):
    #test_genotype = [6.70891752445785, -2.984975676757869, 148.50048150101875, 655.0303108723926]
    # send genotype to robot
    send_genotype(genotype)
    
    # run for some time
    run_seconds(60)
    #store fitness
    fitness = getPerformanceData()
    #print("Supervisor:Fitness of ",genotype," - %f "%(fitness))
   
    sbr.resetPhysics()
    restore_robot_position()

    # Restore Robots Position
    run_seconds(5,True)
    
    sbr.resetPhysics()
    restore_robot_position()
    
    # reset physics
    return fitness

 

(3) Crossover, Mutate and Create New Population

Then I rank the parameters according to the fitness. The good ones move unaltered to the next generation. Then I implement crossover and mutation for the remaining individuals. Mutation is done so that the parameters don't escape their own domain.

def population_reproduce(p,fitness):

    size_p = len(p)
    new_p = []
    
    dataframe = pd.DataFrame({"Param":p,"Fitness":fitness})
    dataframe = dataframe.sort_values(['Fitness'])
    dataframe = dataframe.reset_index(drop=True)    

    sorted_p = dataframe['Param'].tolist()

    elite_part = round(ELITE_PART*size_p)
    new_p = new_p + sorted_p[:elite_part]

    for i in range(size_p-elite_part):
        mom = p[random.randint(0,size_p-1)]
        dad = p[random.randint(0,size_p-1)]
        child = crossover(mom,dad)
        child = mutate(child)
        new_p.append(child)
    
    return new_p

def crossover(p1,p2):
    
    crossover = []
    locii = [random.randint(0,8) for _ in range(len(p1))]
    
    for i in range(len(p1)):
        if locii[i]>4:
            crossover.append(p2[i])
        else:
            crossover.append(p1[i])
        
    return crossover

def mutate(c):
    size = len(c)
    for i in range(size):
        if random.random()< MUTATION_PROBABILITY:
            if i==0:
                c[1] += random.gauss(0,2)
            elif i==1:
                c[2] += random.gauss(0,1)
            else:
                c[i] += random.gauss(0,2)*10
            
    return c   
    

(4) Repeat Step (2) and (3) For a predefined number of times

These are some outputs I got.

Generation 1 
Best Fitness  [6.438820290364836, -2.6048039057927954,117.55012307994608, 630.0038471538783] 
Best Fitness Value - 0.934142 
Average Fitness - 3943375.740127

Generation 2 Robot: 
Best Fitness  [6.091447760688381, -3.012376769204556,140.9627885511517, 639.3500316914635] 
Best Fitness Value - 69.073272 
Average Fitness - 1858776.135231

Generation 3 Best Fitness  [6.091447760688381, -3.012376769204556,163.4547194017456, 569.1154953081709] 
Best Fitness Value - 5.653302
Average Fitness - 1248849.999916

Generation 4 Best Fitness  [4.731672906621537, -3.088996850105585,150.1271801370339, 537.8098244643127] 
Best Fitness Value - 505.242039 
Average Fitness - 1454077.404463

Generation 5 Best Fitness  [4.731672906621537, -3.088996850105585,129.0944277341561, 536.0589859999795] 
Best Fitness Value - 22.842133 
Average Fitness - 1037094.178346

Each time I train this I get a solution that works.

I would like to know

1. Is this a correct implementation of a genetic algorithm?
2. In which ways could I improve this implementation?

Below are the links to the full scripts and also a video of how this works if you are interested.

Population Script

Supervisor

Robot Controller

Video