def from_yaml(spec):
try:
if not isinstance(spec, dict):
raise ValueError('Expected a dict, got %s' % spec)
s = dict(**spec)
id_stream = s.pop('id', None)
desc = s.pop('desc', None)
required = ['shape', 'format', 'range']
for x in required:
if not x in s:
raise ValueError('Missing entry %r.' % x)
shape = s.pop('shape')
check('list[>0](int,>0)', shape) # XXX: slow
format = s.pop('format') # @ReservedAssignment
range = s.pop('range') # @ReservedAssignment
extra = s.pop('extra', {})
filtered = s.pop('filtered', None)
default = s.pop('default', None)
names = s.pop('names', None) # TODO: do something useful with this
if names:
extra['names'] = names
if s.keys():
logger.warning('While reading\n%s\nextra keys detected: %s' %
((spec), s.keys()))
streamels = streamels_from_spec(shape, format, range, default)
return StreamSpec(id_stream, streamels, extra, filtered, desc)
except:
logger.error('Error while parsing the StreamSpec:\n%s' % spec)
raise
def from_yaml(xo):
try:
if not isinstance(xo, dict):
raise ValueError('Expected a dict, got %s' % xo)
x = dict(**xo) # make a copy
check_contained('observations', x)
check_contained('commands', x)
observations = StreamSpec.from_yaml(x.pop('observations'))
commands = StreamSpec.from_yaml(x.pop('commands'))
extra = x.pop('extra', None)
desc = x.pop('desc', None)
id_robot = x.pop('id', None)
if x.keys():
logger.warning('While reading\n%s\nextra keys detected: %s' %
(pformat(xo), x.keys()))
return BootSpec(observations, commands,
id_robot=id_robot,
extra=extra,
desc=desc)
except:
logger.error('Error while parsing the BootSpec:\n%s' % xo)
raise
def index_robots(file2streams):
''' Groups the streams by robot, making sure the specs are compatible.
Returns dict: id_robot -> list of streams.
'''
robot2streams = defaultdict(lambda: [])
robot2spec = {}
for _, streams in file2streams.items():
for stream in streams:
id_robot = stream.get_id_robot()
if not id_robot in robot2spec:
robot2spec[id_robot] = stream.get_spec()
else:
# XXX:
stream_spec = stream.get_spec()
if str(stream_spec) != str(robot2spec[id_robot]):
msg = 'Warning! You got your logs mixed up. \n'
msg += ('Problem spec in:\n\t%s\nis\n\t%s\n' %
(stream, stream_spec))
msg += ('and this is different from:\n\t%s\n'
'found in e.g.,:\n\t%s' %
(robot2spec[id_robot], robot2streams[id_robot][0]))
msg += '\nI will skip this stream.'
logger.error(msg)
continue
robot2streams[id_robot].append(stream)
for robot in robot2streams:
robot2streams[robot] = sorted(robot2streams[robot],
key=lambda x: list(x.get_id_episodes())[0])
return dict(**robot2streams)
def compmake_job(self, *args, **kwargs):
""" Calls compmake's self.compmake_job() function. """
try:
from compmake import comp
except ImportError:
logger.error('Compmake not installed')
return comp(*args, **kwargs)
def hdf_list_streams(filename):
f = tables.openFile(filename)
try:
# TODO: check this has a valid format
group = f.root.boot_olympics.streams
ids = group._v_children.keys()
streams = []
for sid in ids:
table = group._v_children[sid].boot_stream
extra = group._v_children[sid].extra
spec = spec_from_group(group._v_children[sid])
id_episodes = set(np.unique(table[:]['id_episode']))
summaries = [episode_summary(table, extra, id_episode)
for id_episode in id_episodes]
topic = sid
id_robot = table[0]['id_robot']
filename = os.path.realpath(filename)
stream = BootStream(id_robot=id_robot,
filename=filename,
topic=topic, spec=spec,
summaries=summaries)
streams.append(stream)
return streams
except (ValueError, AttributeError):
logger.error('Error while trying to index %r.' % filename)
raise
finally:
f.close()
def cmd_list_states(data_central, argv):
'''Shows a summary of the states present in DB. '''
parser = OptionParser(prog='list-states',
usage=cmd_list_states.short_usage)
parser.disable_interspersed_args()
parser.add_option("-v", dest='verbose', default=False, action='store_true',
help="Show more verbose output.")
(options, args) = parser.parse_args(argv)
check_no_spurious(args)
db = data_central.get_agent_state_db()
combinations = list(db.list_states())
if not combinations:
logger.info('No learning states saved in DB.')
else:
logger.info('Found %d combinations in DB.' % len(combinations))
for id_agent, id_robot in combinations:
logger.info('- Found state a: %-35s r: %-25s' % (id_agent, id_robot))
if options.verbose:
try:
state = db.get_state(id_robot=id_robot, id_agent=id_agent)
logger.info(' # episodes: %s' % len(state.id_episodes))
logger.info(' object: %s' %
describe_value(state.agent_state))
except Exception as e:
logger.error(' (could not load state: %s) ' % e)
if not options.verbose:
logger.debug('Use -v for more information.')
def index_directory(directory, ignore_cache=False, warn_if_longer=3):
''' Returns a hash filename -> list of streams. '''
file2streams = {}
# logger.debug('Indexing directory %r (ignore cache: %s).' %
# (friendly_path(directory), ignore_cache))
with warn_long_time(warn_if_longer, 'indexing directory %r' %
friendly_path(directory)):
files = get_all_log_files(directory)
# Shuffle the list so that multiple threads will index different files
import random
random.seed()
random.shuffle(files)
with warn_long_time(warn_if_longer, 'indexing %d files (use cache: %s)' %
(len(files), not ignore_cache)):
for filename in files:
reader = LogsFormat.get_reader_for(filename)
try:
file2streams[filename] = \
reader.index_file_cached(filename, ignore_cache=ignore_cache)
for stream in file2streams[filename]:
assert isinstance(stream, BootStream)
if not file2streams[filename]:
logger.warning('No streams found in file %r.' %
friendly_path(filename))
except None: # XXX
logger.error('Invalid data in file %r.' % friendly_path(filename))
logger.error(traceback.format_exc())
return file2streams
def yaml_load(yaml_string):
try:
return load(yaml_string, Loader=Loader)
except KeyboardInterrupt:
raise
except:
logger.error('Could not deserialize YAML')
dump_emergency_string(yaml_string)
raise
def servo_stats_summary(data_central, id_agent, id_robot, id_episode): # @UnusedVariable
from geometry import (SE2, SE2_from_SE3, translation_from_SE2,
angle_from_SE2, SE3)
log_index = data_central.get_log_index()
errors = []
timestamps = []
poses = []
dist_xy = []
dist_th = []
for observations in \
log_index.read_robot_episode(id_robot, id_episode,
read_extra=True):
extra = observations['extra'].item()
servoing = extra.get('servoing', None)
if servoing is None:
logger.error('Warning, no "servoing" in episode %r.' %
id_episode)
break
obs0 = np.array(servoing['obs0'])
pose0 = SE2_from_SE3(SE3.from_yaml(servoing['pose0']))
pose1 = SE2_from_SE3(SE3.from_yaml(servoing['pose1']))
poseK = SE2_from_SE3(SE3.from_yaml(servoing['poseK']))
pose1 = SE2.multiply(SE2.inverse(pose0), pose1)
poseK = SE2.multiply(SE2.inverse(pose0), poseK)
poses.append(poseK)
dist_xy.append(np.linalg.norm(translation_from_SE2(poseK)))
dist_th.append(np.abs(angle_from_SE2(poseK)))
# obs1 = np.array(servoing['obs1'])
obsK = np.array(servoing['obsK'])
err_L2 = np.linalg.norm(obs0 - obsK)
errors.append(err_L2)
timestamps.append(observations['timestamp'])
# last['time_from_episode_start'] = observations['time_from_episode_start']
initial_distance = np.linalg.norm(translation_from_SE2(pose1))
summary = {}
summary['pose0'] = pose0
summary['pose1'] = pose1
summary['poses'] = poses
summary['errors'] = errors
summary['timestamps'] = timestamps
summary['initial_translation'] = translation_from_SE2(pose1)
summary['initial_distance'] = initial_distance
summary['initial_rotation'] = angle_from_SE2(pose1)
summary['dist_xy'] = dist_xy
summary['dist_th'] = dist_th
return summary
def write_stream(self, filename, id_stream, boot_spec):
writer = HDFLogWriter(filename, id_stream, boot_spec)
try:
yield writer
except:
logger.error("Got exception in the write_stream() context manager")
writer.cleanup()
raise
else:
writer.close()
def close(self):
if self.num == 0:
logger.error('No data given for writing; deleting tmp file.')
self.bag.close()
os.unlink(self.tmp_filename)
else:
self.bag.close()
if os.path.exists(self.filename):
os.unlink(self.filename)
os.rename(self.tmp_filename, self.filename)
def reload_state_for_agent(self, id_agent, id_robot, agent):
state = self.get_state(id_agent, id_robot)
logger.debug('State after learning %d episodes.' %
len(state.id_episodes))
try:
agent.set_state(state.agent_state)
except:
logger.error('Could not set agent to previous state.')
raise
return state
def get(self, key):
if not self.exists(key):
raise Exception('Could not find key %r.' % key)
filename = self.filename_for_key(key)
try:
with warn_long_time(self.warn_long_time, 'reading %r' % key):
return safe_pickle_load(filename)
except Exception as e:
msg = "Could not unpickle file %r: %s" % (filename, e)
logger.error(msg)
raise
def save_report(data_central, report, filename, resources_dir=None,
save_pickle=False, save_hdf=True,
check_hdf_written_correctly=True):
""" filename.html """
report.text('report_date', isodate()) # TODO: add other stuff
ds = data_central.get_dir_structure()
report.to_html(filename, resources_dir=resources_dir)
ds.file_is_done(filename)
if save_pickle:
pickle_name = os.path.splitext(filename)[0] + '.pickle'
with warn_long_time_writing(pickle_name):
safe_pickle_dump(report, pickle_name, protocol=2)
ds.file_is_done(pickle_name)
if save_hdf:
hdf_name = os.path.splitext(filename)[0] + '.rr1.h5'
with warn_long_time_writing(hdf_name):
report.to_hdf(hdf_name)
if check_hdf_written_correctly:
with warn_long_time_reading(hdf_name, logger=logger):
r2 = report_from_hdf(hdf_name)
if report != r2:
logger.error("Dont match")
logger.error(report.format_tree())
logger.error(r2.format_tree())
raise Exception()
ds.file_is_done(hdf_name)
def check_logs_formats(id_agent, agent, id_robot, robot): # @UnusedVariable
with create_tmp_dir() as root:
os.mkdir(os.path.join(root, 'config'))
data_central = DataCentral(root)
# Simulate two episodes
# NO! there is a bug in bag reading; the messages are read
# in timestamp order; and for now different episodes can
# have overlapping timestamps
try:
simulate(data_central, id_agent=id_agent, id_robot=id_robot,
max_episode_len=2,
num_episodes=1, # changed from 2 (see above)
cumulative=False,
id_episodes=None,
stateful=False,
interval_print=None,
write_extra=True)
except UnsupportedSpec:
return
log_index = data_central.get_log_index()
log_index.reindex()
streams = log_index.get_streams_for(id_robot, id_agent)
if len(streams) != 1:
msg = 'Expected to find 1 stream, not %d' % len(streams)
raise Exception(msg)
stream_orig = streams[0]
for logs_format, interface in LogsFormat.formats.items():
try:
dirname = os.path.join(root, logs_format)
safe_makedirs(dirname)
filename = os.path.join(dirname, 'example.%s' % logs_format)
written = []
id_stream = 'example'
with interface.write_stream(filename, id_stream,
robot.get_spec()) as writer:
for observations in stream_orig.read():
logger.info('Writing %s:%s (%s)' %
(observations['id_episode'],
observations['counter'],
observations['timestamp']))
writer.push_observations(observations)
written.append(observations)
count = 0
for obs_read in interface.read_from_stream(filename,
id_stream):
logger.info('Reading %s:%s (%s)' %
(obs_read['id_episode'],
obs_read['counter'],
obs_read['timestamp']))
original = written[count]
try:
if obs_read['counter'] != original['counter']:
msg = ('Not even the counter is the same!'
' %s vs %s' %
(obs_read['counter'], original['counter']))
raise Exception(msg)
assert_allclose(obs_read['timestamp'],
original['timestamp'])
assert_allclose(obs_read['observations'],
original['observations'])
assert_allclose(obs_read['commands'],
original['commands'])
except:
logger.error('Error at count = %d' % count)
logger.error(' original: %s' % original)
logger.error(' obs_read: %s' % obs_read)
raise
count += 1
if count != len(written):
msg = ('I wrote %d entries, but obtained %d.' %
(len(written), count))
raise Exception(msg)
except:
logger.error('Could not pass tests for format %r.'
% logs_format)
raise
def dump_emergency_string(s):
emergency = '/home/andrea/yaml_load.yaml' # XXX FIXME
with open(emergency, 'w') as f:
f.write(s)
logger.error('String written to %r.' % emergency)
def check_conversions(stream_spec1, nuisance):
# print('Checking %s / %s ' % (stream_spec1, nuisance))
nuisance_inv = None
try:
try:
stream_spec2 = nuisance.transform_spec(stream_spec1)
except UnsupportedSpec as e:
logger.info('Skipping %s/%s because incompatible: %s' %
(stream_spec1, nuisance, e))
return
value1 = stream_spec1.get_random_value()
stream_spec1.check_valid_value(value1)
value2 = nuisance.transform_value(value1)
stream_spec2.check_valid_value(value2)
try:
nuisance_inv = nuisance.inverse()
except NuisanceNotInvertible as e:
logger.info('Skipping some tests %s/%s because not invertible:'
' %s' % (stream_spec1, nuisance, e))
return
try:
stream_spec1b = nuisance_inv.transform_spec(stream_spec2)
except UnsupportedSpec as e:
msg = ('The inverse of the nuisance does not seem to be able '
'to handle the result:\n%s\n\n'
' stream_spec1: %s\n'
' nuisance: %s\n'
' stream_spec2: %s\n'
' nuisance_inv: %s\n' %
(indent(str(e), '>'),
stream_spec1.to_yaml(), nuisance,
stream_spec2.to_yaml(), nuisance_inv))
raise ValueError(msg)
try:
StreamSpec.check_same_spec(stream_spec1, stream_spec1b)
except Exception as e:
msg = ('The inverse of the nuisance does not recreate the '
'initial spec:\n%s\n\n'
' stream_spec1: %s\n'
' nuisance: %s\n'
' stream_spec2: %s\n'
' nuisance_inv: %s\n'
' stream_spec1b: %s\n' %
(indent(str(e), '>'),
stream_spec1.to_yaml(), nuisance,
stream_spec2.to_yaml(), nuisance_inv,
stream_spec1b.to_yaml()))
raise ValueError(msg)
value1b = nuisance_inv.transform_value(value2)
stream_spec1.check_valid_value(value1b)
# TODO: if exact
assert_allclose(value1, value1b, rtol=1e-5)
except:
logger.error('Error while testing:')
logger.error(' stream_spec: %s ' % stream_spec1.to_yaml())
logger.error(' nuisance: %s' % nuisance)
logger.error(' nuisance_inv: %s' % nuisance_inv)
raise
def batch_process_manager(data_central, which_sets, command=None):
try:
import compmake # @UnusedImport
except:
logger.error('Compmake not installed; multiprocessor '
'processes not available.')
raise
from compmake import (comp_prefix, use_filesystem,
compmake_console, batch_command)
batch_config = BatchConfigMaster()
configs = data_central.get_dir_structure().get_config_directories()
for config in configs:
batch_config.load(config)
sets_available = batch_config.sets.keys()
# logger.info('Available: %r' % sets_available)
# logger.info('Sets: %r' % which_sets)
which_sets_int = expand_string(which_sets, options=sets_available)
if not which_sets_int:
msg = 'Specified sets %r not found.' % which_sets
msg += ' Available: %s' % sets_available
raise UserError(msg)
# logger.info('Expanded: %r' % which_sets)
for x in which_sets_int:
if not x in sets_available:
msg = 'Set %r not available.' % x
raise UserError(msg)
if len(which_sets_int) == 1:
combid = which_sets[0]
else:
combid = '-'.join(which_sets)
# Create the new root
root = data_central.root
root_set = os.path.join(data_central.root, 'sets', combid)
safe_makedirs(root_set)
data_central_set = DataCentral(root_set)
# add symbolic links to logs and config
main_config = os.path.realpath(os.path.join(root, 'config'))
set_config = os.path.join(root_set, 'config')
safe_symlink(main_config, set_config)
safe_makedirs(os.path.join(root_set, 'logs'))
safe_symlink(os.path.join(root, 'logs'),
os.path.join(root_set, 'logs', 'original'))
storage = data_central_set.get_dir_structure().get_storage_dir()
compmake_storage = os.path.join(storage, 'compmake')
logger.debug('Using storage directory %r.' % friendly_path(compmake_storage))
use_filesystem(compmake_storage)
for id_set in which_sets:
if len(which_sets) > 1:
comp_prefix(id_set)
try:
spec = batch_config.sets[x]
batch_set(data_central_set, id_set, spec)
except ConfToolsException:
msg = ('Bad configuration for the set %r with spec\n %s' %
(id_set, pformat(spec)))
logger.error(msg)
raise
if command:
return batch_command(command)
else:
compmake_console()
return 0
def servonav_episode(
id_robot,
robot,
id_servo_agent,
servo_agent,
writer,
id_episode,
max_episode_len,
save_robot_state,
interval_write=1,
interval_print=5,
resolution=0.5, # grid resolution
delta_t_threshold=0.2, # when to switch
MIN_PATH_LENGTH=8,
MAX_TIME_FOR_SWITCH=20.0,
fail_if_not_working=False,
max_tries=10000,
):
"""
:arg:displacement: Time in seconds to displace the robot.
"""
from geometry import SE2_from_SE3, translation_from_SE2, angle_from_SE2, SE3
stats_write = InAWhile(interval_print)
# Access the vehicleSimulation interface
vsim = get_vsim_from_robot(robot)
for _ in xrange(max_tries):
# iterate until we can do this correctly
episode = robot.new_episode()
locations = get_grid(robot=robot, vsim=vsim, resolution=resolution)
if len(locations) < MIN_PATH_LENGTH:
logger.info("Path too short, trying again.")
else:
break
else:
msg = "Could not find path in %d tries." % max_tries
raise Exception(msg)
locations_yaml = convert_to_yaml(locations)
vsim.vehicle.set_pose(locations[0]["pose"])
current_goal = 1
current_goal_obs = locations[current_goal]["observations"]
servo_agent.set_goal_observations(current_goal_obs)
counter = 0
time_last_switch = None
num_written = 0
for robot_observations, boot_observations in run_simulation_servonav(
id_robot,
robot,
id_servo_agent,
servo_agent,
100000,
max_episode_len,
id_episode=id_episode,
id_environment=episode.id_environment,
raise_error_on_collision=fail_if_not_working,
):
current_time = boot_observations["timestamp"].item()
if time_last_switch is None:
time_last_switch = current_time
time_since_last_switch = float(current_time - time_last_switch)
def obs_distance(obs1, obs2):
return float(np.linalg.norm(obs1.flatten() - obs2.flatten()))
curr_pose = robot_observations.robot_pose
curr_obs = boot_observations["observations"]
curr_goal = locations[current_goal]["observations"]
prev_goal = locations[current_goal - 1]["observations"]
curr_err = obs_distance(curr_goal, curr_obs)
prev_err = obs_distance(prev_goal, curr_obs)
current_goal_pose = locations[current_goal]["pose"]
current_goal_obs = locations[current_goal]["observations"]
delta = SE2_from_SE3(SE3.multiply(SE3.inverse(curr_pose), current_goal_pose))
delta_t = np.linalg.norm(translation_from_SE2(delta))
delta_th = np.abs(angle_from_SE2(delta))
if stats_write.its_time():
msg = " deltaT: %.2fm deltaTh: %.1fdeg" % (delta_t, np.rad2deg(delta_th))
logger.debug(msg)
# If at the final goal, go closer
is_final_goal = current_goal == len(locations) - 1
if is_final_goal:
delta_t_threshold *= 0.3
# TODO: should we care also about delta_th?
time_to_switch = (delta_t < delta_t_threshold) or (time_since_last_switch > MAX_TIME_FOR_SWITCH)
# does not work: curr_err < SWITCH_THRESHOLD * prev_err:
if time_to_switch:
current_goal += 1
logger.info("Switched to goal %d." % current_goal)
time_last_switch = current_time
if current_goal >= len(locations):
# finished
logger.info("Finished :-)")
break
threshold_lost_m = 3
if delta_t > threshold_lost_m:
msg = "Breaking because too far away."
if not (fail_if_not_working):
logger.error(msg)
break
else:
raise Exception(msg)
servo_agent.set_goal_observations(current_goal_obs)
extra = {}
extra["servoing_base"] = dict(goal=curr_goal.tolist(), current=curr_obs.tolist())
extra["servoing_poses"] = dict(goal=SE3.to_yaml(current_goal_pose), current=SE3.to_yaml(curr_pose))
extra["servonav"] = dict(
poseK=SE3.to_yaml(curr_pose),
obsK=boot_observations["observations"].tolist(),
pose1=SE3.to_yaml(current_goal_pose),
locations=locations_yaml,
current_goal=current_goal,
curr_err=curr_err,
prev_err=prev_err,
time_last_switch=time_last_switch,
time_since_last_switch=time_since_last_switch,
)
if counter % interval_write == 0:
if save_robot_state:
extra["robot_state"] = robot.get_state()
writer.push_observations(observations=boot_observations, extra=extra)
num_written += 1
counter += 1
if num_written == 0:
msg = "This log was too short to be written (%d observations)" % counter
raise Exception(msg)
