def test_trajectory():
res = traj.min_snap_1d([1, 2, 3], [1, 2], False)
plt.figure(figsize=(6, 5))
traj.plot_trajectories_time_history([res])
plt.show(block=False)
plt.pause(1)
plt.close()
g.goto(form=formCircle, duration=2, color='gold')
g.spiral(form=formCircle, z=1, n=6, duration=12, color='red')
g.goto(formTakeoff, 2, color='blue')
#%% plan trajectories
trajectories, data = g.plan_trajectory()
with open('scripts/data/geometry_3drone.json', 'w') as f:
json.dump(data, f)
tgen.plot_trajectories(trajectories)
# tgen.animate_trajectories('geometry.mp4', trajectories, 1)
#%%
plt.figure()
tgen.plot_trajectories_time_history(trajectories)
plt.show()
#%%
plt.figure()
tgen.plot_trajectories_magnitudes(trajectories)
plt.show()
#%%
print('number of segments', len(trajectories[0].coef_array()))
#%%
plt.figure()
plt.title('durations')
plt.bar(range(len(g.T)), g.T)
plt.show()
