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()
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_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')
renderer.show()