def plot_costmaps():
workspace = sample_circle_workspaces(nb_circles=4)
grid_sparse = workspace.box.stacked_meshgrid(24)
grid_dense = workspace.box.stacked_meshgrid(100)
extent = workspace.box.box_extent()
sdf = SignedDistanceWorkspaceMap(workspace)
viewer = WorkspaceDrawer(workspace,
wait_for_keyboard=True,
rows=1,
cols=2,
scale=1.)
viewer.set_drawing_axis(0)
viewer.set_workspace(workspace)
viewer.draw_ws_img(sdf(grid_dense).T)
viewer.draw_ws_obstacles()
viewer.set_drawing_axis(1)
viewer.set_workspace(workspace)
viewer.draw_ws_img(sdf(grid_sparse).T)
viewer.draw_ws_obstacles()
viewer.show_once()
hd.TIME_FACTOR = 2000
hd.TIME_STEP = 1e-5
hd.ALGORITHM = "forward"
matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
circles = []
circles.append(Circle(origin=[.1, .0], radius=0.1))
circles.append(Circle(origin=[.1, .25], radius=0.05))
circles.append(Circle(origin=[.2, .25], radius=0.05))
circles.append(Circle(origin=[.0, .25], radius=0.05))
workspace = Workspace()
workspace.obstacles = circles
renderer = WorkspaceDrawer(workspace, rows=ROWS, cols=COLS)
x_source = np.array([0.2, 0.15])
# ------------------------------------------------------------------------------
iterations = ROWS * COLS
U = hd.heat_diffusion(workspace, x_source, iterations)
for i in range(iterations):
renderer.set_drawing_axis(i)
renderer.draw_ws_obstacles()
renderer.draw_ws_point([x_source[0], x_source[1]], color='r', shape='o')
renderer.background_matrix_eval = False
renderer.draw_ws_img(U[i],
interpolate="bicubic",
color_style=plt.cm.tab20c)
renderer.show()
# circles.append(Circle(origin=[-2, -2], radius=0.1))
circles.append(Circle(origin=[.1, .25], radius=0.05))
circles.append(Circle(origin=[.2, .25], radius=0.05))
circles.append(Circle(origin=[.0, .25], radius=0.05))
circles.append(Circle(origin=[-.2, 0], radius=0.1))
workspace = Workspace()
workspace.obstacles = circles
X, Y = workspace.box.meshgrid(N)
occ = occupancy_map(N, workspace)
f = sdf(occ).T
U = -1 * np.gradient(f.T, axis=0).T
V = -1 * np.gradient(f.T, axis=1).T
phi = hd.distance_from_gradient(U, V, 1. / N, f)
phi -= phi.min() # set min to 0 for comparison
f -= f.min()
# d = np.linalg.norm(phi - f)
# print(d)
renderer = WorkspaceDrawer(workspace, rows=1, cols=2)
renderer.set_drawing_axis(0)
renderer.draw_ws_obstacles()
renderer.draw_ws_img(sdf(occupancy_map(100, workspace)),
interpolate="none", color_style=plt.cm.hsv)
renderer._ax.quiver(X, Y, U, V, units='width')
renderer._ax.set_title("original")
renderer.set_drawing_axis(1)
renderer.draw_ws_obstacles()
renderer.draw_ws_img(phi.T, interpolate="none", color_style=plt.cm.hsv)
renderer._ax.quiver(X, Y, U, V, units='width')
renderer._ax.set_title("recovered from vector field")
renderer.show()