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()
import demos_common_imports
from pyrieef.geometry.workspace import *
from pyrieef.geometry.diffeomorphisms import *
from pyrieef.rendering.workspace_planar import WorkspaceDrawer
env = EnvBox(
origin=np.array([.0, .0]),
dim=np.array([2., 2.]))
# polygon = ConvexPolygon(origin=env.origin + np.array([-.2, -.2]))
polygon = ellipse_polygon(.7, .3, [-.2, .0], [-.1, -.1], .7)
workspace = Workspace(env)
workspace.obstacles.append(polygon)
viewer = WorkspaceDrawer(workspace, wait_for_keyboard=True)
sdf = SignedDistanceWorkspaceMap(workspace)
viewer.draw_ws_obstacles()
viewer.draw_ws_circle(origin=env.origin, radius=1.)
# viewer.background_matrix_eval = False
viewer.draw_ws_background(sdf, nb_points=200)
for theta in np.linspace(0, 2 * math.pi, 20, endpoint=False):
p1 = np.array([np.cos(theta), np.sin(theta)])
p2 = polygon.intersection_point(p1)
viewer.draw_ws_line_fill([polygon.focus(), p1], color='k')
viewer.draw_ws_point(p2, color='r', shape='o')
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(AnalyticCircle(origin=[.2, .25], radius=0.05))
circles.append(AnalyticCircle(origin=[.0, .25], radius=0.05))
workspace = Workspace()
workspace.obstacles = [circle.object() for circle in circles]
renderer = WorkspaceDrawer(workspace)
x_goal = np.array([0.4, 0.4])
nx, ny = (5, 4)
x = np.linspace(-.2, -.05, nx)
y = np.linspace(-.5, -.1, ny)
analytical_circles = AnalyticMultiDiffeo(circles)
sclar_color = 0.
for i, j in itertools.product(list(range(nx)), list(range(ny))):
sclar_color += 1. / (nx * ny)
x_init = np.array([x[i], y[j]])
print("x_init : ", x_init)
# Does not have an inverse.
[line, line_inter] = InterpolationGeodescis(
analytical_circles, x_init, x_goal)
# line = NaturalGradientGeodescis(analytical_circles, x_init, x_goal)
renderer.draw_ws_line(line, color=cmap(sclar_color))
renderer.draw_ws_point([x_init[0], x_init[1]], color='r', shape='o')
renderer.draw_ws_obstacles()
renderer.draw_ws_point([x_goal[0], x_goal[1]], color='r', shape='o')
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()