def _Test_Draw():
T = 1000
quad = Quadruped()
sim = PYROSIM(playPaused=False, playBlind=True, evalTime=T)
quad.Send_To_Simulator(sim)
sim.Start()
sim.Wait_To_Finish()
data = sim.Get_Results()
print data[4, 1, T - 1]
sim2 = PYROSIM(playPaused=False, playBlind=False, evalTime=T)
quad.Send_To_Simulator(sim2)
sim2.Start()
sim2.Wait_To_Finish()
data = sim2.Get_Results()
print data[4, 1, T - 1]
mutant = quad.Copy_And_Mutate(variable=.99, sigma=5)
sim3 = PYROSIM(playPaused=False, playBlind=False, evalTime=T)
IDs = quad.Send_To_Simulator(sim3)
print IDs
IDs = mutant.Send_To_Simulator(sim3,
objID=IDs[0],
jointID=IDs[1],
sensorID=IDs[2],
neuronID=IDs[3])
sim3.Start()
sim3.Wait_To_Finish()
data = sim3.Get_Results()
print data[4, 1, T - 1], data[9, 1, T - 1]
class INDIVIDUAL:
def __init__(self, i):
self.ID = i
self.genome = numpy.random.random([4, 8]) * 2 - 1
self.fitness = 0
def Start_Evaluation(self, pp, pb):
self.sim = PYROSIM(playPaused=pp, playBlind=pb, evalTime=500)
robot = ROBOT(self.sim, self.genome)
self.sim.Start()
def Compute_Fitness(self):
self.sim.Wait_To_Finish()
sensorData = self.sim.Get_Sensor_Data(sensorID=4, s=1)
self.fitness = sensorData[499]
if (self.fitness > 10.0):
self.fitness = 10.0
del self.sim
def Mutate(self):
s = random.randint(0, 3)
m = random.randint(0, 7)
self.genome[s, m] = random.gauss(self.genome[s, m],
math.fabs(self.genome[s, m]))
if (self.genome[s, m] > 1.0):
self.genome[s, m] = 1.0
if (self.genome[s, m] < -1.0):
self.genome[s, m] = -1.0
def Print(self):
print '[',
print self.ID,
print self.fitness,
print '] ',
def _Test_Tree():
T = 1000
sim = PYROSIM(playPaused=False,
playBlind=False,
evalTime=T,
xyz=[0, 0, 5],
hpr=[90, -90, 0])
#sim = PYROSIM(playPaused=False, playBlind=False, evalTime=T, xyz=[-5,0,2], hpr=[0,-20,0])
t = Treebot()
t.Send_To_Simulator(sim)
sim.Start()
sim.Wait_To_Finish()
data = sim.Get_Results()
class SIMULATION:
def __init__(self):
self.sim = PYROSIM(playPaused=False)
self.arena = ARENA(self.sim)
xPosition = random.random()*(c.endX-c.startX) + c.startX
yPosition = random.random()*(c.endY-c.startY) + c.startY
self.robot = ROBOT(self.sim, x = xPosition , y = yPosition , theta = 0.0 )
self.sim.Start()
def Play_Indv(robot, eval_time, environment=False):
sensor_data = {}
if environment:
for i, env in enumerate(environment[:8]):
sim = PYROSIM(playPaused=True,
playBlind=False,
evalTime=eval_time,
xyz=(0, -2, 3),
hpr=(90, -30, 0))
offset = robot.Send_To_Simulator(sim, eval_time=eval_time)
env.Send_To_Simulator(sim, ID_offset=offset[0])
sim.Start()
sim.Wait_To_Finish()
sensor_data[i] = sim.Get_Results()
print sensor_data
return sensor_data
def Evaluate(self, hideSim=True, startPaused=False):
sim = PYROSIM(playPaused=startPaused,
playBlind=hideSim,
evalTime=c.evaluationTime)
robot = ROBOT(sim, self.genome)
sim.Start()
sim.Wait_To_Finish()
x = sim.Get_Sensor_Data(sensorID=4, s=0)
y = sim.Get_Sensor_Data(sensorID=4, s=1)
z = sim.Get_Sensor_Data(sensorID=4, s=2)
# self.sensorData_0 = sim.Get_Sensor_Data(sensorID=0)
# self.sensorData_1 = sim.Get_Sensor_Data(sensorID=1)
# self.sensorData_2 = sim.Get_Sensor_Data(sensorID=2)
# self.sensorData_3 = sim.Get_Sensor_Data(sensorID=3)
# self.sensorData_4 = sim.Get_Sensor_Data(sensorID=4)
# print(sim.Get_Sensor_Data(sensorID=4))
self.fitness = y[-1]
del sim
from pyrosim import PYROSIM import matplotlib.pyplot as plt sim = PYROSIM(playPaused=True , evalTime=100) sim.Send_Cylinder( objectID=0, x=0 , y=0 , z=.6 , length=1.0 , radius=0.1 ) sim.Send_Cylinder( objectID=1 , x=0 , y=.5 , z=1.1 , r1=0 , r2=1, r3=0 , length=1.0 , radius=0.1 , r=1, g=0, b=0) sim.Send_Joint( jointID = 0 , firstObjectID = 0 , secondObjectID = 1, x=0 , y=0 , z=1.1, n1 = -1 , n2 = 0 , n3 = 0, lo=-3.14159/2 , hi=3.14159/2 ) sim.Send_Touch_Sensor( sensorID = 0 , objectID = 0 ) sim.Send_Touch_Sensor( sensorID = 1 , objectID = 1 ) sim.Start() sim.Wait_To_Finish() sensorData = sim.Get_Sensor_Data(sensorID = 1) print sensorData #f = plt.figure() #pn = f.add_subplot(111) #plt.plot(sensorData) #plt.show()
class INDIVIDUAL:
def __init__(self, i):
self.ID = i
# genome[0] = Hidden <-> Sensor Neurons
# genome[1] = Hidden <-> Motor Neurons
# genome[2] = Hidden <-> Hidden Recurrent Neurons
self.genome = [
MatrixCreate(c.numHidNeurons, 5),
MatrixCreate(c.numHidNeurons, 8),
MatrixCreate(c.numHidNeurons, c.numHidNeurons)
]
self.genome[0] = MatrixRandomize(self.genome[0], 1, -1)
self.genome[1] = MatrixRandomize(self.genome[1], 1, -1)
self.genome[2] = MatrixRandomize(self.genome[2], 1, -1)
self.fitness = 0
self.stability = 0
self.uniqueness = 0
self.touchSensorData = np.array([])
def Evaluate(self,
environment,
hideSim=True,
startPaused=False,
printFalse=False):
self.Start_Evaluation(environment=environment,
hideSim=hideSim,
startPaused=startPaused)
self.Compute_Fitness(printFit=printFalse)
self.Store_Sensor_Data()
del self.sim
def Mutate(self, s=c.sMutation):
# This will choose a random gene to mutate. but this is based on a Gauss distribution. This has been edited to
# accept 2 dimensional genes
for i in range(0, 3):
# Synapse Layer
sLayer = random.randint(0, len(self.genome) - 1)
x = random.randint(0, len(self.genome[sLayer]) - 1)
y = random.randint(0, len(self.genome[sLayer][x]) - 1)
# random.gauss(mean, sigma)
self.genome[sLayer][x, y] = random.gauss(
self.genome[sLayer][x, y],
math.fabs(self.genome[sLayer][x, y] * s))
# # This will mutate ALL genes
# for i in range(0, len(self.genome)):
# self.genome[i] = random.gauss(self.genome[i], math.fabs(self.genome[i]))
# Clip the value of Gene
self.genome[sLayer][x, y] = np.clip(self.genome[sLayer][x, y], -1,
1)
#self.genome[x, y] = max(min(1, self.genome[x, y]), -1)
def Print(self):
print('[', self.ID, self.fitness, self.uniqueness, ']', end=" ")
#print(self.fitness, end=" "),
#print(self.genome)
def Start_Evaluation(self, environment, hideSim=True, startPaused=False):
self.sim = PYROSIM(playPaused=startPaused,
playBlind=hideSim,
evalTime=c.evaluationTime)
robot = ROBOT(self.sim, self.genome)
environment.Send_To(self.sim)
self.sim.Start()
def Compute_Fitness(self, printFit=False):
self.sim.Wait_To_Finish()
# x = self.sim.Get_Sensor_Data( sensorID=4, s=0 ) # s is the flag for the desired coordinate
# y = self.sim.Get_Sensor_Data( sensorID=4, s=1 )
# z = self.sim.Get_Sensor_Data( sensorID=4, s=2 )
# the returned value is the inverse squared of the distance to light source
distLight = self.sim.Get_Sensor_Data(sensorID=4, s=0)
distX = self.sim.Get_Sensor_Data(sensorID=5, s=0)
distY = self.sim.Get_Sensor_Data(sensorID=5, s=1)
# Touch Sensor Values.... Comes in vector
for t in range(0, 4):
self.stability += sum(self.sim.Get_Sensor_Data(sensorID=t)) * 0.25
# DEBUGGING #
if printFit:
plt.plot(distLight**0.25)
plt.show()
# Fitness Computation
self.fitness += distLight[-1]**0.25
# self.fitness += np.clip(distLight[-1], 0, 0.1)
def Store_Sensor_Data(self):
touchSensorData = np.zeros(shape=(4, c.evaluationTime))
for t in range(0, 4):
touchSensorData[t, :] = self.sim.Get_Sensor_Data(sensorID=t, s=0)
self.touchSensorData = touchSensorData
# Stability Data
columnSums = np.sum(touchSensorData, axis=0)
columnThresholds = np.clip(columnSums, 0, 1)
self.stability = np.sum(columnThresholds)
def Compute_Distance_Between(self, other):
try:
self.touchSensorData
except NameError:
print("Sensor Data was not Stored")
else:
#euclideanDistance = np.linalg.norm(self.touchSensorData - other.touchSensorData)
#euclideanDistance = self.fitness - other.fitness
delta = np.sum(
np.fabs(self.touchSensorData - other.touchSensorData))
return delta
# tOnGround = np.sum(self.touchSensorData)
# tOnGround_Other = np.sum(other.touchSensorData)
# euclideanDistance = np.fabs(tOnGround - tOnGround_Other)
#
# return euclideanDistance
def Mutate_Neat(self):
# synapse layer
sLayer = random.randint(0, len(self.genome) - 1)
# Shuffles the row of that Synapse layer
self.genome[sLayer] = self.genome[sLayer][
np.random.permutation(len(self.genome[sLayer][0])), :]
class INDIVIDUAL:
def __init__(self, i):
self.ID = i
self.genome = numpy.random.random([5, 8]) * 2 - 1
self.fitness = 0
def Start_Evaluation(self, env, pp, pb):
self.sim = PYROSIM(playPaused=pp,
playBlind=pb,
evalTime=constants.evaluationTime)
robot = ROBOT(self.sim, self.genome)
env.Send_To(self.sim)
self.sim.Start()
def Compute_Fitness(self):
self.sim.Wait_To_Finish()
sensorData = self.sim.Get_Sensor_Data(sensorID=4, s=0)
currentFitness = sensorData[-1]
if (currentFitness < self.fitness):
self.fitness = currentFitness
# self.fitness = self.fitness + currentFitness
# if ( self.fitness > 10.0 ):
# self.fitness = 10.0
del self.sim
def Mutate(self):
s = random.randint(0, 4)
m = random.randint(0, 7)
self.genome[s, m] = random.gauss(self.genome[s, m],
math.fabs(self.genome[s, m]))
if (self.genome[s, m] > 1.0):
self.genome[s, m] = 1.0
if (self.genome[s, m] < -1.0):
self.genome[s, m] = -1.0
def Print(self):
print '[',
print self.ID,
print self.fitness,
print '] ',
def Play(self, T=100):
sim = PYROSIM(playPaused=False, playBlind=False, evalTime=T)
self.Send_To_Simulator(sim)
sim.Start()
def run_experiment(
generator_fcn,
fitness_fcn,
name,
trial_num,
pop_size,
gens,
eval_time,
env_order,
dimensions,
env_space=False,
fitness_threshold=False,
development_layers=1,
):
time_stamp = dt.datetime.now().strftime('%Y-%m-%d_%H-%M-%S')
data = {}
envs_to_train_in = [env_space[i] for i in env_order]
evolver = evolvers.AFPO(max_population_size=pop_size,
generator_fcn=generator_fcn,
fitness_fcn=fitness_fcn,
environments=envs_to_train_in,
development_layers=development_layers,
max_generations=gens,
eval_time=eval_time,
fitness_threshold=fitness_threshold)
data['pop_size'] = evolver.max_population_size
#data['fitness_function'] = fitness_function
data['development_layers'] = evolver.development_layers
data['eval_time'] = evolver.eval_time
evolved_data = evolver.Evolve()
gens = evolver.generation - 1
data['gens'] = gens
best_robot = evolved_data[gens]['pareto_front'][0]['genome']
data['data'] = evolved_data
data['best_robot'] = best_robot
data['motor_speed'] = robots.MOTOR_SPEED
data['environments'] = envs_to_train_in
data['env_space'] = env_space
data['name'] = name
data['threshold'] = fitness_threshold
data['dimensions'] = dimensions
data['num_trained_in'] = len(envs_to_train_in)
env_fitness = [0] * 16
for index, env in enumerate(env_space):
sim = PYROSIM(playBlind=True, playPaused=False, evalTime=eval_time)
ids = best_robot.Send_To_Simulator(sim, eval_time=eval_time)
env.Send_To_Simulator(sim, ID_offset=ids[0])
sim.Start()
sim.Wait_To_Finish()
sensor_data = sim.Get_Results()
env_fitness[index] = fitness_fcn(sensor_data, env)
data_folder = './Data'
file_name = name + '_' + str(trial_num) + '_' + str(
len(env_order)) + '_' + str(dimensions) + '_' + str(time_stamp)
print file_name
pickle_file_name = file_name + '.pickle'
json_file_name = file_name + '.json'
if not os.path.isdir(data_folder):
os.makedirs(data_folder)
pickle_path_to_file = data_folder + '/' + pickle_file_name
json_path_to_file = data_folder + '/' + json_file_name
with open(pickle_path_to_file, 'w') as f:
pickle.dump(data, f)
#Create json daat
json_data = {}
print 'env fit', env_fitness
print 'env order', env_order
#store json data
json_data['env_fitness'] = env_fitness
json_data['type'] = name
json_data['threshold'] = fitness_threshold
json_data['dimensions'] = dimensions
json_data['num_trained_in'] = len(envs_to_train_in)
json_data['env_order'] = env_order
#Write json
with open(json_path_to_file, 'w') as f:
json.dump(json_data, f)
class INDIVIDUAL:
def __init__(self, i):
self.ID = i
self.genome = MatrixCreate(4, 8)
self.genome = MatrixRandomize(self.genome, 1, -1)
self.fitness = 0
def Evaluate(self, hideSim=True, startPaused=False):
sim = PYROSIM(playPaused=startPaused,
playBlind=hideSim,
evalTime=c.evaluationTime)
robot = ROBOT(sim, self.genome)
sim.Start()
sim.Wait_To_Finish()
x = sim.Get_Sensor_Data(sensorID=4, s=0)
y = sim.Get_Sensor_Data(sensorID=4, s=1)
z = sim.Get_Sensor_Data(sensorID=4, s=2)
# self.sensorData_0 = sim.Get_Sensor_Data(sensorID=0)
# self.sensorData_1 = sim.Get_Sensor_Data(sensorID=1)
# self.sensorData_2 = sim.Get_Sensor_Data(sensorID=2)
# self.sensorData_3 = sim.Get_Sensor_Data(sensorID=3)
# self.sensorData_4 = sim.Get_Sensor_Data(sensorID=4)
# print(sim.Get_Sensor_Data(sensorID=4))
self.fitness = y[-1]
del sim
def Mutate(self):
# This will choose a random gene to mutate. but this is based on a Gauss distribution. This has been edited to
# accept 2 dimensional genes
x = random.randint(0, len(self.genome) - 1)
y = random.randint(0, len(self.genome[0]) - 1)
self.genome[x, y] = random.gauss(self.genome[x, y],
math.fabs(self.genome[x, y]))
# # This will mutate ALL genes
# for i in range(0, len(self.genome)):
# self.genome[i] = random.gauss(self.genome[i], math.fabs(self.genome[i]))
def Print(self):
print('[', self.ID, self.fitness, ']', end=" ")
#print(self.fitness, end=" "),
#print(self.genome)
def Start_Evaluation(self, hideSim=True, startPaused=False):
self.sim = PYROSIM(playPaused=startPaused,
playBlind=hideSim,
evalTime=c.evaluationTime)
robot = ROBOT(self.sim, self.genome)
self.sim.Start()
def Compute_Fitness(self):
self.sim.Wait_To_Finish()
x = self.sim.Get_Sensor_Data(sensorID=4, s=0)
y = self.sim.Get_Sensor_Data(sensorID=4, s=1)
z = self.sim.Get_Sensor_Data(sensorID=4, s=2)
# self.sensorData_0 = sim.Get_Sensor_Data(sensorID=0)
# self.sensorData_1 = sim.Get_Sensor_Data(sensorID=1)
# self.sensorData_2 = sim.Get_Sensor_Data(sensorID=2)
# self.sensorData_3 = sim.Get_Sensor_Data(sensorID=3)
# self.sensorData_4 = sim.Get_Sensor_Data(sensorID=4)
self.fitness = y[-1]
del self.sim
