コード例 #1
0
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()
コード例 #2
0
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()
コード例 #3
0
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()