def _plot(x_path, y_path, th_path):
k = 0
for i in range(0, len(x_path)):
x = x_path[i][k]
#print("X:", x)
y = y_path[i][k]
#print("Y:", y)
#plt.plot(x, y)
th = th_path[i][k]
#print("TH:", th)
cp = calculate_car_crucial_points(x, y, th)
for j, p in enumerate(cp):
plt.plot(p[:, 0], p[:, 1], zorder=6)
i = len(x_path) - 1
x = x_path[i][k]
y = y_path[i][k]
th = th_path[i][k]
cp = calculate_car_crucial_points(x, y, th)[1:]
_plot_car(cp, -1, 'r', 0.5)
i = 0
x = x_path[i][k]
y = y_path[i][k]
th = th_path[i][k]
cp = calculate_car_crucial_points(x, y, th)[1:]
_plot_car(cp, 0, 'g', 0.5)
def _plot(x_path, y_path, th_path, data, step, print=False):
_, _, free_space = data
for k in range(len(x_path[0])):
plt.figure(num=None,
figsize=(9, 2),
dpi=300,
facecolor='w',
edgecolor='k')
plt.fill([-100., 100., 100., -100., -100.],
[-100., -100., 100., 100., -100.],
'brown',
zorder=1)
plt.xlim(-33., 4.5)
plt.ylim(-0.25, 5.75)
for i in range(len(x_path)):
x = x_path[i][k]
y = y_path[i][k]
#plt.plot(x, y)
th = th_path[i][k]
cp = calculate_car_crucial_points(x, y, th)
for p in cp:
plt.plot(p[:, 0], p[:, 1])
m = free_space[k]
seq = [(0, 0), (0, 1), (0, 2), (0, 3), (1, 2), (1, 3), (2, 2), (2, 3),
(2, 0), (2, 1), (1, 0), (1, 1), (0, 0)]
x = [m[s][0] for s in seq]
y = [m[s][1] for s in seq]
plt.fill(x, y, 'w', zorder=2)
i = len(x_path) - 1
x = x_path[i][k]
y = y_path[i][k]
th = th_path[i][k]
cp = calculate_car_crucial_points(x, y, th)[1:]
_plot_car(cp, -1, 'r')
i = 0
x = x_path[i][k]
y = y_path[i][k]
th = th_path[i][k]
cp = calculate_car_crucial_points(x, y, th)[1:]
_plot_car(cp, 0, 'g')
if print:
plt.show()
else:
#plt.savefig("last_path" + str(k).zfill(6) + ".pdf")
plt.savefig("last_path" + str(k).zfill(6) + ".png")
plt.clf()
def _plot(x_path, y_path, th_path, data, step, n, print=False):
_, _, free_space, _ = data
for i in range(len(x_path)):
x = x_path[i][n]
y = y_path[i][n]
th = th_path[i][n]
cp = calculate_car_crucial_points(x, y, th)
for p in cp:
plt.plot(p[:, 0], p[:, 1], 'r*')
for i in range(free_space.shape[1]):
for j in range(4):
fs = free_space
plt.plot([fs[0, i, j - 1, 0], fs[0, i, j, 0]],
[fs[0, i, j - 1, 1], fs[0, i, j, 1]])
#plt.xlim(-25.0, 25.0)
#plt.ylim(0.0, 50.0)
# plt.xlim(-15.0, 20.0)
# plt.ylim(0.0, 35.0)
#plt.xlim(-35.0, 5.0)
#plt.ylim(-2.0, 6.0)
if print:
plt.show()
else:
plt.savefig("last_path" + str(step).zfill(6) + ".png")
plt.clf()
def _plot(x_path, y_path, th_path, ax):
x_path = tf.concat(x_path, 0)
y_path = tf.concat(y_path, 0)
th_path = tf.concat(th_path, 0)
path = tf.stack([x_path, y_path, th_path], axis=-1)
cp = calculate_car_crucial_points(x_path, y_path, th_path)
cp = tf.stack(cp, axis=1)
_plot_car(cp[0, :, 0], ax, 'g')
cl = ['c', 'g', 'b', 'm', 'k']
for s in range(path.shape[0]):
x = x_path[s]
y = y_path[s]
th = th_path[s]
cps = calculate_car_crucial_points(x, y, th)
for j, p in enumerate(cps):
x, y = transform_to_img(p[:, 0], p[:, 1])
ax.plot(x, y, color=cl[j], zorder=4)
_plot_car(cp[-1, :, -1], ax, 'r')
def _plot(x_path, y_path, th_path, gidx):
shuffle(gidx)
gidx = list(gidx[:1, 0])
cl = ['c', 'g', 'b', 'm', 'k']
for k in gidx:
for i in range(0, len(x_path)):
x = x_path[i][k]
#print("X:", x)
y = y_path[i][k]
#print("Y:", y)
#plt.plot(x, y)
th = th_path[i][k]
#print("TH:", th)
cp = calculate_car_crucial_points(x, y, th)
for j, p in enumerate(cp):
plt.plot(p[:, 0], p[:, 1], color=cl[j], zorder=4)
i = len(x_path) - 1
x = x_path[i][k]
y = y_path[i][k]
th = th_path[i][k]
cp = calculate_car_crucial_points(x, y, th)[1:]
#xy = [[p[-1, 0].numpy(), p[-1, 1].numpy()] for p in cp]
_plot_car(cp, -1)
def invalidate(x, y, fi, free_space):
"""
Check how much specified points violate the environment constraints
"""
crucial_points = calculate_car_crucial_points(x, y, fi)
crucial_points = tf.stack(crucial_points, -2)
penetration = dist(free_space, crucial_points)
in_obstacle = tf.reduce_sum(penetration, -1)
violation_level = tf.reduce_sum(in_obstacle, -1)
# violation_level = integral(env.free_space, crucial_points)
return violation_level
def invalidate(x, y, fi, free_space):
"""
Check how much specified points violate the environment constraints
"""
crucial_points = calculate_car_crucial_points(x, y, fi)
crucial_points = tf.stack(crucial_points, -2)
d = tf.sqrt(tf.reduce_sum((crucial_points[:, 1:] - crucial_points[:, :-1]) ** 2, -1))
penetration = dist(free_space, crucial_points)
in_obstacle = tf.reduce_sum(d * penetration[:, :-1], -1)
violation_level = tf.reduce_sum(in_obstacle, -1)
return violation_level
def invalidate(x, y, fi, free_space, path):
"""
Check how much specified points violate the environment constraints
"""
crucial_points = calculate_car_crucial_points(x, y, fi)
car_contour = calculate_car_contour(crucial_points)
crucial_points = tf.stack(crucial_points, -2)
xy = tf.stack([x, y], axis=-1)[:, :, tf.newaxis]
d = tf.linalg.norm(xy[:, 1:] - xy[:, :-1], axis=-1)
path_cp = calculate_car_crucial_points(path[..., 0], path[..., 1],
path[..., 2])
path_cp = tf.stack(path_cp, -2)
penetration = path_dist_cp(path_cp, crucial_points)
not_in_collision = if_inside(free_space, car_contour)
not_in_collision = tf.reduce_all(not_in_collision, axis=-1)
penetration = tf.where(not_in_collision, tf.zeros_like(penetration),
penetration)
violation_level = tf.reduce_sum(d[..., 0] * penetration[:, :-1], -1)
supervised_loss = tf.reduce_sum(penetration, -1)
return violation_level, supervised_loss
def _plot(x_path, y_path, th_path, data, step, print=False):
res = 0.2
free_space, path, ddy0 = data
path = path[..., :3]
plt.imshow(free_space[0, ..., 0])
for i in range(len(x_path)):
x = x_path[i][0]
y = y_path[i][0]
th = th_path[i][0]
cp = calculate_car_crucial_points(x, y, th)
for p in cp:
u = -p[:, 1] / res + 64
v = 120 - p[:, 0] / res
plt.plot(u, v)
#plt.plot(p[:, 0] * 10, (10. - p[:, 1]) * 10)
#plt.plot(path[0, :, 0] * 10, (10 - path[0, :, 1]) * 10, 'r')
u = -path[0, :, 1] / res + 64
v = 120 - path[0, :, 0] / res
plt.plot(u, v, 'r')
if print:
plt.show()
else:
plt.savefig("last_path" + str(step).zfill(6) + ".png")
plt.clf()