def _initialize(self, **kwargs):
"""
:param kwargs:
:return:
"""
super(CoreComponentWithMics, self)._initialize(**kwargs)
half_width = WIDTH / 2.0
right_angle = 3.14159265 / 2.0
# rotates clockwise around axis by angle:
mic_pose_1 = Pose(position=Vector3(0, half_width, 0),
rotation=Quaternion.from_angle_axis(
angle=right_angle, axis=Vector3(-1, 0, 0)))
mic_pose_2 = Pose(position=Vector3(half_width, 0, 0),
rotation=Quaternion.from_angle_axis(
angle=right_angle, axis=Vector3(0, 1, 0)))
mic_pose_3 = Pose(position=Vector3(0, -half_width, 0),
rotation=Quaternion.from_angle_axis(
angle=right_angle, axis=Vector3(1, 0, 0)))
mic_pose_4 = Pose(position=Vector3(-half_width, 0, 0),
rotation=Quaternion.from_angle_axis(
angle=right_angle, axis=Vector3(0, -1, 0)))
# The second argument ("direction") is the type of the sensor that is checked in SensorFactory.cpp.
# Any existing type of gazebo sensors can be used. Beside that, a new type can be registered in WorldController.cpp
mic_1 = SdfSensor("dir_sensor_1",
"direction",
pose=mic_pose_1,
update_rate=self.conf.sensor_update_rate)
mic_2 = SdfSensor("dir_sensor_2",
"direction",
pose=mic_pose_2,
update_rate=self.conf.sensor_update_rate)
mic_3 = SdfSensor("dir_sensor_3",
"direction",
pose=mic_pose_3,
update_rate=self.conf.sensor_update_rate)
mic_4 = SdfSensor("dir_sensor_4",
"direction",
pose=mic_pose_4,
update_rate=self.conf.sensor_update_rate)
self.link.add_sensor(mic_1)
self.link.add_sensor(mic_2)
self.link.add_sensor(mic_3)
self.link.add_sensor(mic_4)
def do_mate(world, ra, rb):
"""
:param world:
:param ra:
:param rb:
:return:
"""
mate = None
num_attempts = 0
while num_attempts < 10:
# Attempt reproduction
mate = yield From(world.attempt_mate(ra, rb))
if mate:
break
num_attempts += 1
# logger.debug("Mates selected, highlighting points...")
new_pos = pick_position()
new_pos.z = insert_z
# hls = yield From(create_highlights(
# world, ra.last_position, rb.last_position, new_pos))
# yield From(trollius.sleep(2))
if mate:
logger.debug("Inserting child...")
child, bbox = mate
pose = Pose(position=new_pos)
spawn_robot(world, tree=child, pose=pose, parents=[ra, rb])
else:
logger.debug("Could not mate")
def do_mate(world, ra, rb):
"""
:param world:
:param ra:
:param rb:
:return:
"""
mate = None
while True:
# Attempt reproduction
mate = yield From(world.attempt_mate(ra, rb))
if mate:
break
logger.debug("Mates selected, highlighting points...")
new_pos = get_insert_position(world.conf, ra, rb, world)
new_pos.z = insert_z
hls = yield From(create_highlights(
world, ra.last_position, rb.last_position, new_pos))
yield From(trollius.sleep(2))
logger.debug("Inserting child...")
child, bbox = mate
pose = Pose(position=new_pos)
future = yield From(world.insert_robot(child, pose, parents=[ra, rb]))
yield From(future)
yield From(sleep_sim_time(world, 1.8))
logger.debug("Removing highlights...")
yield From(remove_highlights(world, hls))
def run(self, conf):
conf.min_parts = 1
conf.max_parts = 3
conf.arena_size = (3, 3)
conf.max_lifetime = 99999
conf.initial_age_mu = 99999
conf.initial_age_sigma = 1
conf.age_cutoff = 99999
self.body_spec = get_body_spec(conf)
self.brain_spec = get_brain_spec(conf)
self.nn_parser = NeuralNetworkParser(self.brain_spec)
print "OPENING FILES!!!!!!!!!!!!!!!!!!!"
with open("body/{0}".format(self.body_file), 'r') as robot_file:
robot_yaml = robot_file.read()
for filename in self.brain_files:
with open("brains/{0}".format(filename, 'r')) as brain_file:
br_yaml = brain_file.read()
self.brain_genotypes.append(
yaml_to_genotype(br_yaml, self.brain_spec))
yield From(wait_for(self.pause(True)))
pose = Pose(position=Vector3(0, 0, 0.5), rotation=rotate_vertical(0))
robot_pb = yaml_to_robot(self.body_spec, self.brain_spec, robot_yaml)
tree = Tree.from_body_brain(robot_pb.body, robot_pb.brain,
self.body_spec)
print "INSERTING ROBOT!!!!!!!!!!!!!!!!!!!!!!"
robot = yield From(wait_for(self.insert_robot(tree, pose)))
self.robot_name = robot.name
self.modify_nn_publisher = yield From(
self.manager.advertise(
'/gazebo/default/{0}/modify_neural_network'.format(
self.robot_name),
'gazebo.msgs.ModifyNeuralNetwork',
))
# Wait for connections
yield From(self.modify_nn_publisher.wait_for_listener())
brain_num = 0
num_of_brains = len(self.brain_genotypes)
print "Number of brains = {0}".format(num_of_brains)
yield From(wait_for(self.pause(False)))
while (True):
if self.timers.is_it_time('evaluate', self.time_period,
self.get_world_time()):
n = brain_num % num_of_brains
print "Switching brain to #{0}!!!!!!!!!".format(n)
yield From(self.insert_brain(self.brain_genotypes[n]))
self.timers.reset('evaluate', self.get_world_time())
brain_num += 1
yield From(trollius.sleep(0.1))
def run_server(conf):
"""
:param args:
:return:
"""
conf.proposal_threshold = 0
conf.output_directory = None
conf.min_parts = 6
conf.max_parts = 15
conf.arena_size = (3, 3)
interactive = [True]
world = yield From(World.create(conf))
yield From(world.pause(True))
start_bots = conf.num_initial_bots
poses = [Pose(position=pick_position(conf)) for _ in range(start_bots)]
trees, bboxes = yield From(world.generate_population(len(poses)))
fut = yield From(world.insert_population(trees, poses))
yield From(fut)
# List of reproduction requests
reproduce = []
# Request callback for the subscriber
def callback(data):
req = Request()
req.ParseFromString(data)
imode_value = None
if req.request == "produce_offspring":
reproduce.append(req.data.split("+++"))
imode_value = True
elif req.request == "enable_interaction":
imode_value = True
elif req.request == "disable_interaction":
imode_value = False
if imode_value is not None:
interactive[0] = imode_value
print("Interactive mode is now %s" % ("ON" if interactive[0] else "OFF"))
if not interactive[0]:
reproduce[:] = []
subscriber = world.manager.subscribe(
'/gazebo/default/request', 'gazebo.msgs.Request', callback)
yield From(subscriber.wait_for_connection())
while True:
if interactive[0]:
yield From(interactive_mode(world, reproduce))
else:
yield From(automatic_mode(conf, world, interactive))
yield From(trollius.sleep(0.1))
yield From(cleanup(world))
def spawn_initial_robots(world, number):
"""
:param world:
:type world: World
:return:
"""
poses = [Pose(position=pick_position()) for _ in range(number)]
trees, bboxes = yield From(world.generate_population(len(poses)))
for index in range(number):
spawn_robot(world, trees[index], poses[index])
def birth(self, tree, bbox, parents):
"""
Birth process, picks a robot position and inserts
the robot into the world.
Robots are currently placed at a random position within the circular
birth clinic. In this process, 5 attempts are made to place the robot
at the minimum drop distance from other robots. If this fails however
the last generated position is used anyway.
:param tree:
:param bbox:
:param parents:
:return:
"""
s = len(tree)
if (s + self.total_size()) > self.conf.part_limit:
print("Not enough parts in pool to create robot of size %d." % s)
raise Return(False)
# Pick a random radius and angle within the birth clinic
pos = Vector3()
pos.z = -bbox.min.z + self.conf.drop_height
done = False
min_drop = self.conf.min_drop_distance
for i in range(5):
angle = random.random() * 2 * math.pi
radius = self.conf.birth_clinic_diameter * 0.5 * random.random()
pos.x = radius * math.cos(angle)
pos.y = radius * math.sin(angle)
done = True
for robot in self.robots.values():
if robot.distance_to(pos, planar=True) < min_drop:
done = False
break
if done:
break
if not done:
logger.warning("Warning: could not satisfy minimum drop distance.")
# Note that we register the reproduction only if
# the child is actually born, i.e. there were enough parts
# left in the world to satisfy the request.
if parents:
ra, rb = parents
ra.did_mate_with(rb)
rb.did_mate_with(ra)
self.births += 1
fut = yield From(self.insert_robot(tree, Pose(position=pos), parents=parents))
raise Return(fut)
def spawn_population(world, conf):
"""
:param world:
:type world: World
:return:
"""
poses = [Pose(position=pick_position(conf)) for _ in range(1)]
trees, bboxes = yield From(world.generate_population(len(poses)))
fut = yield From(world.insert_population(trees, poses))
yield From(fut)
def get_poses(n, z=in_mm(24.5), spacing=0.4, row_limit=5):
x = 0
y = 0
poses = []
for i in range(n):
poses.append(Pose(position=Vector3(x, y, z)))
x += spacing
if (i % row_limit) == 0:
y += spacing
x = 0
return poses
def run_server(conf):
"""
:param args:
:return:
"""
# conf.proposal_threshold = 0
# conf.output_directory = None
conf.min_parts = 1
conf.max_parts = 3
conf.arena_size = (3, 3)
# interactive = [True]
world = yield From(World.create(conf))
yield From(world.pause(True))
start_bots = conf.num_initial_bots
poses = [Pose(position=pick_position(conf)) for _ in range(start_bots)]
trees, bboxes = yield From(world.generate_population(len(poses)))
fut = yield From(world.insert_population(trees, poses))
yield From(fut)
# List of reproduction requests
reproduce = []
# Request callback for the subscriber
def callback(data):
req = Request()
req.ParseFromString(data)
if req.request == "produce_offspring":
reproduce.append(req.data.split("+++"))
subscriber = world.manager.subscribe(
'/gazebo/default/request', 'gazebo.msgs.Request', callback)
yield From(subscriber.wait_for_connection())
yield From(world.pause(False))
while True:
yield From(spawn_population(world, conf))
yield From(trollius.sleep(12))
yield From(cleanup(world))
def run(conf):
"""
:param conf:
:return:
"""
conf.evaluation_time = 5.0
conf.output_directory = 'output'
world = yield From(World.create(conf))
yield From(world.pause(True))
for i in range(3):
ta, _, _ = yield From(world.generate_valid_robot())
tb, _, _ = yield From(world.generate_valid_robot())
ra = yield From(
wait_for(
world.insert_robot(ta,
pose=Pose(position=Vector3(0, 3 *
i, 0.5)))))
rb = yield From(
wait_for(
world.insert_robot(
tb, pose=Pose(position=Vector3(0, 3 * i + 1, 0.5)))))
while True:
# Attempt reproduction
mate = yield From(world.attempt_mate(ra, rb))
if mate:
break
tree, bbox = mate
yield From(
wait_for(
world.insert_robot(
tree, pose=Pose(position=Vector3(0, 3 * i + 2, 0.5)))))
def get_circle_poses(n, radius):
"""
:param n:
:param radius:
:return:
"""
shift = 2.0 * math.pi / n
poses = []
for i in xrange(n):
angle = i * shift
x, y = radius * math.cos(angle), radius * math.sin(angle)
starting_rotation = Quaternion.from_angle_axis(math.pi + angle, Vector3(0, 0, 1))
poses.append(Pose(position=Vector3(x, y, 0), rotation=starting_rotation))
return poses
def birth(world, tree, bbox, parents):
"""
Birth process, picks a robot position and inserts
the robot into the world.
:param world:
:param tree:
:param bbox:
:param parents:
:return:
"""
angle = random.random() * 2 * math.pi
r = 2.0
# Drop height is 20cm here
# TODO Should we check whether other robots are not too close?
pos = Vector3(r * math.cos(angle), r * math.sin(angle), -bbox.min.z + 0.2)
fut = yield From(world.insert_robot(tree, Pose(position=pos), parents))
raise Return(fut)
def evaluate_pair(self, tree, bbox, parents=None):
"""
Evaluates a single robot tree.
:param tree:
:param bbox:
:param parents:
:return: Evaluated Robot object
"""
# Pause the world just in case it wasn't already
yield From(wait_for(self.pause(True)))
pose = Pose(position=Vector3(0, 0, -bbox.min.z))
fut = yield From(self.insert_robot(tree, pose, parents=parents))
robot = yield From(fut)
max_age = self.conf.evaluation_time + self.conf.warmup_time
# Unpause the world to start evaluation
yield From(wait_for(self.pause(False)))
before = time.time()
while True:
if robot.age() >= max_age:
break
# Sleep for the pose update frequency, which is about when
# we expect a new age update.
yield From(trollius.sleep(1.0 / self.state_update_frequency))
yield From(wait_for(self.delete_robot(robot)))
yield From(wait_for(self.pause(True)))
diff = time.time() - before
if diff > self.conf.evaluation_threshold:
sys.stderr.write(
"Evaluation threshold exceeded, shutting down with nonzero status code.\n"
)
sys.stderr.flush()
sys.exit(15)
raise Return(robot)
def run_server():
conf = Config(proposal_threshold=0, output_directory='output')
world = yield From(World.create(conf))
yield From(world.pause(True))
wall_x = conf.arena_size[0] / 2.0
wall_y = conf.arena_size[1] / 2.0
wall_points = [
Vector3(-wall_x, -wall_y, 0),
Vector3(wall_x, -wall_y, 0),
Vector3(wall_x, wall_y, 0),
Vector3(-wall_x, wall_y, 0)
]
future = yield From(world.build_walls(wall_points))
yield From(future)
grid_size = (1, 2)
spacing = 3 * conf.mating_distance
grid_x, grid_y = grid_size
x_offset = -(grid_x - 1) * spacing * 0.5
y_offset = -(grid_y - 1) * spacing * 0.5
poses = [
Pose(position=Vector3(x_offset + spacing * i, y_offset +
spacing * j, 0))
for i, j in itertools.product(range(grid_x), range(grid_y))
]
trees, bboxes = yield From(world.generate_population(len(poses)))
for pose, bbox in itertools.izip(poses, bboxes):
pose.position += Vector3(0, 0, bbox[2])
fut = yield From(world.insert_population(trees, poses))
yield From(fut)
while True:
yield From(trollius.sleep(0.1))
yield From(world.perform_reproduction())
async def run():
conf = parser.parse_args()
conf.evaluation_time_sigma = 2.0
conf.weight_mutation_probability = 0.8
conf.weight_mutation_sigma = 5.0
conf.param_mutation_probability = 0.8
conf.param_mutation_sigma = 0.25
# this is the world state update frequency in simulation Hz
conf.pose_update_frequency = 5 # in simulation Hz
# update frequency of sensors in simulation Hz (default 10Hz)
# conf.sensor_update_rate = 10.
# # these are irrelevant parameters but we need to set them anyway,
# # otherwise it won't work
# conf.min_parts = 1
# conf.max_parts = 3
# conf.arena_size = (3, 3)
# conf.max_lifetime = 99999
# conf.initial_age_mu = 99999
# conf.initial_age_sigma = 1
# conf.age_cutoff = 99999
# create the learning manager
world = await LearningManager.create(conf)
await world.pause(True)
path_to_log_dir = os.path.join(world.path_to_log_dir, "learner1")
body_spec = get_body_spec(conf)
brain_spec = get_extended_brain_spec(conf)
# mutation spec contains info about what parameters of neurons can be mutated
mut_spec = get_extended_mutation_spec(conf.param_mutation_sigma,
conf.weight_mutation_sigma)
# what types of neurons can be added to the network
# allowed_types = ["Simple", "Sigmoid", "DifferentialCPG"]
# allowed_types = ["Simple", "Sigmoid"]
allowed_types = ["Simple"]
mutator = Mutator(mut_spec, allowed_neuron_types=allowed_types)
# if we are not restoring a saved state:
if not world.do_restore:
with open(conf.test_bot, 'r') as yamlfile:
bot_yaml = yamlfile.read()
pose = Pose(position=Vector3(0, 0, 0.2))
bot = yaml_to_robot(body_spec, brain_spec, bot_yaml)
tree = Tree.from_body_brain(bot.body, bot.brain, body_spec)
robot = await world.insert_robot(tree, pose)
learner = RobotLearner(world=world,
robot=robot,
brain_spec=brain_spec,
mutator=mutator,
conf=conf)
gen_files = []
init_brain_list = None
if (os.path.isdir(path_to_log_dir)):
gen_files = list(fname for fname in os.listdir(path_to_log_dir) if \
fnmatch.fnmatch(fname, "gen_*_genotypes.log"))
# if we are reading an initial population from a file:
if len(gen_files) > 0:
gen_files = sorted(gen_files,
key=lambda item: int(item.split('_')[1]))
last_gen_file = gen_files[-1]
num_generations = int(last_gen_file.split('_')[1]) # + 1
num_brains_evaluated = conf.population_size * num_generations
print("last generation file = {0}".format(last_gen_file))
# get list of brains from the last generation log file:
init_brain_list, min_mark, max_mark = \
get_brains_from_file(os.path.join(path_to_log_dir, last_gen_file), brain_spec)
print("Max historical mark = {0}".format(max_mark))
# set mutator's innovation number according to the max historical mark:
mutator.innovation_number = max_mark + 1
learner.total_brains_evaluated = num_brains_evaluated
learner.generation_number = num_generations
# initialize learner with initial list of brains:
await world.add_learner(learner, "learner1", init_brain_list)
print(learner.parameter_string())
# log experiment parameter values:
create_param_log_file(conf, learner.generation_number,
os.path.join(path_to_log_dir, "parameters.log"))
# copy the robot body file:
shutil.copy(conf.test_bot, path_to_log_dir)
# if we are restoring a saved state:
else:
print("WORLD RESTORED FROM {0}".format(world.world_snapshot_filename))
print("STATE RESTORED FROM {0}".format(world.snapshot_filename))
print("WORLD CREATED")
await world.run()
def run():
"""
The main coroutine, which is started below.
"""
# Parse command line / file input arguments
conf = parser.parse_args()
conf.output_directory = "output"
conf.restore_directory = "restore"
with open("models/robot_26.yaml", 'r') as yamlfile:
bot_yaml1 = yamlfile.read()
with open("models/robot_150.yaml", 'r') as yamlfile:
bot_yaml2 = yamlfile.read()
# Create the world, this connects to the Gazebo world
world = yield From(World.create(conf))
# These are useful when working with YAML
body_spec = world.builder.body_builder.spec
brain_spec = world.builder.brain_builder.spec
# Create a robot from YAML
protobot1 = yaml_to_robot(body_spec=body_spec,
nn_spec=brain_spec,
yaml=bot_yaml1)
# Create a robot from YAML
protobot2 = yaml_to_robot(body_spec=body_spec,
nn_spec=brain_spec,
yaml=bot_yaml2)
# Create a revolve.angle `Tree` representation from the robot, which
# is what is used in the world manager.
robot_tree1 = Tree.from_body_brain(body=protobot1.body,
brain=protobot1.brain,
body_spec=body_spec)
# Create a revolve.angle `Tree` representation from the robot, which
# is what is used in the world manager.
robot_tree2 = Tree.from_body_brain(body=protobot2.body,
brain=protobot2.brain,
body_spec=body_spec)
# Insert the robot into the world. `insert_robot` resolves when the insert
# request is sent, the future it returns resolves when the robot insert
# is actually confirmed and a robot manager object has been created
pose1 = Pose(position=Vector3(0, 0, 0.05))
pose2 = Pose(position=Vector3(0, 2, 0.05))
future1 = yield From(
world.insert_robot(
tree=robot_tree1,
pose=pose1,
# name="robot_26"
))
future2 = yield From(
world.insert_robot(
tree=robot_tree2,
pose=pose2,
# name="robot_26"
))
robot_manager1 = yield From(future1)
robot_manager2 = yield From(future2)
# I usually start the world paused, un-pause it here. Note that
# pause again returns a future for when the request is sent,
# that future in turn resolves when a response has been received.
# This is the general convention for all message actions in the
# world manager. `wait_for` saves the hassle of grabbing the
# intermediary future in this case.
yield From(wait_for(world.pause(True)))
# Start a run loop to do some stuff
while True:
# Print robot fitness every second
print("Robot fitness is {fitness}".format(
fitness=robot_manager1.fitness()))
yield From(trollius.sleep(1.0))