def show_multiple(costmaps,
original_costmaps,
workspace,
show_result,
weights=None,
centers=None,
starts=None,
targets=None,
paths=None,
ex_paths=None,
title=None,
directory=None):
""" Show multiple maps with demonstrations and example paths
plot all demonstrations and example paths of one iteration in one map
"""
pixel_map = workspace.pixel_map(original_costmaps[0].shape[0])
cols = max(math.ceil(math.sqrt(len(costmaps))), len(original_costmaps) + 1)
rows = math.ceil(len(costmaps) / cols) + 1
viewer = render.WorkspaceDrawer(workspace,
wait_for_keyboard=True,
rows=rows,
cols=cols,
scale=rows / (rows * cols))
for i in range(cols):
viewer.set_drawing_axis(i)
if i < len(original_costmaps):
viewer.draw_ws_img(original_costmaps[i], interpolate="none")
# viewer._ax.set_title('Training Costmap', size=32 / cols)
viewer.remove_axis()
for i in range(cols, rows * cols):
viewer.set_drawing_axis(i)
if i < len(costmaps) + cols:
viewer.draw_ws_img(costmaps[i - cols], interpolate="none")
# viewer._ax.set_title('Learned Costmap: \n {}.'.format(i + 1 - cols),
# size=32 / cols)
if weights is not None:
show_weights(viewer, weights[i - cols], centers)
if paths is not None:
show_paths(paths, pixel_map, starts, targets, viewer)
if ex_paths is not None:
show_paths(ex_paths[i - cols], pixel_map, starts, targets,
viewer)
viewer.remove_axis()
# viewer._colorbar.ax.tick_params(labelsize=FONTSIZE)
viewer._fig.tight_layout()
if title is not None:
viewer.set_title('\n'.join(wrap(title, 60)), fontsize=32)
if show_result == 'SHOW':
viewer.show_once()
elif show_result == 'SAVE':
viewer.save_figure(directory)
def show_predictions(costmap,
original_costmap,
workspace,
show_result,
starts=None,
targets=None,
paths=None,
ex_paths=None,
title=None,
directory=None):
""" Show multiple maps with demonstrations and example paths """
pixel_map = workspace.pixel_map(original_costmap.shape[0])
cols = min(5, len(ex_paths))
rows = math.ceil(len(ex_paths) / 5) + 1
viewer = render.WorkspaceDrawer(workspace,
wait_for_keyboard=True,
rows=rows,
cols=cols,
scale=rows / (rows * cols))
viewer.draw_ws_img(original_costmap, interpolate="none")
# viewer._ax.set_title('Training Costmap', size=32 / cols)
for i in range(cols):
viewer.set_drawing_axis(i)
viewer.remove_axis()
for i in range(cols, rows * cols):
viewer.set_drawing_axis(i)
if i < len(ex_paths) + cols:
viewer.draw_ws_img(costmap, interpolate="none")
if paths is not None:
show_paths([paths[i - cols]], pixel_map, [starts[i - cols]],
[targets[i - cols]], viewer)
if ex_paths is not None:
show_paths([ex_paths[i - cols]], pixel_map, [starts[i - cols]],
[targets[i - cols]], viewer)
viewer.remove_axis()
viewer._fig.tight_layout()
if title is not None:
viewer.set_title('\n'.join(wrap(title, 60)), fontsize=32)
#viewer._colorbar.ax.tick_params(labelsize=FONTSIZE)
if show_result == 'SHOW':
viewer.show_once()
elif show_result == 'SAVE':
viewer.save_figure(directory + '.pdf')
def show(costmap,
workspace,
show_result,
starts=None,
targets=None,
paths=None,
ex_paths=None,
weights=None,
d=None,
predecessors=None,
title=None,
directory=None,
centers=None):
""" Show a single map with demonstrations and example paths
"""
viewer = render.WorkspaceDrawer(workspace, wait_for_keyboard=True)
viewer.draw_ws_img(costmap, interpolate="none")
viewer._colorbar.ax.tick_params(labelsize=FONTSIZE)
pixel_map = workspace.pixel_map(costmap.shape[0])
if paths is not None:
show_paths(paths, pixel_map, starts, targets, viewer)
if ex_paths is not None:
show_paths(ex_paths, pixel_map, starts, targets, viewer)
if weights is not None:
show_weights(viewer, weights, centers)
if d is not None:
show_D(d, pixel_map, viewer)
if predecessors is not None:
show_policy(pixel_map, predecessors, viewer)
viewer.remove_axis()
if title is not None:
viewer.set_title('\n'.join(wrap(title, 60)))
if show_result == 'SHOW':
viewer.show_once()
elif show_result == 'SAVE':
viewer.save_figure(directory)
for i in range(10):
s_w = sample_collision_free(workspace)
t_w = sample_collision_free(workspace)
s = pixel_map.world_to_grid(s_w)
t = pixel_map.world_to_grid(t_w)
try:
print("planning...")
time_0 = time.time()
path = converter.dijkstra_on_map(costmap, s[0], s[1], t[0], t[1])
except Exception as e:
continue
paths.append(path)
starts.append(s_w)
targets.append(t_w)
print("took t : {} sec.".format(time.time() - time_0))
if show_result:
viewer = render.WorkspaceDrawer(workspace, wait_for_keyboard=True)
viewer.draw_ws_img(costmap, interpolate="none")
for s_w, t_w, path in zip(starts, targets, paths):
trajectory = [None] * len(path)
for i, p in enumerate(path):
trajectory[i] = pixel_map.grid_to_world(np.array(p))
c = cmap(np.random.rand())
viewer.draw_ws_line(trajectory, color=c)
viewer.draw_ws_point(s_w, color=c)
viewer.draw_ws_point(t_w, color=c)
viewer.show_once()
print(path)
def show_esf_maps(costmaps,
original_costmap,
workspace,
show_result,
esf,
loss_aug_maps,
loss_aug_esf,
weights=None,
centers=None,
starts=None,
targets=None,
paths=None,
ex_paths=None,
title=None,
directory=None):
""" show the costmap with its log augmentation as well as the corresponding
expected state frequencies
"""
pixel_map = workspace.pixel_map(original_costmap.shape[0])
cols = 4
rows = len(costmaps) + 1
viewer = render.WorkspaceDrawer(workspace,
wait_for_keyboard=True,
rows=rows,
cols=cols,
scale=rows / (rows * cols))
viewer.draw_ws_img(original_costmap, interpolate="none")
viewer._ax.set_title('Training Costmap', size=32 / cols)
for i in range(cols):
viewer.set_drawing_axis(i)
viewer.remove_axis()
for i in range(1, rows):
viewer.set_drawing_axis(i * cols)
viewer.remove_axis()
viewer.draw_ws_img(costmaps[i - 1], interpolate="none")
viewer._ax.set_title('learned costmap: \n {}.'.format(i),
size=32 / cols)
if weights is not None and centers is not None:
show_weights(viewer, weights[i - 1], centers)
viewer.set_drawing_axis(i * cols + 1)
viewer.remove_axis()
viewer.draw_ws_img(loss_aug_maps[i - 1], interpolate="none")
viewer._ax.set_title('loss augmented costmap: \n {}.'.format(i),
size=32 / cols)
viewer.set_drawing_axis(i * cols + 2)
viewer.remove_axis()
viewer.draw_ws_img(esf[i - 1], interpolate="none")
viewer._ax.set_title('occupancy: \n {}.'.format(i), size=32 / cols)
if paths is not None:
show_paths(paths, pixel_map, starts, targets, viewer)
if ex_paths is not None:
show_paths(ex_paths[i - 1], pixel_map, starts, targets, viewer)
if weights is not None and centers is not None:
show_weights(viewer, weights[i - 1], centers)
viewer.set_drawing_axis(i * cols + 3)
viewer.remove_axis()
viewer.draw_ws_img(loss_aug_esf[i - 1], interpolate="none")
viewer._ax.set_title('loss augmented occupancy: \n {}.'.format(i),
size=32 / cols)
if paths is not None:
show_paths(paths, pixel_map, starts, targets, viewer)
if ex_paths is not None:
show_paths(ex_paths[i - 1], pixel_map, starts, targets, viewer)
viewer._fig.tight_layout()
if title is not None:
viewer.set_title('\n'.join(wrap(title, 60)), fontsize=32)
if show_result == 'SHOW':
viewer.show_once()
elif show_result == 'SAVE':
viewer.save_figure(directory)
def show_iteration(costmaps,
original_costmaps,
workspace,
show_result,
weights=None,
starts=None,
targets=None,
paths=None,
centers=None,
ex_paths=None,
title=None,
directory=None):
""" Show multiple maps with demonstrations and example paths
One row for each iteration
One column for each demonstrations with its corresponding example path
"""
pixel_map = workspace.pixel_map(costmaps[0].shape[0])
rows = len(costmaps) + 1
cols = max(len(paths), len(original_costmaps))
viewer = render.WorkspaceDrawer(workspace,
wait_for_keyboard=True,
rows=rows,
cols=cols,
scale=rows / (rows * cols))
for i in range(cols):
viewer.set_drawing_axis(i)
if i < len(original_costmaps):
viewer.draw_ws_img(original_costmaps[i], interpolate="none")
viewer._ax.set_title('Training Costmap', size=32 / cols)
viewer.remove_axis()
for i in range(rows - 1):
for j in range(cols):
viewer.set_drawing_axis((i + 1) * cols + j)
viewer.draw_ws_img(costmaps[i], interpolate="none")
if weights is not None:
show_weights(viewer, weights[i], centers)
if paths is not None:
print(len(paths))
print(len(starts))
show_paths([paths[j]], pixel_map, [starts[j]], [targets[j]],
viewer)
if ex_paths is not None:
show_paths([ex_paths[i][j]], pixel_map, [starts[j]],
[targets[j]], viewer)
viewer._ax.set_title('Learned Costmap: \n {}. Iteration \n'
'{}. path'.format((i + 1), j + 1),
size=32 / cols)
viewer.remove_axis()
viewer._fig.tight_layout()
if title is not None:
viewer._fig.suptitle(title, fontsize=15)
if show_result == 'SHOW':
viewer.show_once()
elif show_result == 'SAVE':
viewer.save_figure(directory)