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