# 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_planar 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()
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),
color_style=plt.cm.Blues)
for cov in Sigma:
confidence_ellipse(cov[0], cov[1], renderer._ax,
n_std=.03, edgecolor='red', facecolor="none")
renderer.show()
Y1[k] = g[0]
Y2[k] = g[1]
k += 1
# Store the Data in a LWR (Linear Weighted Regression)
# object where dimension of the vector field are being abstracted
f = LWR(2, 2)
f.X = [X_data, X_data]
f.Y = [Y1, Y2]
f.D = [np.eye(2), np.eye(2)]
f.ridge_lambda = [.1, .1]
# Querries the regressed function f
# And then draws the interolated field in red and original points in black
X_int, Y_int = workspace.box.meshgrid(30)
U_int, V_int = np.zeros(X_int.shape), np.zeros(Y_int.shape)
for i, j in itertools.product(range(X_int.shape[0]), range(X_int.shape[1])):
g = f(np.array([X_int[i, j], Y_int[i, j]]))
U_int[i, j] = g[0]
V_int[i, j] = g[1]
renderer = WorkspaceDrawer(workspace)
renderer.set_drawing_axis(i)
renderer.draw_ws_obstacles()
renderer.draw_ws_point([0, 0], color='r', shape='o')
renderer.background_matrix_eval = True
renderer.draw_ws_background(sdf, color_style=plt.cm.Blues)
renderer._ax.quiver(X, Y, U, V, units='width', color='k')
renderer._ax.quiver(X_int, Y_int, U_int, V_int, units='width', color='r')
renderer.show()
from pyrieef.geometry.utils import *
from pyrieef.motion.cost_terms import *
from pyrieef.rendering.workspace_planar 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')
renderer.show()