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 set_state(self, id_agent, id_robot, state):
''' Sets the learning state for the given combination. '''
key = tuple2key((id_agent, id_robot))
stats = self.storage.set(key, state)
msg = self.storage.stats_string(stats)
logger.debug(msg)
return stats
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 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 run_simulation_servo(id_robot, robot, id_agent, agent,
max_observations, max_time,
id_episode, id_environment,
check_valid_values=True):
''' Runs an episode of the simulation. The agent should already been
init()ed. '''
keeper = ObsKeeper(boot_spec=robot.get_spec(), id_robot=id_robot,
check_valid_values=check_valid_values)
obs_spec = robot.get_spec().get_observations()
cmd_spec = robot.get_spec().get_commands()
counter = 0
while counter < max_observations:
obs = robot.get_observations()
assert isinstance(obs, RobotObservations)
if check_valid_values:
obs_spec.check_valid_value(obs.observations)
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)
episode_end = obs.episode_end
yield obs, observations
if observations['time_from_episode_start'] > max_time:
logger.debug('Episode ended at %s for time limit %s > %s ' %
(counter, observations['time_from_episode_start'],
max_time))
break
if episode_end: # Fishy
logger.debug('Episode ended at %s due to obs.episode_end.'
% counter)
break
agent.process_observations(observations)
commands = agent.choose_commands() # repeated
if check_valid_values:
cmd_spec.check_valid_value(commands)
robot.set_commands(commands, id_agent)
counter += 1
def read_hints(filename, hints_filename="boot_log.hints.yaml"):
"""
Reads a yaml file in the same directory and returns a dictionary.
Returns {} if not hints file is found.
"""
dirname = os.path.dirname(filename)
hints = os.path.join(dirname, hints_filename)
if not os.path.exists(hints):
logger.debug("No hints file found %s" % hints)
return {}
res = yaml.load(open(hints))
logger.debug('Hints: %s' % res)
return res
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
def servo_stats_summaries(data_central, id_agent, id_robot, id_episodes=None):
log_index = data_central.get_log_index()
if id_episodes is None:
id_episodes = log_index.get_episodes_for_robot(id_robot,
id_agent + '_servo')
if not id_episodes:
msg = 'No episodes found for %s - %s' % (id_robot, id_agent)
raise Exception(msg)
summaries = []
for id_episode in id_episodes:
logger.debug('Reading %s...' % id_episode)
summary = servo_stats_summary(data_central, id_agent, id_robot, id_episode)
summaries.append(summary)
return summaries
def index_file_cached(self, filename, ignore_cache=False):
cache = '%s.index_cache' % filename
if os.path.exists(cache) and not ignore_cache: # TODO: mtime
try:
return safe_pickle_load(cache)
except Exception as e:
msg = 'Could not unpickle cache %r, deleting.' % friendly_path(cache)
msg += '\n%s' % e
logger.warning(msg)
try:
os.unlink(cache)
except:
pass
logger.debug('Indexing file %r' % friendly_path(filename))
res = self.index_file(filename)
for stream in res:
assert isinstance(stream, BootStream)
logger.debug('Now dumping file %r' % friendly_path(cache))
with warn_long_time(1, 'dumping %r' % friendly_path(cache)):
safe_pickle_dump(res, cache, protocol=2)
return res
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 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
