def load_agent_state_core(data_central, id_agent, agent, id_robot,
reset_state=False,
raise_if_no_state=False):
db = data_central.get_agent_state_db()
key = dict(id_robot=id_robot, id_agent=id_agent)
if reset_state:
state = LearningState(id_robot=id_robot, id_agent=id_agent)
return agent, state
else:
if db.has_state(**key):
logger.info('Using previous learned state.')
state = db.reload_state_for_agent(id_agent=id_agent, id_robot=id_robot,
agent=agent)
return agent, state
else:
msg = 'No previous learned state found for %r/%r.' % (id_agent, id_robot)
if raise_if_no_state:
raise Exception(msg)
logger.info(msg)
state = LearningState(id_robot=id_robot, id_agent=id_agent)
return agent, state
assert False
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 __init__(self, data_central, id_robot, id_agent, num_episodes,
cumulative=True, interval_print=5):
self.data_central = data_central
self.id_robot = id_robot
self.cumulative = cumulative
if self.cumulative:
log_index = data_central.get_log_index()
self.done_before = log_index.get_episodes_for_robot(id_robot,
id_agent)
self.num_episodes_done_before = len(self.done_before)
self.num_episodes_todo = (num_episodes -
self.num_episodes_done_before)
logger.info('Preparing to do %d episodes (already done %d).' %
(self.num_episodes_todo,
self.num_episodes_done_before))
else:
self.num_episodes_todo = num_episodes
logger.info('Preparing to do %d episodes.' %
self.num_episodes_todo)
self.num_episodes_done = 0
self.num_observations = 0
self.num_observations_episode = 0
self.observations_per_episode = []
self.interval_print = interval_print
self.tracker = InAWhile(interval_print)
self.id_episodes = set()
try:
from compmake import progress
progress('Simulating episodes', (0, self.num_episodes_todo))
except ImportError:
pass
def task_nuislog(data_central, id_equiv, id_robot, with_extra=False):
log_index = data_central.get_log_index()
add_dummy_robots(data_central)
# First, look for all episodes of id_robot
streams = log_index.get_streams_for_robot(id_robot)
logger.info("Found %d streams for robot %r" % (len(streams), id_robot))
if len(streams) == 0:
msg = "No existing logs for %r" % id_robot
raise UserError(msg)
# Instance the equiv robot
config = data_central.get_bo_config()
equiv = config.robots.instance(id_equiv)
if not isinstance(equiv, EquivRobot):
msg = "I expect the robot %r to be an EquivRobot robot " "but found %s." % (id_equiv, describe_type(equiv))
raise UserError(msg)
assert isinstance(equiv, EquivRobot)
if equiv.inner_robot_name != id_robot:
msg = "The equiv robot inner robot is %r, but you requested %r" % (equiv.inner_robot_name, id_robot)
raise UserError(msg)
# FIXME: this is going to work only for 1 nuisance
for stream in streams:
logger.info("Converting %r" % stream)
nuislog_stream(data_central, stream, id_equiv, equiv, with_extra=with_extra)
def cmd_clean_states(data_central, argv):
''' Cleans agents states. '''
# TODO: to implement
parser = OptionParser(prog='clean-states',
usage=cmd_clean_states.short_usage)
parser.disable_interspersed_args()
parser.add_option("-a", "--agent", dest='agent', help="Agent ID")
parser.add_option("-r", "--robot", dest='robot', help="Robot ID")
(options, args) = parser.parse_args(argv)
check_no_spurious(args)
check_mandatory(options, ['agent', 'robot'])
id_agent = options.agent
id_robot = options.robot
db = data_central.get_agent_state_db()
# TODO: check we know agent and robot?
if not db.has_state(id_agent=id_agent, id_robot=id_robot):
logger.info('The state has already been cleaned.')
else:
logger.info('Cleaning state for %r/%r...' % (id_agent, id_robot))
db.delete_state(id_agent=id_agent, id_robot=id_robot)
def create_video(data_central, id_robot, id_agent,
model='boot_log2movie',
model_params={}, # {'plotter.zoom': 2},
suffix='',
id_episode=''):
logger.info('Creating video for robot: %r agent: %r episode %r' %
(id_robot, id_agent, id_episode))
ds = data_central.get_dir_structure()
basename = ds.get_video_basename(id_robot=id_robot,
id_agent=id_agent,
id_episode=id_episode)
if suffix:
basename += '-%s' % suffix
logdir = ds.get_simulated_logs_dir()
# TODO: check has_procgraph
import procgraph_vehicles # @UnusedImport
from procgraph import pg
# logger.info('Writing to %r.' % basename)
config = dict(logdir=logdir,
id_robot=id_robot,
id_agent=id_agent,
id_episode=id_episode,
output=basename,
**model_params)
pg(model, config=config, stats=False)
ds.file_is_done(basename, desc="")
def learn_log(data_central, id_agent, id_robot,
reset=False,
publish_interval=None,
publish_once=False,
interval_save=None,
interval_print=None,
episodes=None,
save_state=True,
parallel_hint=None,
live_plugins=[]):
''' If episodes is not None, it is a list of episodes id to learn. '''
from bootstrapping_olympics.library.live_plugins import CompmakeProgress
from bootstrapping_olympics.library.live_plugins import PrintStatus
logger.info('id_agent: %r\nepisodes:\n%r' % (id_agent, episodes))
bo_config = data_central.get_bo_config()
live_plugins = [bo_config.live_plugins.instance(x)
for x in live_plugins]
live_plugins.append(CompmakeProgress())
live_plugins.append(PrintStatus(interval_print))
# if publish_interval is not None:
warnings.warn('add publish_interval plugins')
# if publish_interval is not None:
# if 0 == state.num_observations % publish_interval:
# phase = 'learn-active'
# filename = ds.get_report_filename(id_agent=id_agent, id_robot=id_robot,
# id_state=state.id_state, phase=phase)
#
# publish_agent_output(data_central, state, agent, '%05d' % state.num_observations,
# filename)
agent0, state0 = load_agent_state(data_central, id_agent=id_agent,
id_robot=id_robot, reset_state=reset)
if parallel_hint is not None:
logger.info('setting parallel hint: %r' % str(parallel_hint))
agent0.parallel_process_hint(*parallel_hint)
agent, state = learn_log_base(data_central, id_agent, (agent0, state0),
id_robot, episodes,
live_plugins=[])
# Saving agent state
if save_state:
logger.debug('Saving state (end of streams)')
state.agent_state = agent.get_state()
# ds = data_central.get_dir_structure()
db = data_central.get_agent_state_db()
db.set_state(state=state, id_robot=id_robot, id_agent=id_agent)
return agent, state
def file_is_done(self, filename_or_basename, desc=None): # @UnusedVariable
"""
Notifies that some file is done writing.
Used to create a list of recent files that are done.
"""
path = friendly_path(filename_or_basename)
if os.path.exists(filename_or_basename):
size = friendly_filesize(filename_or_basename)
logger.info('Written %r (%r)' % (path, size))
else:
logger.info('Written %r' % (path))
def main(self, agents, robots,
explore=None,
explore_modulus=None,
servo=None,
servonav=None,
predict=None):
if predict is None:
logger.info('No prediction specified.')
if not robots:
raise SemanticMistake('Please specify at least one robot.')
if not agents:
raise SemanticMistake('Please specify at least one agent.')
for id_robot in robots:
self.add_task_robot_report(id_robot=id_robot)
if explore_modulus is not None:
self.add_tasks_explore_modulus(robots, **explore_modulus)
num_ep_expl = explore_modulus['num_episodes']
explorer = explore_modulus['explorer']
# TODO: validate inputs
if explore is not None:
for id_robot in robots:
self.add_tasks_explore(id_robot=id_robot, **explore)
num_ep_expl = explore['num_episodes']
explorer = explore['explorer']
for id_robot, id_agent in itertools.product(robots, agents):
compatible, reason = self.are_compatible(id_robot, id_agent)
if not compatible:
logger.info('Avoiding combination %s / %s: %s' %
(id_robot, id_agent, reason))
continue
# FIXME: num_ep_expl (should work also for logs)
self.add_learning(id_robot, id_agent, num_ep_expl=num_ep_expl,
explorer=explorer, # XXX
publish_progress=False,
save_pickle=True) # TODO: make param
if servo is not None:
self.add_tasks_servo(id_agent=id_agent, id_robot=id_robot,
**servo)
if servonav is not None:
self.add_tasks_servonav(id_agent=id_agent, id_robot=id_robot,
**servonav)
if predict is not None:
self.add_tasks_predict(id_agent=id_agent, id_robot=id_robot,
**predict)
def cmd_learn_log(data_central, argv):
'''
Runs the learning for a given agent and log.
Running a live plugin:
bom [agent/robot options] --reset --dontsave --plugin dummy
'''
parser = OptionParser(prog='learn-log', usage=cmd_learn_log.short_usage)
parser.disable_interspersed_args()
parser.add_option("-a", "--agent", dest='agent', help="Agent ID")
parser.add_option("-r", "--robot", dest='robot', help="Robot ID")
parser.add_option("--reset", default=False, action='store_true',
help="Do not use cached state.")
parser.add_option("-p", "--publish", dest='publish_interval', type='float',
default=None,
help="Publish debug information every N cycles.")
parser.add_option("--once", default=False, action='store_true',
help="Just plot the published information and exit.")
parser.add_option("--interval_save", type='int', default=300,
help="Interval for saving state (seconds) [%default]")
parser.add_option("--interval_print", type='int', default=5,
help="Interval for printing stats (seconds) [%default]")
parser.add_option("--dontsave", default=False, action='store_true',
help="Do not save the state of the agent.")
parser.add_option("--plugin", default=[],
action="append", type="string",
help="Run the specified plugin model during "
"learning. (eg, visualization)")
(options, args) = parser.parse_args(argv)
check_no_spurious(args)
check_mandatory(options, ['agent', 'robot'])
if options.publish_interval is None and not options.once:
msg = 'Not creating any report; pass -p <interval> or --once to do it.'
logger.info(msg)
learn_log(data_central=data_central,
id_agent=options.agent,
id_robot=options.robot,
reset=options.reset,
publish_interval=options.publish_interval,
publish_once=options.once,
interval_save=options.interval_save,
interval_print=options.interval_print,
save_state=not(options.dontsave),
live_plugins=options.plugin)
def cmd_list_agents(data_central, argv):
'''Shows a summary of the agents in the configuration. '''
parser = OptionParser(prog='list-agents',
usage=cmd_list_agents.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)
bo_config = data_central.get_bo_config()
agents = bo_config.agents
which = bo_config.agents.keys() # TODO: selection
max_len = max(len(x) for x in which)
formats = '%%%ds: %%s' % (max_len + 1)
logger.info('I know %d agents:' % len(agents))
for id_agent in which:
agent_spec = agents[id_agent]
logger.info(formats % (id_agent, agent_spec['desc']))
if options.verbose:
for id_agent in which:
agent_spec = agents[id_agent]
logger.info(pformat(agent_spec))
else:
logger.info('Use "-v" to see more information.')
def get_bo_config(self):
if self.bo_config is None:
self.bo_config = get_boot_config()
dirs = self.dir_structure.get_config_directories()
for dirname in dirs:
if not os.path.exists(dirname):
msg = ('Warning, the config dir %r does not exist ' %
friendly_path(dirname))
logger.info(msg)
else:
GlobalConfig.global_load_dir(dirname)
# self.bo_config.load(dirname)
return self.bo_config
def observations(self, observations):
self.id_episodes.add(observations['id_episode'].item())
self.num_observations_episode += 1
self.num_observations += 1
if self.tracker.its_time():
msg = ('simulating %d/%d episodes obs %d (%5.1f fps)' %
(self.num_episodes_done,
self.num_episodes_todo,
self.num_observations, self.tracker.fps()))
if self.num_episodes_done > 0:
msg += (' (mean obs/ep: %.1f)' %
(np.mean(self.observations_per_episode)))
logger.info(msg)
def update(self, update_data):
timestamp = update_data["obs"]["timestamp"]
for k, v in update_data.items():
if not self.model.is_valid_input_name(k):
if self.first_time:
logger.info("%r: not sending" % k)
else:
if self.first_time:
logger.info("%r: sending" % k)
self.model.from_outside_set_input(k, v, timestamp)
self.first_time = False
while self.model.has_more():
self.model.update()
def update(self, up):
self.cur_stream_observations += 1
progress = up['progress']
state = up['state']
if not self.tracker.its_time():
return
perc_obs = 100 * (float(progress.obs.done) / progress.obs.target)
perc_obs_log = 100 * (float(self.cur_stream_observations) / self.stream.get_num_observations())
msg = ('overall %.2f%% (log %3d%%) (eps: %4d/%d, obs: %4d/%d);'
' %5.1f fps' %
(perc_obs, perc_obs_log, len(state.id_episodes),
progress.eps.target, state.num_observations, progress.obs.target,
self.tracker.fps()))
logger.info(msg)
def learn_log_base(data_central, id_agent, agent_state, id_robot, episodes,
live_plugins=[], ignore_learned=False):
log_index = data_central.get_log_index()
agent, state = agent_state
if ignore_learned:
episodes_learned = set()
else:
episodes_learned = state.id_episodes
remain, progress = find_episodes_to_learn(log_index, id_robot,
episodes_to_learn=episodes,
episodes_learned=episodes_learned)
logger.info('Progress for %r %r:\n%s' % (id_robot, id_agent, progress.summary()))
# Initialize plugins
init = dict(data_central=data_central, id_agent=id_agent, id_robot=id_robot)
for plugin in live_plugins:
plugin.init(init)
for stream, to_learn in remain:
# plugins
for plugin in live_plugins:
plugin.starting_stream(stream)
for obs in stream.read(only_episodes=to_learn):
state.num_observations += 1
progress.obs.done += 1
try:
agent.process_observations(obs)
except PassiveAgentInterface.LearningConverged as e:
logger.info('Learning converged: %s' % e)
break
# Update plugins
up = dict(agent=agent, robot=None, obs=obs, progress=progress,
state=state, stream=stream)
for plugin in live_plugins:
plugin.update(up)
state.id_episodes.update(to_learn)
progress.eps.done += len(to_learn)
return agent, state
def bag_read(bag_file, topic, spec, substitute_id_episode, only_episodes=None):
'''
Reads the log in ROS bag file (only the
topic given) and converts it to the structure we use internally.
The ROS2Python object takes care to sanitize the logs;
it makes lots of checks for data meant to come directly from
real robots (so delay, wrong formats, etc. are expected.
'''
from ros import rosbag # @UnresolvedImport
bag = rosbag.Bag(bag_file)
hints = read_hints(bag_file)
warned = False
try:
ros2python = None
for msg in bag.read_messages(topics=[topic]):
topic, ros_obs, t = msg # @UnusedVariable
if spec is None:
spec = boot_spec_from_ros_message(ros_obs)
if ros2python is None:
bag_read_hints = hints.get('bag_read', {})
if bag_read_hints:
logger.info('bag_read using hints: %s' % bag_read_hints)
ros2python = ROS2Python(spec=spec, **bag_read_hints)
if only_episodes and not ros_obs.id_episode in only_episodes:
continue
if ros_obs.id_episode == 'id-episode-not-set':
if not warned:
warned = True
# XXX
logger.info('Changing id_episode to %s'
% substitute_id_episode)
ros_obs.id_episode = substitute_id_episode
obs = ros2python.convert(ros_obs)
if obs is not None:
yield obs
except:
bag.close()
raise
def check_conversions_upper_lower(stream_spec1, nuisance):
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
streamels = stream_spec1.get_streamels()
upper = streamels['upper']
lower = streamels['lower']
stream_spec1.check_valid_value(lower)
stream_spec1.check_valid_value(upper)
upper2 = nuisance.transform_value(upper)
lower2 = nuisance.transform_value(lower)
stream_spec2.check_valid_value(upper2)
stream_spec2.check_valid_value(lower2)
def load_agent_state(data_central, id_agent, id_robot,
reset_state=False,
raise_if_no_state=False):
'''
Load the agent, loading the agent state from the state_db directory.
If the state is not available, then it initializes anew.
Returns a tuple (agent, state).
'''
logger.info('Loading state %s/%s reset=%s ' % (id_agent,
id_robot, reset_state))
config = data_central.get_bo_config()
agent = config.agents.instance(id_agent) # @UndefinedVariable
index = data_central.get_log_index()
has_log = index.has_streams_for_robot(id_robot)
has_instance = id_robot in config.robots
if has_log:
boot_spec = index.get_robot_spec(id_robot)
elif has_instance:
robot = config.robots.instance(id_robot)
boot_spec = robot.get_spec()
else:
msg = 'Cannot load agent state for %r ' % id_agent
msg += 'because I cannot find any streams for robot %r ' % id_robot
msg += 'and I need them to find the BootSpec. '
msg += 'Also I could not instance the robot.'
msg += x_not_found('robot', id_robot, index.list_robots())
raise UserError(msg)
agent.init(boot_spec)
return load_agent_state_core(data_central, id_agent, agent, id_robot,
reset_state=reset_state,
raise_if_no_state=raise_if_no_state)
def write_robot(index, id_robot, output_dir, skip_existing=True):
streams = index.robots2streams[id_robot]
if not os.path.exists(output_dir):
os.makedirs(output_dir)
filename = os.path.join(output_dir, '%s.h5' % id_robot)
if skip_existing and os.path.exists(filename):
logger.info('Skipping %s' % filename)
return
id_stream = '%s_all' % id_robot
logger.info('Writing all to %r' % filename)
boot_spec = streams[0].get_spec()
logs_format = LogsFormat.get_reader_for(filename)
with logs_format.write_stream(filename=filename,
id_stream=id_stream,
boot_spec=boot_spec) as writer:
for stream in streams:
logger.info('- reading stream %s' % stream)
for observations in stream.read(read_extra=False):
writer.push_observations(observations)
def run_simulation_servonav(
id_robot,
robot,
id_agent,
agent,
max_observations,
max_time,
id_episode,
id_environment,
check_valid_values=True,
raise_error_on_collision=True,
):
"""
Runs an episode of the simulation. The agent should already been
init()ed.
yields robot.get_observations(), boot_observatoions
"""
keeper = ObsKeeper(boot_spec=robot.get_spec(), id_robot=id_robot)
obs_spec = robot.get_spec().get_observations()
cmd_spec = robot.get_spec().get_commands()
counter = 0
while True:
obs = robot.get_observations()
if check_valid_values:
obs_spec.check_valid_value(obs.observations)
# print('run_simulation_servonav: obs.obs %s' %
# (str(obs.observations.shape)))
observations = keeper.push(
timestamp=obs.timestamp,
observations=obs.observations,
commands=obs.commands,
commands_source=obs.commands_source,
id_episode=id_episode,
id_world=id_environment,
)
# print('then: obs.obs %s' %
# (str(observations['observations'].shape)))
episode_end = obs.episode_end
yield obs, observations
now = "step %s" % counter
if counter >= max_observations:
logger.info("Finished at %s because %s >= %s" % (now, counter, max_observations))
break
if observations["time_from_episode_start"] > max_time:
logger.info(
"Finished at %s because of max_time: %s > %s" % (now, observations["time_from_episode_start"], max_time)
)
break
if episode_end: # Fishy
msg = "Finished at %s because of robot driver." % now
if raise_error_on_collision:
raise Exception(msg)
else:
logger.info(msg)
break
agent.process_observations(observations)
commands = agent.choose_commands()
if check_valid_values:
cmd_spec.check_valid_value(commands)
robot.set_commands(commands, id_agent)
counter += 1
def boot_olympics_manager(arguments):
usage = substitute(usage_pattern, commands_list=commands_list,
cmd='boot_olympics_manager')
parser = OptionParser(prog='boot_olympics_manager', usage=usage)
parser.disable_interspersed_args()
parser.add_option("-d", dest='boot_root', default=None,
help='Root directory with logs, config, etc. [%default]')
parser.add_option("-c", dest='extra_conf_dirs', action="append", default=[],
help='Adds an extra config dir.')
parser.add_option("-l", dest='extra_log_dirs', action="append", default=[],
help='Adds an extra directory storing logs.')
parser.add_option("--contracts", default=False, action='store_true',
help="Activate PyContracts (disabled by default)")
parser.add_option("--seterr", dest='seterr', default="warn",
help="Sets np.seterr. "
"Possible values: ignore, warn, raise, print, log")
parser.add_option("--profile", default=False, action='store_true',
help="Use Python profiler")
available = LogsFormat.formats.keys()
parser.add_option("--logformat", dest='log_format',
default=BootOlympicsConstants.DEFAULT_LOG_FORMAT,
help="Choose format for writing logs in %s. [%%default]"
% str(available))
(options, args) = parser.parse_args(arguments)
if not args:
msg = ('Please supply command. Available: %s'
% ", ".join(Storage.commands.keys()))
raise UserError(msg)
cmd = args[0]
cmd_options = args[1:]
if not cmd in Storage.commands:
msg = ('Unknown command %r. Available: %s.' %
(cmd, ", ".join(Storage.commands.keys())))
raise UserError(msg)
np.seterr(all=options.seterr)
# underflow is very common in all libraries (e.g. matplotlib)
np.seterr(under='warn')
if not options.contracts:
contracts.disable_all()
if options.boot_root is None:
options.boot_root = DirectoryStructure.DEFAULT_ROOT
logger.info('Using %r as default root directory '
'(use -d <dir> to change)' % options.boot_root)
data_central = DataCentral(options.boot_root)
GlobalConfig.global_load_dir('default') # need skins
for dirname in options.extra_conf_dirs:
GlobalConfig.global_load_dir(dirname)
dir_structure = data_central.get_dir_structure()
dir_structure.set_log_format(options.log_format)
for dirname in options.extra_log_dirs:
dir_structure.add_log_directory(dirname)
def go():
return Storage.commands[cmd](data_central, cmd_options)
if not options.profile:
go()
else:
logger.warning('Note: the profiler does not work when using '
'parallel execution. (use "make" instead of "parmake").')
import cProfile
cProfile.runctx('go()', globals(), locals(), 'bom_prof')
import pstats
p = pstats.Stats('bom_prof')
p.sort_stats('cumulative').print_stats(30)
p.sort_stats('time').print_stats(30)
def servoing_episode(id_robot, robot,
id_servo_agent, servo_agent,
writer, id_episode,
displacement,
max_episode_len,
save_robot_state,
converged_dist_t_m=0.1,
converged_dist_th_deg=1,
max_tries=10000):
'''
:arg:displacement: Time in seconds to displace the robot.
'''
from geometry import SE3
def mean_observations(n=10):
obss = []
for _ in range(n): # XXX: fixed threshold
obss.append(robot.get_observations().observations)
return np.mean(obss, axis=0)
def robot_pose():
return robot.get_observations().robot_pose
def timestamp():
return robot.get_observations().timestamp
def episode_ended():
return robot.get_observations().episode_end
def simulate_hold(cmd0, displacement):
t0 = timestamp()
nsteps = 0
while timestamp() < t0 + displacement:
nsteps += 1
source = BootOlympicsConstants.CMD_SOURCE_SERVO_DISPLACEMENT
robot.set_commands(cmd0, source)
if episode_ended():
logger.debug('Collision after %d steps' % ntries)
return False
logger.debug('%d steps of simulation to displace by %s' %
(nsteps, displacement))
return True
for ntries in xrange(max_tries):
# iterate until we can do this correctly
episode = robot.new_episode()
obs0 = mean_observations()
cmd0 = robot.get_spec().get_commands().get_random_value()
pose0 = robot_pose()
ok = simulate_hold(cmd0, displacement)
if ok:
pose1 = robot_pose()
logger.info('Displacement after %s tries.' % ntries)
break
else:
msg = 'Could not do the displacement (%d tries).' % max_tries
raise Exception(msg)
servo_agent.set_goal_observations(obs0)
for robot_observations, boot_observations in \
run_simulation_servo(id_robot, robot,
id_servo_agent, servo_agent,
100000, max_episode_len,
id_episode=id_episode,
id_environment=episode.id_environment):
def pose_to_yaml(x):
''' Converts to yaml, or sets None. '''
if x is None:
return None
else:
return SE3.to_yaml(x)
extra = {}
sensels_list = boot_observations['observations'].tolist()
extra['servoing_base'] = dict(goal=obs0.tolist(), current=sensels_list)
current_pose = robot_observations.robot_pose
has_pose = current_pose is not None
if has_pose:
# Add extra pose information
extra['servoing_poses'] = dict(goal=pose_to_yaml(pose0),
current=pose_to_yaml(current_pose))
delta = SE2_from_SE3(SE3.multiply(SE3.inverse(current_pose),
pose0))
dist_t_m = np.linalg.norm(translation_from_SE2(delta))
dist_th_deg = np.abs(angle_from_SE2(delta))
# TODO: make it not overlapping
extra['servoing'] = dict(obs0=obs0.tolist(),
pose0=pose_to_yaml(pose0),
poseK=pose_to_yaml(current_pose),
obsK=sensels_list,
displacement=displacement,
cmd0=cmd0.tolist(),
pose1=pose_to_yaml(pose1))
if save_robot_state:
extra['robot_state'] = robot.get_state()
writer.push_observations(observations=boot_observations,
extra=extra)
if has_pose:
if ((dist_t_m <= converged_dist_t_m) and
(dist_th_deg <= converged_dist_th_deg)):
print('Converged!')
break
else:
# TODO: write convergence criterion
# without pose information
pass
def find_episodes_to_learn(log_index, id_robot, episodes_to_learn=None, episodes_learned=None):
"""
Returns tuples of the kind:
(id_stream, episodes_to_learn), progress
"""
logger.info('Finding episodes for %s' % id_robot)
logger.info('To learn: %s' % episodes_to_learn)
logger.info('Learned: %s' % episodes_learned)
if not log_index.has_streams_for_robot(id_robot):
msg = ('No log for robot %r found.\nI have logs for: %s.'
% (id_robot, ", ".join(log_index.robots2streams.keys())))
dirnames = log_index.get_indexed_dirs()
msg += '\nIndexed directories:\n'
msg += '\n'.join('- %s' % d for d in dirnames)
raise ValueError(msg)
if episodes_learned is None:
episodes_learned = set()
if episodes_to_learn is None:
episodes_to_learn = log_index.get_episodes_for_robot(id_robot)
p = Progress()
episodes_found = set()
remain = []
for stream in log_index.get_streams_for_robot(id_robot):
to_learn_in_stream = []
for episode_summary in stream.get_episodes():
id_episode = episode_summary.get_id_episode()
episodes_found.add(id_episode)
ep_length = episode_summary.get_length()
ep_nobs = episode_summary.get_num_observations()
p.len.total += ep_length
p.eps.total += 1
p.obs.total += ep_nobs
target = id_episode in episodes_to_learn
done = id_episode in episodes_learned
if target:
p.len.target += ep_length
p.eps.target += 1
p.obs.target += ep_nobs
if done:
p.len.done += ep_length
p.eps.done += 1
p.obs.done += ep_nobs
else:
p.len.todo += ep_length
p.eps.todo += 1
p.obs.todo += ep_nobs
if target and not done:
to_learn_in_stream.append(id_episode)
if to_learn_in_stream:
remain.append((stream, to_learn_in_stream))
# check to see that we found all episodes we needed to learn
for e in episodes_to_learn:
if not e in episodes_found:
msg = 'I could not find the episode %r.' % e
raise ValueError(msg)
return remain, p
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 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)
def task_servonav(
data_central,
id_agent,
id_robot,
max_episode_len,
num_episodes,
fail_if_not_working,
id_episodes=None, # if None, just use the ID given by the world
cumulative=False,
interval_print=None,
interval_write=10, # write every 10 frames
num_episodes_with_robot_state=0,
resolution=1,
):
""" Returns the list of the episodes IDs simulated. """
# Reseed the generator (otherwise multiprocessing will use the same)
np.random.seed()
if id_episodes is not None:
if len(id_episodes) != num_episodes:
raise ValueError("Expected correct number of IDs.")
# Instance robot object
robot = data_central.get_bo_config().robots.instance(id_robot)
# TODO: check that this is a Vehicles simulation
boot_spec = robot.get_spec()
# Instance agent object
agent, _ = load_agent_state(data_central, id_agent, id_robot, reset_state=False, raise_if_no_state=True)
# TODO: check servo
servo_agent = agent.get_servo()
id_agent_servo = "%s_servo" % id_agent
ds = data_central.get_dir_structure()
id_stream = "%s_%s_%s_servonav" % (id_robot, id_agent, unique_timestamp_string())
filename = ds.get_simlog_filename(id_robot=id_robot, id_agent=id_agent, id_stream=id_stream)
logger.info("Creating stream %r\n in file %r" % (id_stream, filename))
logs_format = LogsFormat.get_reader_for(filename)
bk = BookkeepingServo(
data_central=data_central,
id_robot=id_robot,
id_agent=id_agent_servo,
num_episodes=num_episodes,
cumulative=cumulative,
interval_print=interval_print,
)
if not bk.another_episode_todo():
return
with logs_format.write_stream(filename=filename, id_stream=id_stream, boot_spec=boot_spec) as writer:
counter = 0
while bk.another_episode_todo():
episode = robot.new_episode()
if id_episodes is not None:
id_episode = id_episodes.pop(0)
else:
id_episode = episode.id_episode
save_robot_state = counter < num_episodes_with_robot_state
servonav_episode(
id_robot=id_robot,
robot=robot,
id_servo_agent=id_agent_servo,
servo_agent=servo_agent,
writer=writer,
id_episode=id_episode,
resolution=resolution,
max_episode_len=max_episode_len,
save_robot_state=save_robot_state,
interval_write=interval_write,
fail_if_not_working=fail_if_not_working,
max_tries=10000,
)
bk.episode_done()
counter += 1
def add_tasks_explore_modulus(self, robots, episodes_per_tranche=50, **explore_args):
"""
Adds exploration tasks, making sure to use the same
data for robots up to nuisances.
"""
# ID robot -> (obsn, original, cmdn)
robot2canonical = {}
# (original, cmd) -> list(str)
from collections import defaultdict
core2robots = defaultdict(list)
for id_robot in robots:
canform = self.get_robot_modulus(id_robot)
robot2canonical[id_robot] = canform
_, original, cmdn = canform
core2robots[(original, cmdn)].append(id_robot)
logger.info('Canonical form of %r: %s' %
(id_robot, robot2canonical[id_robot]))
config = self.data_central.get_bo_config()
for original, cmd in core2robots:
derived = core2robots[(original, cmd)]
logger.info('Core robot (%s,%s) corresponds to %s' %
(original, cmd, derived))
# XXX: not sure of order
new_robot_name = "".join(['U%s' % x for x in cmd]) + original
if not new_robot_name in config.robots:
msg = 'Bug found, or bad naming practices.'
msg += 'I want to instantiate a %r but not found.' % new_robot_name
raise Exception(msg)
explore_args['episodes_per_tranche'] = episodes_per_tranche
id_episodes = self.add_tasks_explore(new_robot_name, **explore_args)
logger.info('Defined %d episodes for %r.' % (len(id_episodes), new_robot_name))
# Now convert one episode into the other
for id_derived in derived:
logger.info('Considering derived %s ' % id_derived)
derived_obs, x, derived_cmd = robot2canonical[id_derived]
assert x == original
assert derived_cmd == cmd
if not derived_obs:
logger.info(' ... skipping because pure.')
# this is a pure robot
continue
episodes_tranches = self.get_tranches(id_episodes,
episodes_per_tranche)
for i, tranche in enumerate(episodes_tranches):
job_id = 'derive-%s-%d' % (id_derived, i)
extra_dep = []
for id_episode in tranche:
extra_dep.append(self.dep_episode_done(new_robot_name,
id_episode))
job = self.compmake_job(nuislog_episodes,
data_central=self.data_central,
id_robot_original=new_robot_name,
id_episodes=tranche,
id_equiv=id_derived,
obs_nuisances=derived_obs,
cmd_nuisances=[], # <- correct
with_extras=True,
job_id=job_id,
extra_dep=extra_dep)
for id_episode in tranche:
self.set_dep_episode_done(id_robot=id_derived,
id_episode=id_episode, job=job)
def simulate_agent_robot(
data_central,
id_agent,
id_robot,
max_episode_len,
num_episodes,
cumulative,
id_episodes=None, # if None, just use the ID given by the world
stateful=False,
interval_print=None,
write_extra=True,
):
""" If not cumulative, returns the list of the episodes IDs simulated,
otherwise it returns all episodes. """
# Reseed the generator (otherwise multiprocessing will use the same)
np.random.seed()
if id_episodes is not None:
if len(id_episodes) != num_episodes:
raise ValueError("Expected correct number of IDs.")
# Instance agent object
config = data_central.get_bo_config()
agent = config.agents.instance(id_agent) # @UndefinedVariable
# Instance robot object
robot = config.robots.instance(id_robot) # @UndefinedVariable
# logger = logging.getLogger("BO:%s(%s)" % (id_agent, id_robot))
# logger.setLevel(logging.DEBUG)
# AgentInterface.logger = logger # XXX
boot_spec = robot.get_spec()
# If --stateful is passed, we try to load a previous state.
if stateful:
db = data_central.get_agent_state_db()
if db.has_state(id_agent=id_agent, id_robot=id_robot):
logger.info("Using previous state.")
db.reload_state_for_agent(id_agent=id_agent, id_robot=id_robot, agent=agent)
else:
logger.info("No previous state found.")
agent.init(boot_spec)
else:
agent.init(boot_spec)
ds = data_central.get_dir_structure()
timestamp = unique_timestamp_string()
timestamp = timestamp.replace("_", "")
id_stream = "%s-%s-%s" % (id_robot, id_agent, timestamp)
filename = ds.get_simlog_filename(id_robot=id_robot, id_agent=id_agent, id_stream=id_stream)
logger.info("Creating stream %r\n in file %r" % (id_stream, filename))
logs_format = LogsFormat.get_reader_for(filename)
bk = Bookkeeping(
data_central=data_central,
id_robot=id_robot,
num_episodes=num_episodes,
cumulative=cumulative,
interval_print=interval_print,
)
if bk.another_episode_todo():
with logs_format.write_stream(filename=filename, id_stream=id_stream, boot_spec=boot_spec) as writer:
while bk.another_episode_todo():
if id_episodes is not None:
id_episode = id_episodes.pop(0)
logger.info("Simulating episode %s" % id_episode)
else:
id_episode = None
for observations in run_simulation(
id_robot, robot, id_agent, agent, 100000, max_episode_len, id_episode=id_episode
):
bk.observations(observations)
if write_extra:
extra = dict(robot_state=robot.get_state())
else:
extra = {}
writer.push_observations(observations=observations, extra=extra)
bk.episode_done()
logger.info("Peacefully done all episodes")
else:
logger.warn("No episodes to do?")
logger.info("done")
if cumulative:
return bk.get_all_episodes()
else:
return bk.get_id_episodes()
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
