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()
k = 0
for i, j in itertools.product(range(nb_points), range(nb_points)):
X_data[k] = grid.grid_to_world(np.array([i, j]))
Y_data[k] = value[i, j]
k += 1
f = LWR(1, 2)
f.X = [X_data]
f.Y = [Y_data]
f.D = [8 * np.eye(2)]
f.ridge_lambda = [.1, .1]
else:
# Regress using cubic-splines
f = RegressedPixelGridSpline(value, grid.resolution, grid.extent)
print("calculate gradient...")
X, Y = workspace.box.meshgrid(15)
U, V = np.zeros(X.shape), np.zeros(Y.shape)
for i, j in itertools.product(range(X.shape[0]), range(X.shape[1])):
p = np.array([X[i, j], Y[i, j]])
g = f.gradient(p)
g /= np.linalg.norm(g)
U[i, j] = g[0]
V[i, j] = g[1]
renderer.set_drawing_axis(i)
renderer.draw_ws_obstacles()
renderer.draw_ws_point([0, 0], color='r', shape='o')
renderer.draw_ws_img(value, interpolate="none", color_style=plt.cm.Blues)
renderer._ax.quiver(X, Y, U, V, units='width', color='k')
renderer.show()
env = EnvBox(dim=np.array([2., 2.])) box = Box(origin=np.array([-.2, -.2])) segment = Segment(origin=np.array([.4, -.1]), orientation=0.2) circle = Circle(origin=np.array([.5, .5]), radius=0.2) workspace = Workspace(env) workspace.obstacles.append(box) workspace.obstacles.append(segment) workspace.obstacles.append(circle) # Compute Occupancy map and SDF nb_points = 20 occupancy_map = occupancy_map(nb_points, workspace) signed_distance_field = sdf(occupancy_map) # Setup viewer viewer = WorkspaceDrawer(workspace, wait_for_keyboard=True) viewer.draw_ws_img(signed_distance_field) # viewer.draw_ws_img(occupancy_map) # import cv2 # Draw blured image # viewer.draw_ws_img( # ndimage.gaussian_filter( # cv2.resize(src=signed_distance_field, # dsize=(300, 300), # interpolation=cv2.INTER_NEAREST), sigma=3)) viewer.draw_ws_obstacles() viewer.show_once()
hd.TIME_STEP = 1e-5
# hd.ALGORITHM = "crank-nicholson"
hd.ALGORITHM = "euler"
matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
style = plt.cm.tab20c
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=style)
renderer.show()
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()
# PERFORMANCE OF THIS SOFTWARE.
#
# Jim Mainprice on Wed January 22 2019
from demos_common_imports import *
from pyrieef.geometry.workspace import *
from pyrieef.geometry import heat_diffusion
from pyrieef.rendering.workspace_planar import WorkspaceDrawer
import matplotlib.pyplot as plt
ROWS = 1
COLS = 2
heat_diffusion.NB_POINTS = 101
heat_diffusion.TIME_FACTOR = 50
heat_diffusion.ALGORITHM = "forward"
iterations = 10
workspace = Workspace()
source = [0, 0]
renderer = WorkspaceDrawer(workspace, rows=ROWS, cols=COLS)
U = heat_diffusion.heat_diffusion(workspace, source, iterations)
U_e = heat_diffusion.compare_with_kernel(U[-1], 9.020E-03, workspace)
for i in range(2):
renderer.set_drawing_axis(i)
renderer.draw_ws_obstacles()
renderer.draw_ws_point(source, color='k', shape='o')
renderer.background_matrix_eval = False
renderer.draw_ws_img(
U[-1] if i == 0 else U_e,
interpolate="none", color_style=plt.cm.gray)
renderer.show()