hd.TIME_FACTOR = 2000
hd.TIME_STEP = 1e-5
hd.ALGORITHM = "forward"
matplotlib.rcParams['pdf.fonttype'] = 42
matplotlib.rcParams['ps.fonttype'] = 42
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=plt.cm.tab20c)
renderer.show()
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.box_extent()
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()
import itertools N = 27 print(hd.discrete_2d_gradient(3, 3, axis=0)) print(hd.discrete_2d_gradient(3, 3, axis=1)) # exit() circles = [] circles.append(Circle(origin=[-2, -2], 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 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) renderer.draw_ws_obstacles() renderer.draw_ws_img(f.T, interpolate="none", color_style=plt.cm.hsv) renderer._ax.quiver(X, Y, U, V, units='width') renderer.show()
from pyrieef.geometry.utils import *
from pyrieef.rendering.workspace_renderer import WorkspaceDrawer
import itertools
import matplotlib.pyplot as plt
cmap = plt.get_cmap('viridis')
circles = []
circles.append(AnalyticCircle(origin=[.1, .0], radius=0.1))
circles.append(AnalyticCircle(origin=[.1, .25], radius=0.05))
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.
# circles.append(Circle(origin=[-2, -2], 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))
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()
#
# Jim Mainprice on Wed February 12 2019
from demos_common_imports import *
import numpy as np
from pyrieef.geometry.workspace import *
from pyrieef.geometry.interpolation import *
from pyrieef.rendering.workspace_renderer import WorkspaceDrawer
import matplotlib.pyplot as plt
import itertools
# Creates a workspace with just one circle
workspace = Workspace()
workspace.obstacles = [Circle(origin=[.0, .0], radius=0.1)]
renderer = WorkspaceDrawer(workspace)
sdf = SignedDistanceWorkspaceMap(workspace)
# Creates a vector field as the gradient of the signed distance field
nb_points = 14
X, Y = workspace.box.meshgrid(nb_points)
U, V = np.zeros(X.shape), np.zeros(Y.shape)
X_data = np.empty((nb_points ** 2, 2))
Y1 = np.empty((nb_points ** 2))
Y2 = np.empty((nb_points ** 2))
k = 0
for i, j in itertools.product(range(X.shape[0]), range(X.shape[1])):
p = np.array([X[i, j], Y[i, j]])
g = sdf.gradient(p)
g /= np.linalg.norm(g)
U[i, j] = g[0]
""" Plots a covariance ellipse centered at mean """
vals, vecs = eigsorted(cov)
if np.any(np.isnan(vals)):
return
if np.any(np.isnan(vecs)):
return
theta = np.degrees(np.arctan2(*vecs[:, 0][::-1]))
w, h = 2. * n_std * np.sqrt(vals)
ellipse = Ellipse(mean, width=w, height=h, angle=theta, **kwargs)
return ax.add_patch(ellipse)
# Creates a workspace with just one circle
workspace = Workspace()
workspace.obstacles = [Circle(origin=[.0, .0], radius=0.1)]
renderer = WorkspaceDrawer(workspace)
sdf = SignedDistanceWorkspaceMap(workspace)
cost = ObstaclePotential2D(sdf, 1., 10.)
# Querries the hessian of the cost
nb_points = 11
X, Y = workspace.box.meshgrid(nb_points)
Sigma = []
for i, j in itertools.product(range(X.shape[0]), range(X.shape[1])):
p = np.array([X[i, j], Y[i, j]])
H = cost.hessian(p)
Sigma.append([p, H]) # np.linalg.inv(H)])
renderer.set_drawing_axis(i)
renderer.background_matrix_eval = False
renderer.draw_ws_background(Compose(RangeSubspaceMap(3, [0]), cost),
# PERFORMANCE OF THIS SOFTWARE.
#
# Jim Mainprice on Sunday June 13 2018
import demos_common_imports
from pyrieef.geometry.workspace import *
from pyrieef.geometry.pixel_map import sdf
from pyrieef.rendering.workspace_renderer import WorkspaceDrawer
import scipy.misc
from scipy.misc import imresize
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)
viewer = WorkspaceDrawer(workspace, wait_for_keyboard=True)
nb_points = 20
occupancy_map = occupancy_map(nb_points, workspace)
signed_distance_field = sdf(occupancy_map)
# viewer.draw_ws_img(occupancy_map)
viewer.draw_ws_img(
ndimage.gaussian_filter(imresize(signed_distance_field, (300, 300),
'nearest'),
sigma=3))
viewer.draw_ws_obstacles()
viewer.show_once()
import numpy as np
from pyrieef.geometry.workspace import *
from pyrieef.geometry.interpolation import *
from pyrieef.rendering.workspace_renderer 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)
# LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR # OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR # PERFORMANCE OF THIS SOFTWARE. # # Jim Mainprice on Sunday June 13 2018 import demos_common_imports from pyrieef.geometry.workspace import * from pyrieef.geometry.diffeomorphisms import * from pyrieef.rendering.workspace_renderer import WorkspaceDrawer env = EnvBox(dim=np.array([2., 2.])) box = Box(origin=np.array([-.5, -.5]), dim=np.array([.5, .5])) segment = Segment(origin=np.array([.4, -.5]), orientation=0.2) circle = Circle(origin=np.array([.5, .5]), radius=0.2) ellipse = Ellipse(a=0.5, b=0.3) polygon = hexagon(.33) # polygon = ConvexPolygon() workspace = Workspace(env) workspace.obstacles.append(box) workspace.obstacles.append(segment) workspace.obstacles.append(circle) workspace.obstacles.append(polygon) # workspace.obstacles.append(ellipse) viewer = WorkspaceDrawer(workspace, wait_for_keyboard=True) sdf = SignedDistanceWorkspaceMap(workspace) viewer.draw_ws_obstacles() viewer.background_matrix_eval = True viewer.draw_ws_background(sdf, nb_points=200) viewer.show_once()
from pyrieef.motion.trajectory import *
from pyrieef.motion.control import KinematicTrajectoryFollowingLQR
from pyrieef.rendering.workspace_renderer import WorkspaceDrawer
"""
Integrate LQR forward in time by following a straight line constant
speed trajectory
"""
tracked_trajectory = linear_interpolation_trajectory(q_init=-.22 * np.ones(2),
q_goal=.4 * np.ones(2),
T=22)
lqr = KinematicTrajectoryFollowingLQR(dt=0.1, trajectory=tracked_trajectory)
lqr.solve_ricatti(Q_p=13, Q_v=3, R_a=1)
viewer = WorkspaceDrawer(Workspace(), wait_for_keyboard=True)
viewer.draw_ws_line(tracked_trajectory.list_configurations())
# create squared meshgrid
d = np.linspace(-.4, 0.1, 5)
X, Y = np.meshgrid(d, d)
start_points = np.vstack([X.ravel(), Y.ravel()]).T
# integrate all points in grid forward in time
for p_init in start_points:
viewer.draw_ws_point(p_init)
viewer.draw_ws_line_fill(lqr.integrate(p_init).list_configurations(),
color='k',
linewidth=.1)
viewer.show_once()
import demos_common_imports
from pyrieef.geometry.workspace import *
from pyrieef.geometry.diffeomorphisms import *
from pyrieef.rendering.workspace_renderer 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()
# LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
# OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
# PERFORMANCE OF THIS SOFTWARE.
#
# Jim Mainprice on Monday January 27 2020
import demos_common_imports
from pyrieef.geometry.workspace import *
from pyrieef.geometry.rotations import *
from pyrieef.kinematics.robot import *
from pyrieef.rendering.workspace_renderer import WorkspaceDrawer
robot = create_robot_from_file(scale=.02)
# robot = create_robot_with_even_keypoints(scale=.03)
workspace = Workspace()
workspace.obstacles.append(
Box(origin=np.array([-.3, 0]), dim=np.array([.4, .02])))
workspace.obstacles.append(
Box(origin=np.array([.3, 0]), dim=np.array([.4, .02])))
sdf = SignedDistanceWorkspaceMap(workspace)
viewer = WorkspaceDrawer(workspace, wait_for_keyboard=True)
q = np.array([.0, -.2, .2])
viewer.draw_ws_obstacles()
viewer.draw_ws_polygon(robot.shape, q[:2], q[2])
for name, i in robot.keypoint_names.items():
p = robot.keypoint_map(i)(q)
viewer.draw_ws_point(p, color='b', shape='o')
viewer.background_matrix_eval = True
viewer.draw_ws_background(sdf, nb_points=100)
viewer.show_once()
# 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_renderer 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()
from pyrieef.geometry.geodesics import *
from pyrieef.geometry.diffeomorphisms import *
from pyrieef.geometry.utils import *
from pyrieef.motion.cost_terms import *
from pyrieef.rendering.workspace_renderer import WorkspaceDrawer
import itertools
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)
sdf = SignedDistanceWorkspaceMap(workspace)
cost = ObstaclePotential2D(sdf, 1., 1.7)
x_goal = np.array([0.4, 0.4])
nx, ny = (3, 3)
x = np.linspace(-.2, -.1, nx)
y = np.linspace(-.5, -.1, ny)
for i, j in itertools.product(list(range(nx)), list(range(ny))):
x_init = np.array([x[i], y[j]])
line = NaturalGradientGeodescis(cost, x_init, x_goal, attractor=False)
renderer.draw_ws_line(line)
renderer.draw_ws_background(sdf)
renderer.draw_ws_obstacles()
renderer.draw_ws_point([x_goal[0], x_goal[1]], color='k', shape='o')
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)
