def display(start=None,
goal=None,
grid_obs=[],
originalPath=[],
optimPath=[],
losses=[],
delta_t=None,
currentOptimIdx=None,
grad=[],
hold=False):
print('plotting...')
ax.clear()
ax.set_xlim(-0.5, grid_obs.shape[0])
ax.set_ylim(-0.5, grid_obs.shape[0])
ax.set_title('Trajectory')
ax2.clear()
obs = []
x, y = np.mgrid[0:grid_obs.shape[0], 0:grid_obs.shape[1]]
np.vectorize(lambda node, x, y: obs.append(patches.Rectangle([x, y], 1, 1))
if node == Node.OBSTACLE else None)(grid_obs, x, y)
# obs = [patches.Rectangle([x, y], w, h) for x, y, w, h in extractRect(grid_obs)]
ax.add_collection(collections.PatchCollection(obs))
if start is not None:
ax.add_patch(
patches.Circle(getPos(start), .3, linewidth=1, facecolor='green'))
if goal is not None:
ax.add_patch(
patches.Circle(getPos(goal), .3, linewidth=1, facecolor='blue'))
if len(originalPath) > 0:
smoothed = bsplineUpsample(originalPath)
ax.plot(originalPath[:, 0],
originalPath[:, 1],
'x',
color='red',
markersize=5)
ax.plot(smoothed[:, 0], smoothed[:, 1], '-', color='red', linewidth=.8)
if currentOptimIdx is not None:
ax.plot(originalPath[currentOptimIdx:currentOptimIdx +
OPTIMIZED_POINTS, 0],
originalPath[currentOptimIdx:currentOptimIdx +
OPTIMIZED_POINTS, 1],
'x',
color='green',
markersize=5)
if len(optimPath) > 0:
smoothed = bsplineUpsample(optimPath)
ax.plot(optimPath[:, 0],
optimPath[:, 1],
'o',
color='blue',
markersize=5)
ax.plot(smoothed[:, 0],
smoothed[:, 1],
'-',
color='blue',
linewidth=.8)
if delta_t is not None:
for i in range(2, len(optimPath) - 3):
p = bspline(0, extractPts(optimPath, i))
v = .5 * bsplineVel(0, extractPts(optimPath, i), delta_t)
# a = .3 * bsplineAcc(0, extractPts(optimPath, i), delta_t)
ax.arrow(p[0],
p[1],
v[0],
v[1],
width=.1,
head_width=.5,
color='orange',
alpha=.3)
# ax.arrow(p[0], p[1], a[0], a[1], width=.01, head_width=.2, color='red', alpha=.6)
if currentOptimIdx is not None:
ax.plot(
optimPath[currentOptimIdx:currentOptimIdx + OPTIMIZED_POINTS,
0],
optimPath[currentOptimIdx:currentOptimIdx + OPTIMIZED_POINTS,
1],
'o',
color='green',
markersize=5)
velCurve = np.linalg.norm(bsplineVelUpsample(optimPath, delta_t),
axis=1)
accCurve = np.linalg.norm(bsplineAccUpsample(optimPath, delta_t),
axis=1)
jerkCurve = np.linalg.norm(bsplineJerkUpsample(optimPath, delta_t),
axis=1)
snapCurve = np.linalg.norm(bsplineSnapUpsample(optimPath, delta_t),
axis=1)
ax2.plot(velCurve / np.max(velCurve), color='orange', label='Velocity')
ax2.plot(accCurve / np.max(accCurve),
color='red',
label='Acceleration')
ax2.plot(jerkCurve / np.max(jerkCurve), color='cyan', label='Jerk')
ax2.plot(snapCurve / np.max(snapCurve), color='blue', label='Snap')
ax2.legend()
if currentOptimIdx and len(grad) > 0:
for i, g in enumerate(grad):
ax.arrow(optimPath[currentOptimIdx + i, 0],
optimPath[currentOptimIdx + i, 1],
-g[0],
-g[1],
width=.05,
head_width=.2,
color='magenta',
alpha=.8)
if hold and isinstance(hold, bool):
plt.show()
else:
plt.pause(DISPLAY_DELAY if isinstance(hold, bool) else hold)
def computeVelocities(sample):
p = bsplineVel(sample[1], extractPts(pts, sample[0] + startIdx),
delta_t)
return norm(p)
def cost(pts, globalPts, startIdx, distObs, delta_t):
# Endpoint cost
posDiff = bspline(0, extractPts(pts,
startIdx + 5)) - globalPts[startIdx + 5]
velDiff = bsplineVel(0, extractPts(pts, startIdx + 5),
delta_t) - bsplineVel(
0, extractPts(globalPts, startIdx + 5), delta_t)
E_ep = lambda_p * np.dot(posDiff, posDiff) + lambda_v * np.dot(
velDiff, velDiff)
# Collision cost
u = np.linspace(0, 1, 5)
samples = np.vstack((np.repeat(np.arange(6), len(u)), np.tile(u, 6)))
def computeDist(sample):
p = bspline(sample[1], extractPts(pts, sample[0] + startIdx))
return distObs[np.clip(np.int(p[0]), 0, distObs.shape[0] - 1),
np.clip(np.int(p[1]), 0, distObs.shape[1] - 1)]
distances = np.apply_along_axis(computeDist, 0, samples)
mask = distances <= OBSTACLE_DISTANCE_THRESHOLD
distances[mask] = np.square(distances[mask] - OBSTACLE_DISTANCE_THRESHOLD
) / (2 * OBSTACLE_DISTANCE_THRESHOLD)
distances[np.invert(mask)] = 0
def computeVelocities(sample):
p = bsplineVel(sample[1], extractPts(pts, sample[0] + startIdx),
delta_t)
return norm(p)
velocities = np.apply_along_axis(computeVelocities, 0, samples)
E_c = lambda_c * np.sum(np.dot(distances, velocities)) / (len(u) * 6)
# Squared derivative cost
q2, q3, q4 = Q_2(delta_t), Q_3(delta_t), Q_4(delta_t)
E_q = 0
for i in range(6):
A = np.dot(M_6, extractPts(pts, startIdx + i))
B = A.T
E_q = E_q + np.sum(lambda_q2 * np.dot(np.dot(B, q2), A) +
lambda_q3 * np.dot(np.dot(B, q3), A) +
lambda_q4 * np.dot(np.dot(B, q4), A))
# Derivative limit cost
max_vel, max_acc, max_jerk, max_snap = np.array([1000, 1000]), np.array(
[1000, 1000]), np.array([1e10, 1e10]), np.array([1e10, 1e10])
u = np.linspace(0, 1, 5)
samples = np.vstack((np.repeat(np.arange(6), len(u)), np.tile(u, 6)))
def derivativeCost(pFunc, max_p, delta_t):
def f(sample):
p = pFunc(sample[1], extractPts(pts, sample[0] + startIdx),
delta_t)
norm_max = norm(max_p)
norm_p = norm(p)
return np.exp(norm_p - norm_max) - 1 if norm_p > norm_max else 0
return f
E_l = 0
for sample in zip(samples[0], samples[1]):
E_l = E_l + derivativeCost(bsplineVel, max_vel, delta_t)(sample)
E_l = E_l + derivativeCost(bsplineAcc, max_acc, delta_t)(sample)
E_l = E_l + derivativeCost(bsplineJerk, max_jerk, delta_t)(sample)
E_l = E_l + derivativeCost(bsplineSnap, max_snap, delta_t)(sample)
E_l = E_l / (len(u) * 6)
# Total cost
E = E_ep + E_c + E_q + E_l
# if not isinstance(E_ep, autograd.numpy.numpy_boxes.ArrayBox):
# print('[{}] {} | {} | {} | {} => {}'.format(startIdx, E_ep, E_c, E_q, E_l, E))
return E
def computeDist(sample):
p = bspline(sample[1], extractPts(pts, sample[0] + startIdx))
return distObs[np.clip(np.int(p[0]), 0, distObs.shape[0] - 1),
np.clip(np.int(p[1]), 0, distObs.shape[1] - 1)]
def f(sample):
p = pFunc(sample[1], extractPts(pts, sample[0] + startIdx),
delta_t)
norm_max = norm(max_p)
norm_p = norm(p)
return np.exp(norm_p - norm_max) - 1 if norm_p > norm_max else 0