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()
hd.NB_POINTS = 101 hd.TIME_FACTOR = 200 hd.TIME_STEP = 2e-5 hd.ALGORITHM = "forward" hd.CONSTANT_SOURCE = True N = 40 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.21666667, 0.15]) # ------------------------------------------------------------------------------ # iterations = ROWS * COLS iterations = 9 u_t = hd.heat_diffusion(workspace, x_source, iterations) grid = workspace.pixel_map(hd.NB_POINTS) X, Y = workspace.box.meshgrid(N) U, V = np.zeros(X.shape), np.zeros(Y.shape) print(u_t[-1].shape) print(X.shape) print(Y.shape) phi = np.empty(X.shape) f = RegressedPixelGridSpline(u_t[-1], grid.resolution, grid.extent)
import numpy as np
from pyrieef.geometry.workspace import *
from pyrieef.geometry.interpolation import *
from pyrieef.rendering.workspace_planar import WorkspaceDrawer
from pyrieef.planning.mdp import GridMDP
from pyrieef.planning.mdp import value_iteration
import matplotlib.pyplot as plt
import itertools
USE_LWR = True
# Creates a workspace with just one circle
nb_points = 30
workspace = Workspace()
workspace.obstacles = [Circle(origin=[.1, .2], radius=0.1)]
renderer = WorkspaceDrawer(workspace)
grid = workspace.pixel_map(nb_points)
occupany = occupancy_map(nb_points, workspace)
reward = np.where(occupany > 0, -10., -.001)
reward[0, 0] = 10
# Calculate value function using value iteration
mdp = GridMDP(reward.tolist(), terminals=[(0, 0)])
X = value_iteration(mdp)
value = np.zeros(reward.shape)
for x in X:
value[x] = X[x]
value = np.flip(value, 1).T
if USE_LWR:
# Regress using LWR (Linear Weighted Regression)
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()