def rotate(self, duration, angle):
self.waypoints.append(
formation.rotate_points_z(form, np.deg2rad(angle / 2)))
self.T.append(duration / 4)
self.waypoints.append(
formation.rotate_points_z(form, np.deg2rad(angle)))
self.T.append(duration / 4)
self.waypoints.append(
formation.rotate_points_z(form, np.deg2rad(angle / 2)))
self.T.append(duration / 4)
self.waypoints.append(form)
self.T.append(duration / 4)
def rotate(self, form, n, duration, color):
for t in np.linspace(0, 1, n):
rot_form = formation.rotate_points_z(form, t * 2 * np.pi)
self.waypoints.append(rot_form)
self.T.append(duration / n)
self.colors.append(self.rgb[color])
n_drones = form.shape[1]
self.delays.append(np.zeros(n_drones).tolist())
def spiral(self, form, z, n, duration, color):
for t in np.linspace(0, 1, n):
rot_form = formation.rotate_points_z(form, t * 2 * np.pi)
shift = np.array([[0, 0, z * np.sin(t)]]).T
self.waypoints.append(rot_form + shift)
self.T.append(duration / n)
self.colors.append(self.rgb[color])
n_drones = form.shape[1]
self.delays.append(np.zeros(n_drones).tolist())
plt.title('flat P')
# %% A slanted P formation.
P2 = np.array([[-1, 1, 2], [0, 1, 2], [1, 1, 2], [-1, 0, 1.5], [0, 0, 1.5],
[1, 0, 1.5], [-1, -1, 1]]).T
plt.figure()
ax = plt.axes(projection='3d')
ax.plot3D(P[0, :], P[1, :], P[2, :], 'ro')
ax.plot3D(P2[0, :], P2[1, :], P2[2, :], 'bo')
plt.title('slanted P')
# %% Create waypoints for flat P -> slanted P -> rotating slanted P -> flat P
waypoints = [P]
for theta in np.linspace(0, 2 * np.pi, 8):
waypoints.append(form.rotate_points_z(P2, theta))
waypoints.append(P)
waypoints = np.array(waypoints)
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')
# %% plan trajectories
dist = np.linalg.norm(waypoints[1:, :, :] - waypoints[:-1, :, :], axis=1)
dist_max = np.max(dist, axis=1)
dist_max
