def __init__(self, name, start, end, thickness, height, **kwargs):
"""
Construct a wall of the given thickness and height from
`start` to `end`. This simply results in a box.
:param start: Starting point of the wall.
:type start: Vector3
:param end: Ending point of the wall.
:type end: Vector3
:return:
"""
super(Wall, self).__init__(name, static=True, **kwargs)
if start.z != end.z:
raise AssertionError(
"Walls with different start / end z-axis are undefined.")
center = 0.5 * (end + start)
diff = end - start
size = abs(diff)
self.wall = Link("wall_link")
self.wall.make_box(10e10, size, thickness, height)
# Rotate the wall so it aligns with the vector from
# x to y
self.align(Vector3(0, 0, 0), Vector3(1, 0, 0), Vector3(0, 0, 1),
center, diff, Vector3(0, 0, 1), Posable("mock"))
self.add_element(self.wall)
def parse_vec3(source: str):
# example source: Vector3(2.394427e+00, 3.195821e-01, 2.244915e-02)
assert (source[:8] == 'Vector3(')
assert (source[-1:] == ')')
source = source[8:-1]
try:
x, y, z = [float(n) for n in source.split(', ')]
except ValueError:
print(f'could not parse vector3 for "{source}"')
return Vector3()
return Vector3(x, y, z)
def export_life_data(self, folder):
life_measures = {
'distance':
str(measures.displacement(self.manager)[0]),
'distance_magnitude':
str(measures.displacement(self.manager)[0].magnitude()),
'velocity':
str(measures.velocity(self.manager)),
'displacement_velocity':
str(measures.displacement_velocity(self.manager)),
'path_length':
str(measures.path_length(self.manager)),
}
life = {
'starting_time':
float(self.manager.starting_time),
'age':
float(self.age()),
'charge':
self.charge(),
'start_pos':
str(self.starting_position()),
'last_pos':
str(self.pos()),
'avg_orientation':
str(
Vector3(self.manager.avg_roll, self.manager.avg_pitch,
self.manager.avg_yaw)),
'avg_pos':
str(
Vector3(self.manager.avg_x, self.manager.avg_y,
self.manager.avg_z)),
'last_mate':
str(self.manager.last_mate),
'alive':
str(self.alive()),
'birth':
str(self.birth_type),
'parents': [parent.name for parent in self.parents]
if self.parents is not None else 'None',
'children': [child.name for child in self.children],
'measures':
life_measures,
}
with open(os.path.join(folder, f'life_{self.id}.yaml'), 'w') as f:
f.write(str(yaml.dump(life)))
def _robot_inserted(
self,
robot,
msg
):
"""
Registers a newly inserted robot and marks the insertion
message response as handled.
:param robot: RevolveBot
:param msg:
:type msg: pygazebo.msgs.response_pb2.Response
:return:
"""
inserted = ModelInserted()
inserted.ParseFromString(msg.serialized_data)
model = inserted.model
time = Time(msg=inserted.time)
p = model.pose.position
position = Vector3(p.x, p.y, p.z)
robot_manager = self.create_robot_manager(
robot,
position,
time
)
self.register_robot(robot_manager)
return robot_manager
async def _evaluate_robot(self, simulator_connection, robot, conf):
if robot.failed_eval_attempt_count == 3:
logger.info(
f'Robot {robot.phenotype.id} evaluation failed (reached max attempt of 3), fitness set to None.'
)
robot_fitness = None
return robot_fitness, None
else:
# Change this `max_age` from the command line parameters (--evalution-time)
max_age = conf.evaluation_time
robot_manager = await simulator_connection.insert_robot(
robot.phenotype, Vector3(0, 0, self._settings.z_start),
max_age)
start = time.time()
# Start a run loop to do some stuff
while not robot_manager.dead: # robot_manager.age() < max_age:
await asyncio.sleep(1.0 / 2) # 5= state_update_frequency
end = time.time()
elapsed = end - start
logger.info(f'Time taken: {elapsed}')
robot_fitness = conf.fitness_function(robot_manager, robot)
simulator_connection.unregister_robot(robot_manager)
# await simulator_connection.delete_all_robots()
# await simulator_connection.delete_robot(robot_manager)
# await simulator_connection.pause(True)
await simulator_connection.reset(rall=True,
time_only=True,
model_only=False)
return robot_fitness, measures.BehaviouralMeasurements(
robot_manager, robot)
async def run():
"""
The main coroutine, which is started below.
"""
# Parse command line / file input arguments
settings = parser.parse_args()
# Load a robot from yaml
robot = revolve_bot.RevolveBot()
if settings.robot_yaml is None:
robot.load_file("experiments/bo_learner/yaml/spider.yaml")
else:
robot.load_file(settings.robot_yaml)
robot.update_substrate()
# Connect to the simulator and pause
world = await World.create(settings)
await world.pause(True)
await world.delete_model(robot.id)
await asyncio.sleep(2.5)
# Insert the robot in the simulator
robot_manager = await world.insert_robot(robot, Vector3(0, 0, 0.025))
# Resume simulation
await world.pause(False)
# Start a run loop to do some stuff
while True:
await asyncio.sleep(5.0)
async def run():
"""
The main coroutine, which is started below.
"""
# Parse command line / file input arguments
settings = parser.parse_args()
# Load a robot from yaml
robot = revolve_bot.RevolveBot()
robot.load_file("experiments/examples/yaml/spider.yaml")
# Connect to the simulator and pause
world = await World.create(settings)
await world.pause(True)
# Insert the robot in the simulator
robot_manager = await world.insert_robot(robot, Vector3(0, 0, 0.05))
# Resume simulation
await world.pause(False)
# Start a run loop to do some stuff
while True:
# Print robot fitness every second
print("Robot fitness is {fitness}".format(
fitness=robot_manager.fitness()))
await asyncio.sleep(1.0)
async def run():
"""
The main coroutine, which is started below
"""
log = logger.create_logger('experiment',
handlers=[
logging.StreamHandler(sys.stdout),
])
# Set debug level to DEBUG
log.setLevel(logging.DEBUG)
# Parse command line / file input arguments
settings = parser.parse_args()
# Start Simulator
if settings.simulator_cmd != 'debug':
simulator_supervisor = DynamicSimSupervisor(
world_file=settings.world,
simulator_cmd=settings.simulator_cmd,
simulator_args=["--verbose"],
plugins_dir_path=os.path.join('.', 'build', 'lib'),
models_dir_path=os.path.join('.', 'models'),
simulator_name='gazebo')
await simulator_supervisor.launch_simulator(port=settings.port_start)
await asyncio.sleep(0.1)
# Connect to the simulator and pause
connection = await World.create(settings,
world_address=('127.0.0.1',
settings.port_start))
await asyncio.sleep(1)
# initialization finished
# load robot file
robot = RevolveBot(_id=settings.test_robot)
robot.load_file("/Users/nihed/Desktop/nihedssnakes/nihedssnake6.yaml",
conf_type='yaml')
robot.save_file(
f'{"/Users/nihed/Desktop/nihedssnakes/nihedssnake6.yaml"}.sdf',
conf_type='sdf')
# insert robot into the simulator
robot_manager = await connection.insert_robot(robot,
Vector3(0, 0, 0.25),
life_timeout=None)
await asyncio.sleep(1.0)
# Start the main life loop
while True:
# Print robot fitness every second
status = 'dead' if robot_manager.dead else 'alive'
print(
f"Robot fitness ({status}) is \n"
f" OLD: {fitness.online_old_revolve(robot_manager)}\n"
f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n"
f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}")
await asyncio.sleep(1.0)
async def analyze_robot(self, robot, max_attempts=5):
"""
Performs body analysis of a given Robot object.
:param robot:
:type robot: Robot
:param max_attempts:
:return:
"""
sdf = robot.to_sdf(pose=Vector3(0, 0, 0))
ret = await self.analyze_sdf(sdf, max_attempts=max_attempts)
return ret
async def run():
"""
The main coroutine, which is started below.
"""
robot_file_path = "experiments/examples/yaml/spider.yaml"
# Parse command line / file input arguments
settings = parser.parse_args()
# Start Simulator
if settings.simulator_cmd != 'debug':
simulator_supervisor = DynamicSimSupervisor(
world_file=settings.world,
simulator_cmd=settings.simulator_cmd,
simulator_args=["--verbose"],
plugins_dir_path=os.path.join('.', 'build', 'lib'),
models_dir_path=os.path.join('.', 'models'),
simulator_name='gazebo')
await simulator_supervisor.launch_simulator(port=settings.port_start)
await asyncio.sleep(0.1)
# Load a robot from yaml
robot = revolve_bot.RevolveBot()
robot.load_file(robot_file_path)
robot.update_substrate()
# robot._brain = BrainRLPowerSplines()
# Connect to the simulator and pause
connection = await World.create(settings,
world_address=('127.0.0.1',
settings.port_start))
await asyncio.sleep(1)
# Starts the simulation
await connection.pause(False)
# Insert the robot in the simulator
robot_manager = await connection.insert_robot(
robot, Vector3(0, 0, settings.z_start))
# Start a run loop to do some stuff
while True:
# Print robot fitness every second
status = 'dead' if robot_manager.dead else 'alive'
print(
f"Robot fitness ({status}) is \n"
f" OLD: {fitness.online_old_revolve(robot_manager)}\n"
f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n"
f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}")
await asyncio.sleep(1.0)
def displacement(self):
"""
Returns a tuple of the displacement in both time and space
between the first and last registered element in the speed
window.
:return: Tuple where the first item is a displacement vector
and the second a `Time` instance.
:rtype: tuple(Vector3, Time)
"""
if self.last_position is None:
return Vector3(0, 0, 0), Time()
return (self._positions[-1] - self._positions[0],
self._times[-1] - self._times[0])
def displacement(robot_manager):
"""
Returns a tuple of the displacement in both time and space
between the first and last registered element in the speed
window.
:return: Tuple where the first item is a displacement vector
and the second a `Time` instance.
:rtype: tuple(Vector3, Time)
"""
if len(robot_manager._positions) == 0:
return Vector3(0, 0, 0), Time()
return (
robot_manager._positions[-1] - robot_manager._positions[0],
robot_manager._times[-1] - robot_manager._times[0]
)
async def insert_robot(
self,
revolve_bot,
pose=Vector3(0, 0, 0.05),
life_timeout=None,
):
"""
Inserts a robot into the world. This consists of two steps:
- Sending the insert request message
- Receiving a ModelInfo response
This method is a coroutine because of the first step, writing
the message must be yielded since PyGazebo doesn't appear to
support writing multiple messages simultaneously. For the response,
i.e. the message that confirms the robot has been inserted, a
future is returned.
:param revolve_bot:
:type revolve_bot: RevolveBot
:param pose: Insertion pose of a robot
:type pose: Pose|Vector3
:param life_timeout: Life span of the robot
:type life_timeout: float|None
:return: A future that resolves with the created `Robot` object.
"""
# if the ID is digit, when removing the robot, the simulation will try to remove random stuff from the
# environment and give weird crash errors
assert(not str(revolve_bot.id).isdigit())
sdf_bot = revolve_bot.to_sdf(pose)
# if self.output_directory:
# robot_file_path = os.path.join(
# self.output_directory,
# 'robot_{}.sdf'.format(revolve_bot.id)
# )
# with open(robot_file_path, 'w') as f:
# f.write(sdf_bot)
response = await self.insert_model(sdf_bot, life_timeout)
robot_manager = self._robot_inserted(
robot=revolve_bot,
msg=response
)
return robot_manager
async def _evaluate_robot(simulator_connection, robot, conf):
await simulator_connection.pause(True)
robot_manager = await simulator_connection.insert_robot(
robot, Vector3(0, 0, 0.25))
await simulator_connection.pause(False)
start = time.time()
# Start a run loop to do some stuff
max_age = conf.evaluation_time
while robot_manager.age() < max_age:
await asyncio.sleep(1.0 / 5) # 5= state_update_frequency
end = time.time()
elapsed = end - start
logger.info(f'Time taken: {elapsed}')
robot_fitness = conf.fitness_function(robot_manager)
await simulator_connection.pause(True)
delete_future = await simulator_connection.delete_all_robots(
) # robot_manager
await delete_future
return robot_fitness
async def insert_robot(
self,
revolve_bot,
pose=Vector3(0, 0, 0.05),
):
"""
Inserts a robot into the world. This consists of two steps:
- Sending the insert request message
- Receiving a ModelInfo response
This method is a coroutine because of the first step, writing
the message must be yielded since PyGazebo doesn't appear to
support writing multiple messages simultaneously. For the response,
i.e. the message that confirms the robot has been inserted, a
future is returned.
:param revolve_bot:
:type revolve_bot: RevolveBot
:param pose: Insertion pose of a robot
:type pose: Pose|Vector3
:return: A future that resolves with the created `Robot` object.
"""
sdf_bot = revolve_bot.to_sdf(pose)
if self.output_directory:
robot_file_path = os.path.join(
self.output_directory, 'robot_{}.sdf'.format(revolve_bot.id))
with open(robot_file_path, 'w') as f:
f.write(sdf_bot)
future = Future()
insert_future = await self.insert_model(sdf_bot)
# TODO: Unhandled error in exception handler. Fix this.
insert_future.add_done_callback(lambda fut: self._robot_inserted(
robot=revolve_bot, msg=fut.result(), return_future=future))
return future
def update_state(self, world, time, state, poses_file):
"""
Updates the robot state from a state message.
:param world: Instance of the world
:param time: The simulation time at the time of this
position update.
:type time: Time
:param state: State message
:param poses_file: CSV writer to write pose to, if applicable
:type poses_file: csv.writer
:return:
"""
pos = state.pose.position
position = Vector3(pos.x, pos.y, pos.z)
if self.starting_time is None:
self.starting_time = time
self.last_update = time
self.last_position = position
if poses_file:
age = world.age()
poses_file.writerow([
self.robot.id, age.sec, age.nsec, position.x, position.y,
position.z,
self.get_battery_level()
])
if float(self.age()) < self.warmup_time:
# Don't update position values within the warmup time
self.last_position = position
self.last_update = time
return
# Calculate the distance the robot has covered as the Euclidean
# distance over the x and y coordinates (we don't care for flying),
# as well as the time it took to cover this distance.
last = self.last_position
ds = np.sqrt((position.x - last.x)**2 + (position.y - last.y)**2)
dt = float(time - self.last_update)
# Velocity is of course sum(distance) / sum(time)
# Storing all separate distance and time values allows us to
# efficiently calculate the new speed over the window without
# having to sum the entire arrays each time, by subtracting
# the values we're about to remove from the _dist / _time values.
self._dist += ds
self._time += dt
if len(self._dt) >= self.speed_window:
# Subtract oldest values if we're about to override it
self._dist -= self._ds[-1]
self._time -= self._dt[-1]
self.last_position = position
self.last_update = time
self._positions.append(position)
self._times.append(time)
self._ds.append(ds)
self._dt.append(dt)
async def run():
"""
The main coroutine, which is started below.
"""
arguments = parser.parse_args()
if arguments.robot_name is not None:
# this split will give errors on windows
robot_file_path = "experiments/bodies/" + arguments.robot_name + ".yaml"
if arguments.iterations is not None:
# this split will give errors on windows
iterations = int(arguments.iterations)
#robot_file_path = "experiments/examples/yaml/spider.yaml"
# Parse command line / file input arguments
settings = parser.parse_args()
# Start Simulator
if settings.simulator_cmd != 'debug':
simulator_supervisor = DynamicSimSupervisor(
world_file=settings.world,
simulator_cmd=settings.simulator_cmd,
simulator_args=["--verbose"],
plugins_dir_path=os.path.join('.', 'build', 'lib'),
models_dir_path=os.path.join('.', 'models'),
simulator_name='gazebo')
await simulator_supervisor.launch_simulator(port=settings.port_start)
await asyncio.sleep(0.1)
# Load a robot from yaml
robot = revolve_bot.RevolveBot()
robot.load_file(robot_file_path)
robot.update_substrate()
# robot._brain = BrainRLPowerSplines()
# Connect to the simulator and pause
connection = await World.create(settings,
world_address=('127.0.0.1',
settings.port_start))
await asyncio.sleep(1)
# Starts the simulation
await connection.pause(False)
# Insert the robot in the simulator
#life_timeout = Fixed time a robot stays in the simulator
robot_manager = await connection.insert_robot(
robot, Vector3(0, 0, settings.z_start))
robot_fitness = []
for x in range(1, iterations):
# Print robot fitness every second
robot_fitness.append(
fitness.displacement_velocity(robot_manager, robot))
await asyncio.sleep(0.1)
print("**The Robot Fitness is: ",
str(sum(robot_fitness) / len(robot_fitness)))
def random_spawn_pos():
return Vector3(random.uniform(-LIMIT_X, LIMIT_X),
random.uniform(-LIMIT_Y, LIMIT_Y), Z_SPAWN_DISTANCE)
def random_uniform_unit_vec():
return Vector3(random.uniform(-1, 1), random.uniform(-1, 1),
Z_SPAWN_DISTANCE).normalized()
