M.append(row)
final_row = [row[0], row[1], 0, row[3]]
M.append(final_row)
M.insert(0, row1)
waypoints = np.array(M)
# x, y, z, yaw
T = 10 * np.ones(len(waypoints) - 1)
traj = tgen.min_snap_4d(waypoints, T, stop=True)
res = traj.compute_inputs()
print(res.max_data())
#%%
plt.figure()
tgen.plot_trajectories([traj])
plt.show()
#%%
#plt.figure()
#tgen.plot_trajectory_derivatives([traj])
#plt.show()
data = tgen.trajectories_to_json([traj])
#%%
check_with_other_library = True
if check_with_other_library:
try:
import bswarm.third_party.plot_trajectory as other
traj.write_csv('test_data.csv')
g = Geometry()
g.sin_wave(form=formTakeoff, n=8, duration=16, color='red')
g.goto(form=formCircle, duration=2, color='blue')
g.rotate(form=formCircle, n=6, duration=12, color='green')
# g.rotate(form=formTriangle, n=6, duration=12, color='white')
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()))
#%%