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()
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()