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.extent_data() 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()
diagonal = workspace.box.diag() max_h = diagonal * boxes_height min_h = diagonal * boxes_height * .5 max_w = diagonal * boxes_width min_w = diagonal * boxes_width * .5 workspace.obstacles = [] while len(workspace.obstacles) < 5: origin = workspace.box.sample_uniform() h = (max_h - min_h) * np.random.rand() + min_h w = (max_w - min_w) * np.random.rand() + min_w if workspace.min_dist(origin)[0] < np.linalg.norm([h, w]): continue dimensions = np.array([w, h]) theta = np.pi * np.random.rand() - np.pi orientation = rotation_matrix_2d_radian(theta) workspace.obstacles.append(OrientedBox(origin, dimensions, orientation)) # Compute Occupancy map matrix = occupancy_map(150, workspace) # Compute Signed distance field # meshgrid = workspace.box.stacked_meshgrid(150) # matrix = SignedDistanceWorkspaceMap(workspace)(meshgrid).T # Setup viewer viewer = WorkspaceDrawer(workspace, wait_for_keyboard=True) viewer.draw_ws_img(matrix) viewer.draw_ws_obstacles() viewer.show_once()
vx = RegressedPixelGridSpline(U.T, grid_sparse.resolution, grid_sparse.extent) vy = RegressedPixelGridSpline(V.T, grid_sparse.resolution, grid_sparse.extent) for i, j in itertools.product(range(X.shape[0]), range(X.shape[1])): p = np.array([X[i, j], Y[i, j]]) vxx = vx.gradient(p)[0] vyy = vy.gradient(p)[1] div[i, j] = vxx + vyy for i in range(iterations): if ROWS * COLS == 1 and i < iterations - 1: continue print("plot..") p_source = grid_sparse.world_to_grid(x_source) p = grid_sparse.grid_to_world(p_source) phi = phi.T phi = hd.distance(U, V, div, 1. / N).T renderer.set_drawing_axis(i) renderer.draw_ws_obstacles() renderer.draw_ws_point(p, color='r', shape='o') renderer.background_matrix_eval = False renderer.draw_ws_img(phi, interpolate="bicubic", color_style=plt.cm.hsv) f = RegressedPixelGridSpline(phi, grid_sparse.resolution, grid_sparse.extent) for i, j in itertools.product(range(X.shape[0]), range(X.shape[1])): g = f.gradient(np.array([X[i, j], Y[i, j]])) g /= np.linalg.norm(g) U[i, j] = g[0] V[i, j] = g[1] renderer._ax.quiver(X, Y, U, V, units='width') renderer.show()