label=t["cost"][traj])
except:
print("No trajectory")
- for i in range(len(t["traj"][-1]) - 1):
- n1 = t["traj"][-1][i]
- n2 = t["traj"][-1][i + 1]
- if (sign(n1[4]) != sign(n2[4])):
- plt.plot(n2[0], n2[1], color=COLOR["log"][1], marker=".")
plt.plot(*plot_nodes([s["init"]]), color=COLOR["start"], marker=".")
plt.plot(*plot_nodes([s["goal"]]), color=COLOR["goal"], marker=".")
# end plot here