def plan_trajectory(self):
trajectories = []
origin = np.array([0, 0, 2])
self.waypoints = np.array(self.waypoints)
for drone in range(self.waypoints.shape[2]):
pos_wp = self.waypoints[:, :, drone] + origin
yaw_wp = np.zeros((pos_wp.shape[0], 1))
traj = tgen.min_deriv_4d(4,
np.hstack([pos_wp, yaw_wp]),
self.T,
stop=False)
trajectories.append(traj)
traj_json = tgen.trajectories_to_json(trajectories)
data = {}
for key in traj_json.keys():
data[key] = {
'trajectory': traj_json[key],
'T': self.T,
'color': g.colors,
'delay': [d[key] for d in g.delays]
}
data['repeat'] = 3
assert len(trajectories) < 32
return trajectories, data
def plan_letters(letter_string: str):
letters = Letters()
letters.add('takeoff', color='blue', duration=2, led_delay=0)
#%% Create waypoints for flat P -> slanted P -> rotating slanted P -> flat P
for i, letter in enumerate(letter_string.split(' ')):
letters.add('takeoff', color='blue', duration=5, led_delay=0)
letters.add(letter, color='blue', duration=5, led_delay=0)
letters.add(letter, color='gold', duration=5, led_delay=0.5)
if i == 7:
for theta in np.linspace(0, 2 * np.pi, 5)[1:]:
letters.add(letter,
color='gold',
duration=3,
led_delay=0,
angle=theta)
letters.add('takeoff', color='blue', duration=5, led_delay=0)
letters.add('takeoff', color='white', duration=10, led_delay=0.5)
trajectories = letters.plan_trajectory(origin=np.array([1.5, 2, 2]))
traj_json = tgen.trajectories_to_json(trajectories)
data = {}
for key in traj_json.keys():
data[key] = {
'trajectory': traj_json[key],
'T': letters.T,
'color': letters.colors,
'delay': [d[key] for d in letters.delays]
}
data['repeat'] = 1
return trajectories, data
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')
other.plot_uav_trajectory('test_data.csv')
plt.show()
except ImportError:
print('requires plot_uav_trajectory module')
with open('testPlan.json', 'w') as f:
json.dump(data, f)
c.left(2)
c.right(3)
c.crisscross(5)
c.chacha(5)
c.wait(1)
print('length of waypoitns', len(c.waypoints))
origin = np.array([1.5, 2, 2])
waypoints = np.array(c.waypoints)
T = c.T
trajectories = []
for drone in range(waypoints.shape[2]):
pos_wp = waypoints[:, :, drone] + origin
yaw_wp = np.zeros((pos_wp.shape[0], 1))
traj = tgen.min_deriv_4d(4, np.hstack([pos_wp, yaw_wp]), T, stop=False)
trajectories.append(traj)
tgen.plot_trajectories(trajectories)
tgen.trajectories_to_json(trajectories, 'scripts/data/chacha.json')
plt.show()
# %%
tgen.plot_trajectories_time_history(trajectories)
# %%
tgen.animate_trajectories('chacha.mp4', trajectories, fps=5)
#%%
delays = [0, 0]
assert len(waypoints) < 33
trajectories = []
for drone in range(waypoints.shape[2]):
print(waypoints[:, :, drone])
pos_wp = waypoints[:, :, drone] + origin
print(pos_wp)
yaw_wp = np.zeros((pos_wp.shape[0], 1))
traj = tgen.min_deriv_4d(4,
np.hstack([pos_wp, yaw_wp]),
time_vector,
stop=False)
trajectories.append(traj)
assert len(trajectories) < 32
traj_json = tgen.trajectories_to_json(trajectories)
data = {}
for key in traj_json.keys():
data[key] = {
'trajectory': traj_json[key],
'T': time_vector,
'color': colors,
'delay': delays
}
# how many times to repeat trajectory
data['repeat'] = 1
assert len(trajectories) < 32
assert len(time_vector) < 32
with open('scripts/data/block_land.json', 'w') as f:
json.dump(data, f)
plt.figure()
ax = plt.axes(projection='3d')
for point in range(waypoints.shape[2]):
ax.plot3D(waypoints[:, 0, point], waypoints[:, 1, point],
waypoints[:, 2, point], '-')
ax.view_init(azim=0, elev=40)
plt.title('waypoints')
plt.show()
#%% plan trajectories
dist = np.linalg.norm(waypoints[1:, :, :] - waypoints[:-1, :, :], axis=1)
dist_max = np.max(dist, axis=1)
dist_max
trajectories = []
T = dist_max
for drone in range(waypoints.shape[2]):
pos_wp = waypoints[:, :, drone]
yaw_wp = np.zeros((pos_wp.shape[0], 1))
traj = tgen.min_snap_4d(np.hstack([pos_wp, yaw_wp]), T)
trajectories.append(traj)
tgen.plot_trajectories_3d(trajectories)
tgen.trajectories_to_json(trajectories, 'scripts/data/p_form.json')
plt.show()
#%%
