class PrintStatus(LivePlugin):
def __init__(self, interval_print):
self.tracker = InAWhile(interval_print)
def init(self, data):
pass
def starting_stream(self, stream):
""" A new stream is being read """
self.cur_stream_observations = 0
self.stream = stream
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 __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
class Bookkeeping:
""" Simple class to keep track of how many we have to simulate. """
def __init__(self, data_central, id_robot, 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()
if log_index.has_streams_for_robot(id_robot):
self.done_before = log_index.get_episodes_for_robot(id_robot)
self.num_episodes_done_before = len(self.done_before)
else:
self.done_before = set()
self.num_episodes_done_before = 0
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 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 get_id_episodes(self):
""" Returns the list of episodes simulated. """
return natsorted(self.id_episodes)
def get_all_episodes(self):
""" Returns the list of all episodes, both the already present
and the simulated. """
eps = []
eps.extend(self.id_episodes)
eps.extend(self.done_before)
return natsorted(set(eps))
def episode_done(self):
self.num_episodes_done += 1
self.observations_per_episode.append(self.num_observations_episode)
self.num_observations_episode = 0
try:
from compmake import progress
progress("Simulating episodes", (self.num_episodes_done, self.num_episodes_todo))
except ImportError:
pass
def another_episode_todo(self):
return self.num_episodes_done < self.num_episodes_todo
def __init__(self, interval_print):
self.tracker = InAWhile(interval_print)
class BookkeepingServo():
''' Simple class to keep track of how many we have to simulate. '''
@contract(interval_print='None|>=0')
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 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 get_id_episodes(self):
''' Returns the list of episodes simulated. '''
return natsorted(self.id_episodes)
def get_all_episodes(self):
''' Returns the list of all episodes, both the already present
and the simulated. '''
eps = []
eps.extend(self.id_episodes)
eps.extend(self.done_before)
return natsorted(set(eps))
def episode_done(self):
self.num_episodes_done += 1
self.observations_per_episode.append(self.num_observations_episode)
self.num_observations_episode = 0
try:
from compmake import progress
progress('servoing', (self.num_episodes_done,
self.num_episodes_todo))
except ImportError:
pass
def another_episode_todo(self):
return self.num_episodes_done < self.num_episodes_todo
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)