├── .gitignore ├── .travis.yml ├── LICENSE.txt ├── MANIFEST.in ├── README.md ├── bswarm ├── __init__.py ├── data │ └── package_data.dat ├── formation.py ├── third_party │ ├── plot_trajectory.py │ └── uav_trajectory.py └── trajectory.py ├── environment.yml ├── scripts ├── .ipynb_checkpoints │ └── 3_drone_form-checkpoint.py ├── P_formation.py ├── block_land.py ├── chacha.py ├── data │ ├── HookedOnAFeeling.json │ ├── block_land.json │ ├── chacha.json │ ├── flyMe.json │ ├── geometry.json │ ├── geometry_3drone.json │ ├── geometry_9drone.json │ └── p_form.json ├── error.py ├── flight_qualysis.py ├── flight_script.py ├── flyMeToTheMoon.py ├── geometry_3drone.py ├── geometry_6drone.py ├── geometry_9drone.py ├── hookedonafeeling.py ├── set_tx_power.py └── traj_example.py ├── setup.cfg ├── setup.py ├── tests ├── __init__.py └── test_simple.py └── tox.ini /.gitignore: -------------------------------------------------------------------------------- 1 | # general things to ignore 2 | cache/ 3 | build/ 4 | dist/ 5 | *.mp4 6 | *.egg-info/ 7 | *.egg 8 | *.eggs 9 | .pytest_cache/ 10 | .vscode 11 | *.py[cod] 12 | __pycache__/ 13 | *.so 14 | *~ 15 | 16 | .ipynb_checkpoints 17 | 18 | # due to using tox and pytest 19 | .tox 20 | .cache 21 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # this file is *not* meant to cover or endorse the use of travis, but rather to 2 | # help confirm pull requests to this project. 3 | 4 | language: python 5 | 6 | matrix: 7 | include: 8 | - python: 3.6 9 | env: TOXENV=py36 10 | 11 | install: pip install tox 12 | 13 | script: tox 14 | 15 | notifications: 16 | email: false 17 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2019, James Goppert 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /MANIFEST.in: -------------------------------------------------------------------------------- 1 | # Include the README 2 | include *.md 3 | 4 | # Include the license file 5 | include LICENSE.txt 6 | 7 | # Include the data files 8 | #recursive-include data * 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Drone swarm control 2 | 3 | ## Build 4 | 5 | ```bash 6 | python3 setup.py install --user 7 | ``` 8 | -------------------------------------------------------------------------------- /bswarm/__init__.py: -------------------------------------------------------------------------------- 1 | import pkg_resources 2 | import sys 3 | 4 | 5 | if sys.version_info[0] != 3: 6 | raise ValueError("bswarm requires Python 3") 7 | 8 | def main(): 9 | """Entry point for the application script""" 10 | print("Call your main application code here") 11 | 12 | 13 | def load_file(): 14 | data_file = pkg_resources.resource_filename('bswarm', 'data/package_data.dat') 15 | with open(data_file, 'r') as f: 16 | print(f.read()) 17 | -------------------------------------------------------------------------------- /bswarm/data/package_data.dat: -------------------------------------------------------------------------------- 1 | some data -------------------------------------------------------------------------------- /bswarm/formation.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def rotate_points_z_h(P, theta): 4 | R = np.array([ 5 | [np.cos(theta), np.sin(theta), 0, 0], 6 | [-np.sin(theta), np.cos(theta), 0, 0], 7 | [0, 0, 1, 0], 8 | [0, 0, 0, 1] 9 | ]) 10 | P_h = np.vstack([P, np.zeros(P.shape[1])]) #homogenous coordinates 11 | return R.dot(P_h)[:3, :] 12 | 13 | def rotate_points_z(P, theta): 14 | R = np.array([ 15 | [np.cos(theta), np.sin(theta), 0], 16 | [-np.sin(theta), np.cos(theta), 0], 17 | [0, 0, 1] 18 | ]) 19 | 20 | return R @ P -------------------------------------------------------------------------------- /bswarm/third_party/plot_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | from mpl_toolkits.mplot3d import Axes3D 6 | import matplotlib.gridspec as gridspec 7 | import argparse 8 | 9 | from . import uav_trajectory 10 | 11 | # modified from https://github.com/whoenig/uav_trajectories 12 | 13 | def plot_uav_trajectory(csv_file): 14 | traj = uav_trajectory.Trajectory() 15 | traj.loadcsv(csv_file) 16 | ts = np.arange(0, traj.duration, 0.01) 17 | evals = np.empty((len(ts), 15)) 18 | for t, i in zip(ts, range(0, len(ts))): 19 | e = traj.eval(t) 20 | evals[i, 0:3] = e.pos 21 | evals[i, 3:6] = e.vel 22 | evals[i, 6:9] = e.acc 23 | evals[i, 9:12] = e.omega 24 | evals[i, 12] = e.yaw 25 | evals[i, 13] = e.roll 26 | evals[i, 14] = e.pitch 27 | 28 | velocity = np.linalg.norm(evals[:,3:6], axis=1) 29 | acceleration = np.linalg.norm(evals[:,6:9], axis=1) 30 | omega = np.linalg.norm(evals[:,9:12], axis=1) 31 | 32 | # print stats 33 | print("max speed (m/s): ", np.max(velocity)) 34 | print("max acceleration (m/s^2): ", np.max(acceleration)) 35 | print("max omega (rad/s): ", np.max(omega)) 36 | print("max roll (deg): ", np.max(np.degrees(evals[:,13]))) 37 | print("max pitch (deg): ", np.max(np.degrees(evals[:,14]))) 38 | 39 | # Create 3x1 sub plots 40 | gs = gridspec.GridSpec(6, 1) 41 | fig = plt.figure() 42 | 43 | ax = plt.subplot(gs[0:2, 0], projection='3d') # row 0 44 | ax.plot(evals[:,0], evals[:,1], evals[:,2]) 45 | 46 | ax = plt.subplot(gs[2, 0]) # row 2 47 | ax.plot(ts, velocity) 48 | ax.set_ylabel("velocity [m/s]") 49 | 50 | ax = plt.subplot(gs[3, 0]) # row 3 51 | ax.plot(ts, acceleration) 52 | ax.set_ylabel("acceleration [m/s^2]") 53 | 54 | ax = plt.subplot(gs[4, 0]) # row 4 55 | ax.plot(ts, omega) 56 | ax.set_ylabel("omega [rad/s]") 57 | 58 | ax = plt.subplot(gs[5, 0]) # row 5 59 | ax.plot(ts, np.degrees(evals[:,12])) 60 | ax.set_ylabel("yaw [deg]") 61 | 62 | # ax = plt.subplot(gs[6, 0]) # row 5 63 | # ax.plot(ts, np.degrees(evals[:,13])) 64 | # ax.set_ylabel("roll [deg]") 65 | 66 | # ax = plt.subplot(gs[7, 0]) # row 5 67 | # ax.plot(ts, np.degrees(evals[:,14])) 68 | # ax.set_ylabel("pitch [deg]") 69 | -------------------------------------------------------------------------------- /bswarm/third_party/uav_trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | 4 | # modified from https://github.com/whoenig/uav_trajectories 5 | 6 | def normalize(v): 7 | norm = np.linalg.norm(v) 8 | assert norm > 0 9 | return v / norm 10 | 11 | 12 | class Polynomial: 13 | def __init__(self, p): 14 | self.p = p 15 | 16 | def stretchtime(self, factor): 17 | recip = 1.0 / factor; 18 | scale = recip 19 | for i in range(1, len(self.p)): 20 | self.p[i] *= scale 21 | scale *= recip 22 | 23 | # evaluate a polynomial using horner's rule 24 | def eval(self, t): 25 | assert t >= 0 26 | x = 0.0 27 | for i in range(0, len(self.p)): 28 | x = x * t + self.p[len(self.p) - 1 - i] 29 | return x 30 | 31 | # compute and return derivative 32 | def derivative(self): 33 | return Polynomial([(i+1) * self.p[i+1] for i in range(0, len(self.p) - 1)]) 34 | 35 | 36 | class TrajectoryOutput: 37 | def __init__(self): 38 | self.pos = None # position [m] 39 | self.vel = None # velocity [m/s] 40 | self.acc = None # acceleration [m/s^2] 41 | self.omega = None # angular velocity [rad/s] 42 | self.yaw = None # yaw angle [rad] 43 | self.roll = None # required roll angle [rad] 44 | self.pitch = None # required pitch angle [rad] 45 | 46 | 47 | # 4d single polynomial piece for x-y-z-yaw, includes duration. 48 | class Polynomial4D: 49 | def __init__(self, duration, px, py, pz, pyaw): 50 | self.duration = duration 51 | self.px = Polynomial(px) 52 | self.py = Polynomial(py) 53 | self.pz = Polynomial(pz) 54 | self.pyaw = Polynomial(pyaw) 55 | 56 | # compute and return derivative 57 | def derivative(self): 58 | return Polynomial4D( 59 | self.duration, 60 | self.px.derivative().p, 61 | self.py.derivative().p, 62 | self.pz.derivative().p, 63 | self.pyaw.derivative().p) 64 | 65 | def stretchtime(self, factor): 66 | self.duration *= factor 67 | self.px.stretchtime(factor) 68 | self.py.stretchtime(factor) 69 | self.pz.stretchtime(factor) 70 | self.pyaw.stretchtime(factor) 71 | 72 | # see Daniel Mellinger, Vijay Kumar: 73 | # Minimum snap trajectory generation and control for quadrotors. ICRA 2011: 2520-2525 74 | # section III. DIFFERENTIAL FLATNESS 75 | def eval(self, t): 76 | result = TrajectoryOutput() 77 | # flat variables 78 | result.pos = np.array([self.px.eval(t), self.py.eval(t), self.pz.eval(t)]) 79 | result.yaw = self.pyaw.eval(t) 80 | 81 | # 1st derivative 82 | derivative = self.derivative() 83 | result.vel = np.array([derivative.px.eval(t), derivative.py.eval(t), derivative.pz.eval(t)]) 84 | dyaw = derivative.pyaw.eval(t) 85 | 86 | # 2nd derivative 87 | derivative2 = derivative.derivative() 88 | result.acc = np.array([derivative2.px.eval(t), derivative2.py.eval(t), derivative2.pz.eval(t)]) 89 | 90 | # 3rd derivative 91 | derivative3 = derivative2.derivative() 92 | jerk = np.array([derivative3.px.eval(t), derivative3.py.eval(t), derivative3.pz.eval(t)]) 93 | 94 | thrust = result.acc + np.array([0, 0, 9.81]) # add gravity 95 | 96 | z_body = normalize(thrust) 97 | x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0]) 98 | y_body = normalize(np.cross(z_body, x_world)) 99 | x_body = np.cross(y_body, z_body) 100 | 101 | jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body) 102 | h_w = jerk_orth_zbody / np.linalg.norm(thrust) 103 | 104 | result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw]) 105 | 106 | # compute required roll/pitch angles 107 | result.pitch = np.arcsin(-x_body[2]) 108 | result.roll = np.arctan2(y_body[2], z_body[2]) 109 | 110 | return result 111 | 112 | 113 | class Trajectory: 114 | def __init__(self): 115 | self.polynomials = None 116 | self.duration = None 117 | 118 | def loadcsv(self, filename): 119 | data = np.loadtxt(filename, delimiter=",", skiprows=1, usecols=range(33)) 120 | self.polynomials = [Polynomial4D(row[0], row[1:9], row[9:17], row[17:25], row[25:33]) for row in data] 121 | self.duration = np.sum(data[:,0]) 122 | 123 | def savecsv(self, filename): 124 | data = np.empty((len(self.polynomials), 8*4+1)) 125 | for i, p in enumerate(self.polynomials): 126 | data[i,0] = p.duration 127 | data[i,1:9] = p.px.p 128 | data[i,9:17] = p.py.p 129 | data[i,17:25] = p.pz.p 130 | data[i,25:33] = p.pyaw.p 131 | np.savetxt(filename, data, fmt="%.6f", delimiter=",", header="duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7") 132 | 133 | def stretchtime(self, factor): 134 | for p in self.polynomials: 135 | p.stretchtime(factor) 136 | self.duration *= factor 137 | 138 | def eval(self, t): 139 | assert t >= 0 140 | assert t <= self.duration 141 | 142 | current_t = 0.0 143 | for p in self.polynomials: 144 | if t < current_t + p.duration: 145 | return p.eval(t - current_t) 146 | current_t = current_t + p.duration 147 | -------------------------------------------------------------------------------- /bswarm/trajectory.py: -------------------------------------------------------------------------------- 1 | # %% [markdown] 2 | # # Polynomial 3 | # 4 | # $p(t) = c_0 + c_1 t + c_2 t^2 \dots = \sum\limits_{k=0}^{n-1} c_k t^k$ 5 | # 6 | # where $c_k$ are constant coefficients 7 | # 8 | # ## Time Scaling 9 | # 10 | # We can consider time scaling by a factor of $T$. 11 | # 12 | # $p(t/T) = \sum\limits_{k=0}^{n-1} (c_k/T^k) t^k$ 13 | # 14 | # ## Derivative 15 | # 16 | # $p' = \sum\limits_{k=0}^{n-1} (c_k k) t^{k-1} = \sum\limits_{k=1}^{n-1} (c_k k) t^{k-1} $ 17 | # 18 | # $p'' = \sum\limits_{k=2}^{n-1} (c_k k (k-1)) t^{k-2} $ 19 | # 20 | # $p^{(3)} = \sum\limits_{k=3}^{n-1} (c_k k (k-1) (k-2)) t^{k-3} $ 21 | # 22 | # $p^{(m)} = \sum\limits_{k=m}^{n-1} \dfrac{c_k k!}{(k-m)!} t^{k-m} $ 23 | 24 | # %% 25 | import numpy as np 26 | import math 27 | import dataclasses 28 | import json 29 | from numpy.polynomial import Polynomial 30 | import matplotlib.pyplot as plt 31 | import mpl_toolkits.mplot3d.axes3d as p3 32 | import matplotlib.animation as animation 33 | 34 | from typing import List 35 | 36 | 37 | class Trajectory1D: 38 | 39 | def __init__(self, T: List[float], P: List[Polynomial]): 40 | assert len(T) == len(P) 41 | self.T: List[float] = T 42 | self.P: List[Polynomial] = P 43 | 44 | def derivative(self, m): 45 | return Trajectory1D(self.T, [Pi.deriv(m) for Pi in self.P]) 46 | 47 | def time_scale(self, scale: float): 48 | n = len(self.P[0].coef) 49 | coef_div = np.array([scale**k for k in range(n)]) 50 | P_scaled = [Polynomial(Pi.coef / coef_div) for Pi in self.P] 51 | T_scaled = (np.array(self.T) * scale).tolist() 52 | return Trajectory1D(T_scaled, P_scaled) 53 | 54 | def coef_array(self) -> np.array: 55 | data = [] 56 | for Ti, xi in zip(self.T, self.P): 57 | row = np.hstack([Ti, xi.coef]) 58 | data.append(row) 59 | return np.array(data) 60 | 61 | def eval(self): 62 | t_list = [] 63 | y_list = [] 64 | S = np.hstack([0, np.cumsum(self.T)]) 65 | legs = len(self.T) 66 | for leg, Pi in enumerate(self.P): 67 | gamma = np.arange(0, self.T[leg], 0.1) 68 | t_list.append(gamma + S[leg]) 69 | y_list.append(Pi(gamma)) 70 | return np.hstack(t_list), np.hstack(y_list) 71 | 72 | 73 | @dataclasses.dataclass 74 | class TrajectoryOutput: 75 | t: np.array # time [s] 76 | pos: np.array # position [m] 77 | vel: np.array # velocity [m/s] 78 | acc: np.array # acceleration [m/s^2] 79 | omega: np.array # angular velocity [rad/s] 80 | yaw: np.array # yaw angle [rad] 81 | roll: np.array # required roll angle [rad] 82 | pitch: np.array # required pitch angle [rad] 83 | 84 | def max_data(self): 85 | max_speed = np.max(np.linalg.norm(self.vel, axis=1)) 86 | max_acc = np.max(np.linalg.norm(self.acc, axis=1)) 87 | max_omega = np.max(np.linalg.norm(self.omega, axis=1)) 88 | max_roll = np.rad2deg(np.max(self.roll)) 89 | max_pitch = np.rad2deg(np.max(self.pitch)) 90 | return \ 91 | "max speed (m/s)\t\t\t: {max_speed:10g}\n" \ 92 | "max acceleration (m/s^2)\t: {max_acc:10g}\n" \ 93 | "max omega (rad/s)\t\t: {max_omega:10g}\n" \ 94 | "max roll (deg)\t\t\t: {max_roll:10g}\n" \ 95 | "max pitch (deg)\t\t\t: {max_pitch:10g}\n".format(**locals()) 96 | 97 | 98 | class Trajectory4D: 99 | 100 | def __init__(self, x: Trajectory1D, y: Trajectory1D, z: Trajectory1D, yaw: Trajectory1D): 101 | self.x: Trajectory1D = x 102 | self.y: Trajectory1D = y 103 | self.z: Trajectory1D = z 104 | self.yaw: Trajectory1D = yaw 105 | self.T: List[float] = x.T 106 | assert np.all(x.T == y.T) 107 | assert np.all(x.T == z.T) 108 | assert np.all(x.T == yaw.T) 109 | 110 | def time_scale(self, scale: float): 111 | return Trajectory4D( 112 | self.x.time_scale(scale), 113 | self.y.time_scale(scale), 114 | self.z.time_scale(scale), 115 | self.yaw.time_scale(scale)) 116 | 117 | def derivative(self, m): 118 | return Trajectory4D( 119 | self.x.derivative(m), 120 | self.y.derivative(m), 121 | self.z.derivative(m), 122 | self.yaw.derivative(m)) 123 | 124 | def eval(self): 125 | t, x = self.x.eval() 126 | y = self.y.eval()[1] 127 | z = self.z.eval()[1] 128 | yaw = self.yaw.eval()[1] 129 | return t, np.vstack([x, y, z, yaw]).T 130 | 131 | def compute_inputs(self) -> TrajectoryOutput: 132 | # flat variables 133 | t, p = self.eval() 134 | v = self.derivative(1).eval()[1] 135 | a = self.derivative(2).eval()[1] 136 | j = self.derivative(3).eval()[1] 137 | 138 | pos = p[:, :3] 139 | yaw = p[:, 3] 140 | vel = v[:, :3] 141 | dyaw = v[:, 3] 142 | acc = a[:, :3] 143 | jerk = j[:, :3] 144 | 145 | thrust = acc + np.array([0, 0, 9.81]) # add gravity 146 | 147 | def normalize(v): 148 | n = np.linalg.norm(v, axis=1) 149 | assert np.min(np.abs(n)) > 0 150 | return (v.T / n).T 151 | 152 | z_body = normalize(thrust) 153 | x_world = np.array([np.cos(yaw), np.sin(yaw), np.zeros(len(yaw))]).T 154 | y_body = normalize(np.cross(z_body, x_world)) 155 | x_body = np.cross(y_body, z_body) 156 | 157 | def vector_dot_product(a, b): 158 | d = np.multiply(a, b).sum(1) 159 | return np.vstack([d, d, d]).T 160 | 161 | dprod = vector_dot_product(jerk, z_body) 162 | jerk_orth_zbody = jerk - np.multiply(dprod, z_body) 163 | h_w = (jerk_orth_zbody.T / np.linalg.norm(thrust, axis=1)).T 164 | 165 | omega = np.array([ 166 | -np.multiply(h_w, y_body).sum(1), 167 | np.multiply(h_w, x_body).sum(1), 168 | np.multiply(z_body[:, 2], dyaw)]).T 169 | 170 | # compute required roll/pitch angles 171 | pitch = np.arcsin(-x_body[:, 2]) 172 | roll = np.arctan2(y_body[:, 2], z_body[:, 2]) 173 | 174 | return TrajectoryOutput(t, pos, vel, acc, omega, yaw, roll, pitch) 175 | 176 | def coef_array(self) -> np.array: 177 | data = [] 178 | for Ti, xi, yi, zi in zip(self.T, self.x.P, self.y.P, self.z.P): 179 | row = np.hstack([Ti, xi.coef, yi.coef, zi.coef, np.zeros(8)]) 180 | data.append(row) 181 | return np.array(data) 182 | 183 | def write_csv(self, filename: str) -> None: 184 | header = "Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6," \ 185 | "y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7" 186 | np.savetxt(filename, self.coef_array(), 187 | delimiter=',', fmt='%15g', header=header) 188 | 189 | 190 | def min_deriv_1d(deriv: int, waypoints: List[List[float]], T: List[float], stop: bool) -> Trajectory1D: 191 | n = deriv * 2 # number of poly coeff (order + 1) 192 | S = np.hstack([0, np.cumsum(T)]) 193 | legs = len(T) 194 | assert len(waypoints) == len(T) + 1 195 | 196 | def coef_weights(t: float, m: int, t0: float): 197 | """ 198 | Polynomial coefficient weights 199 | :param t: time 200 | :param m: derivative order 201 | """ 202 | w = np.zeros(n) 203 | for k in range(m, n): 204 | w[k] = (t - t0)**(k - m) * math.factorial(k) / \ 205 | math.factorial(k - m) 206 | return w 207 | 208 | b = np.zeros(n * legs) 209 | A = np.zeros((n * legs, n * legs)) 210 | eq = 0 211 | for leg in range(legs): 212 | # first waypoint 213 | if leg == 0: 214 | for m in range(n // 2): 215 | A[eq, n * leg:n * (leg + 1) 216 | ] = coef_weights(t=S[leg], m=m, t0=S[leg]) 217 | if m == 0: 218 | b[eq] = waypoints[leg] 219 | else: 220 | b[eq] = 0 221 | eq += 1 222 | # any waypoint except for first 223 | else: 224 | for m in range(n // 2 - 1): 225 | if m == 0: 226 | A[eq, n * leg:n * 227 | (leg + 1)] = coef_weights(t=S[leg], m=m, t0=S[leg]) 228 | b[eq] = waypoints[leg] 229 | eq += 1 230 | elif stop: 231 | A[eq, n * leg:n * 232 | (leg + 1)] = coef_weights(t=S[leg], m=m, t0=S[leg]) 233 | b[eq] = 0 234 | eq += 1 235 | 236 | # last waypoint only 237 | if leg == legs - 1: 238 | for m in range(n // 2): 239 | A[eq, n * leg:n * 240 | (leg + 1)] = coef_weights(t=S[leg + 1], m=m, t0=S[leg]) 241 | if m == 0: 242 | b[eq] = waypoints[leg + 1] 243 | else: 244 | b[eq] = 0 245 | eq += 1 246 | 247 | # continuity 248 | if leg > 0: 249 | for m in range(n // 2 + 1): 250 | A[eq, n * (leg - 1):n * leg] = coef_weights(t=S[leg], 251 | m=m, t0=S[leg - 1]) 252 | A[eq, n * leg:n * (leg + 1)] = - \ 253 | coef_weights(t=S[leg], m=m, t0=S[leg]) 254 | b[eq] = 0 255 | eq += 1 256 | 257 | if eq != n * legs: 258 | print('warning: equations: {:d}, coefficients: {:d}'.format( 259 | eq, n * legs)) 260 | c = np.linalg.pinv(A).dot(b) 261 | P_list = [] 262 | for leg in range(legs): 263 | Pi = Polynomial(c[n * leg:n * (leg + 1)]) 264 | P_list.append(Pi) 265 | return Trajectory1D(T, P_list) 266 | 267 | 268 | def min_deriv_4d(deriv: int, waypoints: List[List[float]], T: List[float], stop: bool) -> Trajectory4D: 269 | traj_x = min_deriv_1d(deriv, waypoints[:, 0], T, stop) 270 | traj_y = min_deriv_1d(deriv, waypoints[:, 1], T, stop) 271 | traj_z = min_deriv_1d(deriv, waypoints[:, 2], T, stop) 272 | traj_yaw = min_deriv_1d(deriv, waypoints[:, 3], T, stop) 273 | return Trajectory4D(traj_x, traj_y, traj_z, traj_yaw) 274 | 275 | 276 | def min_snap_1d(waypoints: List[List[float]], T: List[float], stop: bool) -> Trajectory1D: 277 | return min_deriv_1d(4, waypoints, T, stop) 278 | 279 | 280 | def min_snap_4d(waypoints: List[List[float]], T: List[float], stop: bool) -> Trajectory4D: 281 | return min_deriv_4d(4, waypoints, T, stop) 282 | 283 | 284 | def min_accel_1d(waypoints: List[List[float]], T: List[float], stop: bool) -> Trajectory1D: 285 | return min_deriv_1d(2, waypoints, T, stop) 286 | 287 | 288 | def min_accel_4d(waypoints: List[List[float]], T: List[float], stop: bool) -> Trajectory4D: 289 | return min_deriv_4d(2, waypoints, T, stop) 290 | 291 | 292 | def plot_trajectories_time_history(trajectories: List[Trajectory4D]) -> None: 293 | names = ['pos', 'vel', 'acc', 'jerk', 'snap'] 294 | for i, name in enumerate(names): 295 | plt.subplot(len(names), 1, i + 1) 296 | for traj in trajectories: 297 | plt.plot(*traj.derivative(i + 1).eval()) 298 | plt.xlabel('t, sec') 299 | plt.grid() 300 | plt.ylabel(name) 301 | 302 | 303 | def plot_trajectories_magnitudes(trajectories: List[Trajectory4D]) -> None: 304 | names = ['pos', 'vel', 'acc', 'jerk', 'snap'] 305 | for i, name in enumerate(names): 306 | plt.subplot(len(names), 1, i + 1) 307 | for traj in trajectories: 308 | t, a = traj.derivative(i).eval() 309 | plt.plot(t, np.linalg.norm(a, axis=1)) 310 | plt.xlabel('t, sec') 311 | plt.ylabel(name) 312 | 313 | 314 | def plot_trajectories(trajectories: List[Trajectory4D]) -> None: 315 | dataLines = [] 316 | for traj in trajectories: 317 | x = traj.eval()[1] 318 | dataLines.append(x.T) 319 | 320 | fig = plt.gcf() 321 | ax = p3.Axes3D(fig) 322 | ax.set_xlabel('x, m') 323 | ax.set_ylabel('y, m') 324 | ax.set_zlabel('z, m') 325 | border = 0.1 326 | all_x = np.array([dat[0, :] for dat in dataLines]) 327 | ax.set_xlim3d(np.floor(np.min(all_x) - border), 328 | np.ceil(np.max(all_x) + border)) 329 | all_y = np.array([dat[1, :] for dat in dataLines]) 330 | ax.set_ylim3d(np.floor(np.min(all_y) - border), 331 | np.ceil(np.max(all_y) + border)) 332 | all_z = np.array([dat[2, :] for dat in dataLines]) 333 | ax.set_zlim3d(np.floor(np.min(all_z) - border), 334 | np.ceil(np.max(all_z) + border)) 335 | lines = [ax.plot(dat[0, :], dat[1, :], dat[2, :])[0] for dat in dataLines] 336 | 337 | def animate_trajectories(filename, trajectories, fps): 338 | # Attaching 3D axis to the figure 339 | fig = plt.figure(figsize=(10, 10)) 340 | ax = p3.Axes3D(fig) 341 | ax.set_xlabel('x, m') 342 | ax.set_ylabel('y, m') 343 | ax.set_zlabel('z, m') 344 | 345 | # create trajectory data 346 | dataLines = [] 347 | for traj in trajectories: 348 | x = traj.eval()[1] 349 | dataLines.append(x.T) 350 | lines = [ax.plot([dat[0, 0]], [dat[1, 0]], [dat[2, 0]], 351 | 'ro', markersize=10)[0] for dat in dataLines] 352 | 353 | fps = 5 # frames per second 354 | data_period = 0.1 # data period, seconds 355 | data_length = dataLines[0].shape[1] 356 | duration = data_period * data_length 357 | frames = int(np.floor(duration * fps)) 358 | step = data_length // frames 359 | 360 | def update_lines(num, dataLines, lines): 361 | for line, data in zip(lines, dataLines): 362 | # NOTE: there is no .set_data() for 3 dim data... 363 | line.set_data([ 364 | [data[0, num * step]], 365 | [data[1, num * step]], 366 | ]) 367 | line.set_3d_properties([data[2, num * step]]) 368 | return lines 369 | 370 | border = 0.1 371 | all_x = np.array([dat[0, :] for dat in dataLines]) 372 | ax.set_xlim3d(np.floor(np.min(all_x) - border), 373 | np.ceil(np.max(all_x) + border)) 374 | all_y = np.array([dat[1, :] for dat in dataLines]) 375 | ax.set_ylim3d(np.floor(np.min(all_y) - border), 376 | np.ceil(np.max(all_y) + border)) 377 | all_z = np.array([dat[2, :] for dat in dataLines]) 378 | ax.set_zlim3d(np.floor(np.min(all_z) - border), 379 | np.ceil(np.max(all_z) + border)) 380 | 381 | ani = animation.FuncAnimation( 382 | fig, update_lines, frames, fargs=(dataLines, lines), 383 | interval=int(1000 / fps), blit=False) 384 | ani.save(filename) 385 | plt.close() 386 | 387 | 388 | def trajectories_to_json(trajectories: List[Trajectory4D]): 389 | formation = {} 390 | for drone, traj in enumerate(trajectories): 391 | formation[drone] = traj.coef_array().tolist() 392 | return formation 393 | -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: bswarm 2 | channels: 3 | - defaults 4 | - conda-forge 5 | dependencies: 6 | - pip 7 | - patchelf 8 | - cx_freeze 9 | - matplotlib 10 | - pip: 11 | - cfclient 12 | -------------------------------------------------------------------------------- /scripts/.ipynb_checkpoints/3_drone_form-checkpoint.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | sys.path.insert(0, os.getcwd()) 5 | os.chdir('../') 6 | 7 | from mpl_toolkits import mplot3d 8 | import matplotlib.pyplot as plt 9 | from matplotlib.animation import FuncAnimation 10 | 11 | import numpy as np 12 | import bswarm.trajectory as tgen 13 | import bswarm.formation as formation 14 | import bswarm 15 | import json 16 | 17 | def plot_formation(F, name): 18 | plt.figure() 19 | ax = plt.axes(projection='3d') 20 | ax.plot3D(F[0, :], F[1, :], F[2, :], 'ro') 21 | 22 | plt.title(name) 23 | ax.set_xlabel('x, m') 24 | ax.set_ylabel('y, m') 25 | ax.set_zlabel('z, m') 26 | plt.show() 27 | 28 | def scale_formation(form, scale): 29 | formNew = np.copy(form) 30 | for i in range(3): 31 | formNew[i, :] *= scale[i] 32 | return formNew 33 | 34 | #%% parameters 35 | 36 | # defines how the drones are ordered for circles etc. 37 | rotation_order = [2, 1, 0] 38 | 39 | # scaling for formation 40 | form_scale = np.array([1.5, 1.5, 1]) 41 | 42 | # the takeoff formation 43 | formTakeoff = np.array([ 44 | [-1, 0, 0], 45 | [0, 0, 0], 46 | [1, 0, 0], 47 | ]).T 48 | n_drones = formTakeoff.shape[1] 49 | 50 | plot_formation(formTakeoff, 'takeoff') 51 | 52 | # points = [] 53 | # for i_drone in rotation_order: 54 | # theta = i_drone*2*np.pi/n_drones 55 | # points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 56 | # formCircle = scale_formation(np.array(points).T, form_scale) 57 | # plot_formation(formCircle, 'circle') 58 | 59 | # points = [] 60 | # for i_drone in rotation_order: 61 | # theta = i_drone*2*np.pi/n_drones 62 | # if i_drone % 2 == 0: 63 | # points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 64 | # else: 65 | # points.append([0.25*np.cos(theta), 0.25*np.sin(theta), 0]) 66 | # formTriangle = scale_formation(np.array(points).T, form_scale) 67 | # plot_formation(formTriangle, 'triangle') 68 | 69 | # %% 70 | class Geometry: 71 | 72 | rgb = { 73 | 'black': [0, 0, 0], 74 | 'gold': [255, 100, 15], 75 | 'red': [255, 0, 0], 76 | 'green': [0, 255, 0], 77 | 'blue': [0, 0, 255], 78 | 'white': [255, 255, 255] 79 | } 80 | 81 | def __init__(self): 82 | self.waypoints = [formTakeoff] 83 | self.T = [] 84 | self.delays = [] 85 | self.colors = [] 86 | 87 | @staticmethod 88 | def sin_z(form, t): 89 | new_form = np.array(form) 90 | n_drones = form.shape[1] 91 | for i in rotation_order: 92 | new_form[2, i] = np.sin(t + i*2*np.pi/n_drones) 93 | return new_form 94 | 95 | def sin_wave(self, form, n, duration, color=None): 96 | for t in np.linspace(0, duration, n): 97 | formSin = self.sin_z(form, t*np.pi/4) 98 | self.waypoints.append(formSin) 99 | self.T.append(duration/n) 100 | n_drones = form.shape[1] 101 | self.delays.append(np.zeros(n_drones).tolist()) 102 | 103 | if color != None: 104 | self.colors.append(self.rgb[color]) 105 | 106 | def spiral(self, form, z, n, duration, color=None): 107 | for t in np.linspace(0, 1, n): 108 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 109 | shift = np.array([[0, 0, z*np.sin(t)]]).T 110 | self.waypoints.append(rot_form + shift) 111 | self.T.append(duration/n) 112 | n_drones = form.shape[1] 113 | self.delays.append(np.zeros(n_drones).tolist()) 114 | 115 | if color != None: 116 | self.colors.append(self.rgb[color]) 117 | 118 | def rotate(self, form, n, duration, color=None): 119 | for t in np.linspace(0, 1, n): 120 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 121 | self.waypoints.append(rot_form) 122 | self.T.append(duration/n) 123 | n_drones = form.shape[1] 124 | self.delays.append(np.zeros(n_drones).tolist()) 125 | 126 | if color != None: 127 | self.colors.append(self.rgb[color]) 128 | 129 | def goto(self, form, duration, color=None): 130 | self.waypoints.append(form) 131 | self.T.append(duration) 132 | n_drones = form.shape[1] 133 | self.delays.append(np.zeros(n_drones).tolist()) 134 | 135 | if color != None: 136 | self.colors.append(self.rgb[color]) 137 | 138 | def plan_trajectory(self): 139 | trajectories = [] 140 | origin = np.array([1.5, 2, 2]) 141 | self.waypoints = np.array(self.waypoints) 142 | for drone in range(self.waypoints.shape[2]): 143 | pos_wp = self.waypoints[:, :, drone] + origin 144 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 145 | traj = tgen.min_deriv_4d(4, 146 | np.hstack([pos_wp, yaw_wp]), self.T, stop=False) 147 | trajectories.append(traj) 148 | 149 | traj_json = tgen.trajectories_to_json(trajectories) 150 | data = {} 151 | for key in traj_json.keys(): 152 | data[key] = { 153 | 'trajectory': traj_json[key], 154 | 'T': self.T[key], 155 | 'color': g.colors, 156 | 'delay': [d[key] for d in g.delays] 157 | } 158 | data['repeat'] = 3 159 | assert len(trajectories) < 32 160 | return trajectories, data 161 | 162 | # create trajectory waypoints 163 | g = Geometry() 164 | g.sin_wave(form=formTakeoff, n=8, duration=16) 165 | # g.goto(form=formCircle, duration=2, color='blue') 166 | # g.rotate(form=formCircle, n=6, duration=12, color='green') 167 | # g.rotate(form=formTriangle, n=6, duration=12, color='white') 168 | # g.goto(form=formCircle, duration=2, color='gold') 169 | # g.spiral(form=formCircle, z=1, n=6, duration=12, color='red') 170 | g.goto(formTakeoff, 2) 171 | 172 | #%% plan trajectories 173 | trajectories, data = g.plan_trajectory() 174 | 175 | with open('scripts/data/geometry.json', 'w') as f: 176 | json.dump(data, f) 177 | 178 | tgen.plot_trajectories(trajectories) 179 | tgen.animate_trajectories('geometry.mp4', trajectories, 1) 180 | 181 | #%% 182 | plt.figure() 183 | tgen.plot_trajectories_time_history(trajectories) 184 | plt.show() 185 | 186 | #%% 187 | plt.figure() 188 | tgen.plot_trajectories_magnitudes(trajectories) 189 | plt.show() 190 | 191 | #%% 192 | print('number of segments', len(trajectories[0].coef_array())) 193 | #%% 194 | plt.figure() 195 | plt.title('durations') 196 | plt.bar(range(len(g.T)), g.T) 197 | plt.show() 198 | 199 | 200 | #%% 201 | -------------------------------------------------------------------------------- /scripts/P_formation.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | sys.path.insert(0, os.getcwd()) 5 | os.chdir('../') 6 | 7 | from mpl_toolkits import mplot3d 8 | import matplotlib.pyplot as plt 9 | from matplotlib.animation import FuncAnimation 10 | 11 | import numpy as np 12 | import bswarm.trajectory as tgen 13 | import bswarm.formation 14 | import bswarm 15 | import json 16 | 17 | class Formation: 18 | 19 | def __init__(self, points, order): 20 | self.points = points 21 | self.order = order 22 | 23 | def plot_formation(F: Formation, name): 24 | plt.figure() 25 | ax = plt.axes(projection='3d') 26 | points = F.points 27 | for i in range(points.shape[1]): 28 | ax.text3D(points[0, i], points[1, i], points[2, i], str(i)) 29 | ax.plot3D([points[0, i]], [points[1, i]], [points[2, i]], 'r.') 30 | plt.title(name) 31 | plt.show() 32 | 33 | def scale_formation(form, scale): 34 | formNew = np.copy(form) 35 | for i in range(3): 36 | formNew[i, :] *= scale[i] 37 | return formNew 38 | 39 | #%% takeoff 40 | formations = {} 41 | formations['takeoff'] = Formation( 42 | points = np.array([ 43 | [-0.5, -1, 0], 44 | [-0.5, 0, 0], 45 | [-0.5, 1, 0], 46 | [0.5, -1, 0], 47 | [0.5, 0, 0], 48 | [0.5, 1, 0], 49 | ]).T, 50 | order=[0, 1, 2, 3, 4, 5]) 51 | plot_formation(formations['takeoff'], 'takeoff') 52 | 53 | 54 | 55 | #%% P 56 | letter_scale = np.array([1.5, 1.5, 1.5]) 57 | formations['P'] = Formation( 58 | points=scale_formation(np.array([ 59 | [-0.5, -0.5, 1], 60 | [-0.5, 0, 1], 61 | [-0.25, 0.5, 0.75], 62 | [0, -0.5, 0.5], 63 | [0.5, -0.5, 0], 64 | [0, 0, 0.5], 65 | ]).T, letter_scale), 66 | order=[4, 3, 0, 1, 2, 5]) 67 | 68 | plot_formation(formations['P'], 'P') 69 | 70 | #%% U 71 | formations['U'] = Formation( 72 | points=scale_formation(np.array([ 73 | [-0.5, -0.5, 1], 74 | [-0.5, 0.5, 1], 75 | [0, 0.5, 0.5], 76 | [0, -0.5, 0.5], 77 | [0.5, -0.25, 0], 78 | [0.5, 0.25, 0], 79 | ]).T, letter_scale), 80 | order=[0, 3, 4, 5, 2, 1]) 81 | plot_formation(formations['U'], 'U') 82 | 83 | #%% 5 84 | formations['5'] = Formation( 85 | points=scale_formation(np.array([ 86 | [-0.5, -0.5, 1], 87 | [-0.5, 0.5, 1], 88 | [0, 0.25, 0.5], 89 | [0, -0.5, 0.5], 90 | [0.5, -0.5, 0], 91 | [0.5, 0.25, 0], 92 | ]).T, letter_scale), 93 | order=[1, 0, 3, 2, 5, 4]) 94 | plot_formation(formations['5'], '5') 95 | 96 | #%% O 97 | formations['O'] = Formation( 98 | points=scale_formation(np.array([ 99 | [-0.5, -0.25, 1], 100 | [-0.5, 0.25, 1], 101 | [0, 0.5, 0.5], 102 | [0, -0.5, 0.5], 103 | [0.5, -0.25, 0], 104 | [0.5, 0.25, 0] 105 | ]).T, letter_scale), 106 | order=[0, 1, 2, 5, 4, 3]) 107 | plot_formation(formations['O'], 'O') 108 | 109 | #%% A 110 | formations['A'] = Formation( 111 | points=scale_formation(np.array([ 112 | [0, -0.5, 0.5], 113 | [-0.5, 0, 1], 114 | [0, 0.5, 0.5], 115 | [0.5, -0.5, 0], 116 | [0, 0, 0.5], 117 | [0.5, 0.5, 0] 118 | ]).T, letter_scale), 119 | order=[3, 0, 1, 2, 5, 4]) 120 | plot_formation(formations['A'], 'A') 121 | 122 | #%% L 123 | formations['L'] = Formation( 124 | points=scale_formation(np.array([ 125 | [-0.5, -0.5, 1], 126 | [0, -0.5, 0.5], 127 | [0, 0.5, 0.5], 128 | [0.5, -0.5, 0], 129 | [0.5, 0, 0], 130 | [0.5, 0.5, 0] 131 | ]).T, letter_scale), 132 | order=[0, 1, 3, 4, 5, 2]) 133 | plot_formation(formations['L'], 'L') 134 | 135 | #%% 1 136 | formations['1'] = Formation( 137 | points=scale_formation(np.array([ 138 | [-0.5, -0.5, 1], 139 | [-0.5, 0, 1], 140 | [0, 0, 0.5], 141 | [0.5, -0.5, 0], 142 | [0.5, 0, 0], 143 | [0.5, 0.5, 0], 144 | ]).T, letter_scale), 145 | order=[0, 1, 2, 4, 3, 5]) 146 | plot_formation(formations['1'], '1') 147 | 148 | #%% 149 | class Letters: 150 | 151 | rgb = { 152 | 'black': [0, 0, 0], 153 | 'gold': [255, 100, 15], 154 | 'red': [255, 0, 0], 155 | 'green': [0, 255, 0], 156 | 'blue': [0, 0, 255], 157 | 'white': [255, 255, 255] 158 | } 159 | 160 | def __init__(self): 161 | self.waypoints = [formations['takeoff'].points] 162 | self.T = [] 163 | self.delays = [] 164 | self.colors = [] 165 | 166 | def add(self, formation_name: str, color: str, duration: float, led_delay: float, angle: float=0): 167 | formation = formations[formation_name] 168 | assert led_delay*len(formation.order) < duration 169 | self.T.append(duration) 170 | self.waypoints.append(bswarm.formation.rotate_points_z(formation.points, angle)) 171 | delay = [] 172 | order = np.array(formation.order) 173 | delay = np.zeros(len(formation.order)) 174 | for i, drone in enumerate(formation.order): 175 | delay[drone] = i*led_delay 176 | self.delays.append(delay.tolist()) 177 | self.colors.append(self.rgb[color]) 178 | 179 | def plan_trajectory(self, origin): 180 | trajectories = [] 181 | waypoints = np.array(self.waypoints) 182 | assert len(waypoints) < 33 183 | for drone in range(waypoints.shape[2]): 184 | pos_wp = waypoints[:, :, drone] + origin 185 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 186 | traj = tgen.min_deriv_4d(4, 187 | np.hstack([pos_wp, yaw_wp]), self.T, stop=False) 188 | trajectories.append(traj) 189 | assert len(trajectories) < 32 190 | return trajectories 191 | 192 | def plan_letters(letter_string: str): 193 | letters = Letters() 194 | letters.add('takeoff', color='blue', duration=2, led_delay=0) 195 | #%% Create waypoints for flat P -> slanted P -> rotating slanted P -> flat P 196 | for i, letter in enumerate(letter_string.split(' ')): 197 | letters.add('takeoff', color='blue', duration=5, led_delay=0) 198 | letters.add(letter, color='blue', duration=5, led_delay=0) 199 | letters.add(letter, color='gold', duration=5, led_delay=0.5) 200 | if i == 7: 201 | for theta in np.linspace(0, 2*np.pi, 5)[1:]: 202 | letters.add(letter, color='gold', duration=3, led_delay=0, angle=theta) 203 | letters.add('takeoff', color='blue', duration=5, led_delay=0) 204 | letters.add('takeoff', color='white', duration=10, led_delay=0.5) 205 | 206 | trajectories = letters.plan_trajectory(origin=np.array([1.5, 2, 2])) 207 | traj_json = tgen.trajectories_to_json(trajectories) 208 | data = {} 209 | for key in traj_json.keys(): 210 | data[key] = { 211 | 'trajectory': traj_json[key], 212 | 'T': letters.T, 213 | 'color': letters.colors, 214 | 'delay': [d[key] for d in letters.delays] 215 | } 216 | data['repeat'] = 1 217 | return trajectories, data 218 | 219 | trajectories, data = plan_letters('P U A P O L L O') 220 | 221 | with open('scripts/data/p_form.json', 'w') as f: 222 | json.dump(data, f) 223 | 224 | tgen.plot_trajectories(trajectories) 225 | plt.show() 226 | 227 | 228 | #%% 229 | plt.figure() 230 | tgen.plot_trajectories_time_history(trajectories) 231 | plt.show() 232 | 233 | #%% 234 | plt.figure() 235 | tgen.plot_trajectories_magnitudes(trajectories) 236 | plt.show() 237 | 238 | #%% 239 | print('number of segments', len(trajectories[0].coef_array())) 240 | #%% 241 | plt.figure() 242 | plt.title('durations') 243 | plt.bar(range(len(data[0]['T'])), data[0]['T']) 244 | plt.show() 245 | 246 | #%% 247 | 248 | #tgen.animate_trajectories('p_formation.mp4', trajectories, fps=5) 249 | 250 | 251 | #%% 252 | -------------------------------------------------------------------------------- /scripts/block_land.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | sys.path.insert(0, os.getcwd()) 5 | os.chdir('../') 6 | 7 | from mpl_toolkits import mplot3d 8 | import matplotlib.pyplot as plt 9 | from matplotlib.animation import FuncAnimation 10 | 11 | import numpy as np 12 | import bswarm.trajectory as tgen 13 | import bswarm.formation 14 | import bswarm 15 | import json 16 | 17 | 18 | origin = np.array([0, 1, 2]) 19 | waypoints = [] 20 | 21 | # waypoint 1 22 | waypoints.append( 23 | np.array([ 24 | # drone 1 25 | [0, 0, 0] 26 | ]).T) 27 | 28 | # waypoint 2 29 | waypoints.append( 30 | np.array([ 31 | # drone 1 32 | [1, 0, 0] 33 | ]).T) 34 | 35 | # waypoint 3 36 | waypoints.append( 37 | np.array([ 38 | # drone 1 39 | [2, 0, 0] 40 | ]).T) 41 | 42 | waypoints = np.array(waypoints) 43 | 44 | rgb = { 45 | 'black': [0, 0, 0], 46 | 'gold': [255, 100, 15], 47 | 'red': [255, 0, 0], 48 | 'green': [0, 255, 0], 49 | 'blue': [0, 0, 255], 50 | 'white': [255, 255, 255] 51 | } 52 | 53 | 54 | # time to take on each leg, must be length of waypoints - 1 55 | time_vector = [5, 5] 56 | 57 | # color vector, must be length of waypoints - 1 58 | colors = [ 59 | rgb['white'], 60 | rgb['blue'], 61 | ] 62 | 63 | # delay, must be length of waypoints - 1 64 | delays = [0, 0] 65 | 66 | assert len(waypoints) < 33 67 | trajectories = [] 68 | for drone in range(waypoints.shape[2]): 69 | print(waypoints[:, :, drone]) 70 | pos_wp = waypoints[:, :, drone] + origin 71 | print(pos_wp) 72 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 73 | traj = tgen.min_deriv_4d(4, 74 | np.hstack([pos_wp, yaw_wp]), time_vector, stop=False) 75 | trajectories.append(traj) 76 | assert len(trajectories) < 32 77 | traj_json = tgen.trajectories_to_json(trajectories) 78 | data = {} 79 | for key in traj_json.keys(): 80 | data[key] = { 81 | 'trajectory': traj_json[key], 82 | 'T': time_vector, 83 | 'color': colors, 84 | 'delay': delays 85 | } 86 | # how many times to repeat trajectory 87 | data['repeat'] = 1 88 | assert len(trajectories) < 32 89 | assert len(time_vector) < 32 90 | 91 | 92 | 93 | with open('scripts/data/block_land.json', 'w') as f: 94 | json.dump(data, f) 95 | 96 | tgen.plot_trajectories(trajectories) 97 | plt.show() 98 | 99 | 100 | #%% 101 | plt.figure() 102 | tgen.plot_trajectories_time_history(trajectories) 103 | plt.show() 104 | 105 | #%% 106 | plt.figure() 107 | tgen.plot_trajectories_magnitudes(trajectories) 108 | plt.show() 109 | 110 | #%% 111 | print('number of segments', len(trajectories[0].coef_array())) 112 | #%% 113 | plt.figure() 114 | plt.title('durations') 115 | plt.bar(range(len(data[0]['T'])), data[0]['T']) 116 | plt.show() 117 | 118 | #%% 119 | 120 | #tgen.animate_trajectories('p_formation.mp4', trajectories, fps=5) 121 | 122 | 123 | #%% 124 | -------------------------------------------------------------------------------- /scripts/chacha.py: -------------------------------------------------------------------------------- 1 | # %% 2 | import json 3 | import bswarm 4 | import bswarm.formation as formation 5 | import bswarm.trajectory as tgen 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | from mpl_toolkits import mplot3d 9 | import sys 10 | import os 11 | sys.path.insert(0, os.getcwd()) 12 | 13 | 14 | def plot_formation(F, name): 15 | plt.figure() 16 | ax = plt.axes(projection='3d') 17 | ax.plot3D(F[0, :], F[1, :], F[2, :], 'ro') 18 | plt.title(name) 19 | plt.show() 20 | 21 | 22 | def scale_formation(form, scale): 23 | formNew = np.copy(form) 24 | for i in range(3): 25 | formNew[i, :] *= scale[i] 26 | return formNew 27 | 28 | 29 | # %% takeoff 30 | formTakeoff = np.array([ 31 | [-0.5, -1, 0], 32 | [-0.5, 0, 0], 33 | [-0.5, 1, 0], 34 | [0.5, -1, 0], 35 | ]).T 36 | plot_formation(formTakeoff, 'takeoff') 37 | # %% Square 38 | form_scale = np.array([1.25, 1.25, 1]) 39 | form = scale_formation(np.array([ 40 | [0.5, -0.5, 0], 41 | [-0.5, -0.5, 0], 42 | [-0.5, 0.5, 0], 43 | [0.5, 0.5, 0] 44 | ]).T, form_scale) 45 | plot_formation(form, 'formation') 46 | 47 | class ChaCha(): 48 | 49 | def __init__(self, d): 50 | self.waypoints = [form] 51 | self.T = [] 52 | self.d = d 53 | 54 | def wait(self, duration): 55 | self.waypoints.append(form) 56 | self.T.append(duration) 57 | 58 | def move(self, duration, vector): 59 | self.waypoints.append(form + vector) 60 | self.T.append(duration) 61 | 62 | def rotate(self, duration, angle): 63 | self.waypoints.append(formation.rotate_points_z(form, np.deg2rad(angle/2))) 64 | self.T.append(duration / 4) 65 | self.waypoints.append(formation.rotate_points_z(form, np.deg2rad(angle))) 66 | self.T.append(duration / 4) 67 | self.waypoints.append(formation.rotate_points_z(form, np.deg2rad(angle/2))) 68 | self.T.append(duration / 4) 69 | self.waypoints.append(form) 70 | self.T.append(duration / 4) 71 | 72 | def chacha(self, duration): 73 | self.move(duration/2, np.array([[0, 0, self.d]]).T) 74 | self.move(duration/2, np.array([[0, 0, 0]]).T) 75 | 76 | def left(self, duration): 77 | self.move(duration, np.array([[0, -self.d, 0]]).T) 78 | 79 | def right(self, duration): 80 | self.move(duration, np.array([[0, self.d, 0]]).T) 81 | 82 | def back(self, duration): 83 | self.move(duration, np.array([[-self.d, -self.d, 0]]).T) 84 | 85 | def hop(self, duration): 86 | self.move(duration/2, np.array([[0, 0, self.d]]).T) 87 | self.move(duration/2, np.array([[0, 0, 0]]).T) 88 | 89 | def right_stomp(self, duration): 90 | self.move(duration/2, np.array([[0, 0, 0]]).T) 91 | self.move(duration/2, np.array([[0, self.d/2, -self.d/2]]).T) 92 | 93 | def left_stomp(self, duration): 94 | self.move(duration/2, np.array([[0, 0, 0]]).T) 95 | self.move(duration/2, np.array([[0, -self.d/2, -self.d/2]]).T) 96 | 97 | def crisscross(self, duration): 98 | self.rotate(duration, 90) 99 | 100 | c = ChaCha(0.5) 101 | 102 | print(len(c.waypoints)) 103 | for i in range(4): 104 | c.left(3) 105 | c.back(2) 106 | c.hop(3) 107 | c.right_stomp(3) 108 | c.left_stomp(3) 109 | c.chacha(5) 110 | c.right(3) 111 | c.left(3) 112 | c.back(2) 113 | c.hop(3) 114 | c.hop(3) 115 | c.right_stomp(2) 116 | c.left_stomp(3) 117 | c.left(2) 118 | c.right(3) 119 | c.crisscross(5) 120 | c.chacha(5) 121 | c.wait(1) 122 | 123 | 124 | print('length of waypoitns', len(c.waypoints)) 125 | origin = np.array([1.5, 2, 2]) 126 | waypoints = np.array(c.waypoints) 127 | T = c.T 128 | 129 | trajectories = [] 130 | for drone in range(waypoints.shape[2]): 131 | pos_wp = waypoints[:, :, drone] + origin 132 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 133 | traj = tgen.min_deriv_4d(4, 134 | np.hstack([pos_wp, yaw_wp]), T, stop=False) 135 | trajectories.append(traj) 136 | 137 | tgen.plot_trajectories(trajectories) 138 | tgen.trajectories_to_json(trajectories, 'scripts/data/chacha.json') 139 | 140 | plt.show() 141 | 142 | # %% 143 | tgen.plot_trajectories_time_history(trajectories) 144 | 145 | # %% 146 | tgen.animate_trajectories('chacha.mp4', trajectories, fps=5) 147 | 148 | #%% 149 | -------------------------------------------------------------------------------- /scripts/data/block_land.json: -------------------------------------------------------------------------------- 1 | {"0": {"trajectory": [[5.0, 2.8096292779876486e-14, 1.1275719514520062e-14, 8.056843798490563e-15, 2.893554058959509e-15, 0.039225323422429834, -0.01703743159479408, 0.0025503713033142848, -0.00012957958425048144, 0.9999999999999963, 1.0173364063729453e-15, 8.007449809651809e-16, -1.0603366416828043e-16, -3.458171249359765e-14, 1.8931037293334896e-14, -3.4314998759166215e-15, 2.445011424226373e-16, 1.9999999999999927, 2.0346728127458906e-15, 1.6014899619303619e-15, -2.1206732833656086e-16, -6.91634249871953e-14, 3.786207458666979e-14, -6.862999751833243e-15, 4.890022848452746e-16, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [5.0, 1.0000000000000009, 0.01738288722372665, -0.0069202279983209825, 0.06652342255525194, 0.002768091199330816, -0.010770047186362725, 0.0023170850893743425, -0.00014729536792626373, 0.9999999999999993, 6.50868248186498e-14, -1.4704210071769808e-13, -1.2857770403940094e-14, 5.884528975208525e-14, -2.2076090955280847e-14, 3.228320388792838e-15, -1.704365815147213e-16, 1.9999999999999987, 1.301736496372996e-13, -2.9408420143539615e-13, -2.5715540807880188e-14, 1.176905795041705e-13, -4.4152181910561694e-14, 6.456640777585676e-15, -3.408731630294426e-16, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], "T": [5, 5], "color": [[255, 255, 255], [0, 0, 255]], "delay": [0, 0]}, "repeat": 1} -------------------------------------------------------------------------------- /scripts/data/geometry_3drone.json: -------------------------------------------------------------------------------- 1 | {"0": {"trajectory": [[2.0, 0.5000000000002975, 2.1771542481084117e-13, 3.3589257665560393e-13, 3.5706285139270603e-14, 7.573377469515406e-09, -5.46730433486718e-09, 7.410182706994532e-10, 4.962340765218423e-11, -1.0000000000005778, -4.0499086319323563e-13, -6.658872386049254e-13, -7.010447107024548e-14, 3.079450664066532e-08, -2.222792233972751e-08, 3.011229377226266e-09, 2.020940923261758e-10, 2.0000000000014375, 1.1147148279351138e-12, 1.300648658877353e-12, 1.618903813834134e-13, -0.023013542918120317, 0.015127516732794055, -0.001202768674397578, -0.0003038019813277164, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.49999999999990236, -3.052745946358198e-08, -4.443167194290077e-08, -1.1753138104412734e-08, 1.1255948780674173e-08, 1.1192201398235452e-08, -3.461539532245988e-09, 1.2578373655661696e-10, -0.9999999999998445, -1.2411940669598576e-07, -1.806640247601372e-07, -4.779148802537618e-08, 4.577542694202048e-08, 4.550653762855962e-08, -1.4077153634139407e-08, 5.120877805028684e-10, 1.9999999999997098, 0.10673309213288684, 0.16505689528290293, 0.0584202285200396, -0.028969050825724707, -0.052959307156319066, 0.028577824265872735, -0.00409663865832919, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000002641, 2.980102904350095e-07, 3.043255708032427e-07, 4.2575104748790224e-08, -4.929495304586019e-08, -9.443929878157821e-08, 2.860049156012956e-08, -1.356083065235449e-09, -1.0000000000005078, 1.2116649323798157e-06, 1.2373264512893567e-06, 1.730979835354362e-07, -2.004038368340396e-07, -3.8392095231183515e-07, 1.162333595726428e-07, -5.503297694388483e-09, 2.487463956091683, -0.044102643307533954, -0.3106868805073839, -0.013370230463163428, 0.00904850923130266, 0.0786897881089466, -0.04299924038083349, 0.006426647217854475, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999999683, -2.222599169690099e-06, -2.2256165624153274e-06, -3.126843882189611e-07, 3.426382864223993e-07, 6.408191734038024e-07, -1.5648340622180087e-07, -9.714734557437768e-10, -0.9999999999999292, -9.037125165070886e-06, -9.049663357723826e-06, -1.271479855893081e-06, 1.393464888432102e-06, 2.606256043484419e-06, -6.368946309314534e-07, -3.825252007022328e-09, 1.783058130440873, -0.23927378629768106, 0.12032824597808242, -0.07434665118995559, 0.015453188469959783, 0.013675108474935629, -0.005189482421001587, 0.0005102055840139436, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000000775, 1.687264712316516e-05, 1.7178282279081183e-05, 2.479818773508824e-06, -2.910186912828206e-06, -5.580443991213326e-06, 1.856949773274312e-06, -1.2503563670568074e-07, -1.000000000000091, 6.860034052669744e-05, 6.983981745859004e-05, 1.0081198864153596e-05, -1.1828723069767355e-05, -2.2680873645368634e-05, 7.5425154597457495e-06, -5.068984678700321e-07, 1.6090842587661565, 0.17058156917585426, 0.23651591152977308, 0.05168113525504042, -0.01630710851678677, -0.08372689947893162, 0.04470502682265943, -0.006560871727357648, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.49999999999967076, -0.0001236995077738017, -0.00012257881255873954, -1.6927429153474292e-05, 1.769238125264886e-05, 3.247886540333881e-05, -5.737429134289682e-06, -6.411924148940906e-07, -0.9999999999993103, -0.0005029825785114915, -0.0004984642948237828, -6.884419971940895e-05, 7.198189712136388e-05, 0.00013216129858330017, -2.3419609972540716e-05, -2.5893823185695875e-06, 2.390915741232708, 0.16093402359850265, -0.22261920300299481, 0.050864412268551866, -0.00831857760665536, 0.02341108550449328, -0.014613809626997877, 0.0023980149740817592, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000000446, 0.0009584809288239608, 0.0009909186997758988, 0.00014670982311093807, -0.00018129858897422886, -0.0003545753425483443, 0.00014009051382231645, -1.3850935482081064e-05, -1.0000000000000426, 0.0038967460466780056, 0.004028168612967645, 0.0005962712266483277, -0.0007366087645645047, -0.0014404184523909478, 0.0005685275423221944, -5.611694028394935e-05, 2.2169418695588172, -0.24401822423931202, -0.14003999948852225, -0.07456194323891752, 0.0204078925612457, 0.07410021879712057, -0.038503285448270266, 0.005521587131981676, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.49999999999956124, -0.006792749087909486, -0.006532121227355268, -0.0008287342490647362, 0.0008001168798840083, 0.0013041114899244423, 2.224312910477475e-05, -7.510266359877666e-05, -0.9999999999991002, -0.027623382560322606, -0.02657026428161753, -0.0033744167729025977, 0.0032581159713638984, 0.005318895338045337, 8.324263632099099e-05, -0.0003057724894803818, 1.5125360439073536, -0.03681124627073642, 0.30011331142464276, -0.015726928677805296, -0.002742649408936337, -0.059742430956515664, 0.03199908371239941, -0.004733223000675468, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.49999999999947486, 0.057691301859962525, 0.06689655863608324, 0.01923806942835794, -0.005852926282280811, -0.025760209085669103, 0.01401769350386569, -0.002078377083445549, -0.9999999999989019, 0.2343704080520182, 0.27138876522357436, 0.07753255222216018, -0.02417466952342655, -0.10456306786451205, 0.05677298844183448, -0.008400114617289869, 1.99999999999719, 0.13109976616514732, -0.14041208845584022, 0.04188315139589596, -0.005524376419173959, 0.00661515580370981, -0.003580728204796266, 0.0005488691595253184, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.7499999999999871, 0.0683082210166381, -0.051384941569387284, 0.020946089632412956, -0.004338990271720329, 0.001441995688947878, -0.002057315510176358, 0.000439865485450007, 2.4114517152721e-14, 0.2488436739880874, -0.22801321594561977, 0.0812264464654421, -0.015458134499709316, 0.008032497694359066, -0.006102336180264413, 0.0011358881671501083, 1.999999999999924, -0.017124783878048333, 0.016979556207107307, -0.0032554112423113847, -0.0005331460026076144, -0.0006269449039547957, 0.0004883180740154436, -8.035274142078374e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.7499999999999322, -0.10731134372335169, -0.1126506314004744, -0.03893181475965763, 0.009804371933174209, 0.0500357900318515, -0.029493731573659896, 0.004593953638057622, 1.3613719704548012e-13, -0.20332296837867897, -0.17029378453539726, -0.061415136996042806, 0.016775358430075545, 0.07455346124701102, -0.03943341334975364, 0.005745872994096635, 1.999999999999722, 0.0022712836974965248, -0.002304712097554025, 0.0005349812254284668, -2.2781990887727995e-06, 0.0001048391337998747, -6.674927994756687e-05, 1.0546750099043625e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.23176274578121148, -0.31319777432441825, -0.09943212391779971, -0.10544825249365655, 0.026845396488191314, 0.0811448711442037, -0.044299099797463645, 0.006548162025929187, -0.7132923872213651, -0.11745564274665854, 0.22530834366325095, -0.036731078941118725, 0.005149608262136289, -0.03299571424514976, 0.020111957421246506, -0.003232157507327374, 2.000000000000002, -0.0003044793603652269, 0.00030521805982414765, -6.33837521746951e-05, -5.753630447457331e-06, -1.2291731994192624e-05, 8.790957737476527e-06, -1.4224319178372177e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.6067627457812104, -0.1975134942577697, 0.1723384984063326, -0.0657754678937439, 0.013833487342612175, -0.005901623706831524, 0.005112949309592723, -0.0009987176299762273, -0.4408389392193553, 0.28162437098605614, 0.14371526487117148, 0.09254220064616106, -0.02309419096619393, -0.08657612121705108, 0.04775170718705626, -0.00713231135745504, 2.000000000000002, 3.894750549217369e-05, -4.255453528192998e-05, 8.909288630072836e-06, 5.055769067096154e-07, 2.239181996800536e-06, -1.2909511701103308e-06, 1.8692688744037945e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.6067627457812104, 0.20733735227215974, 0.19352907767827982, 0.06761749932206468, -0.01804672754346443, -0.08498038567104396, 0.04733218091609979, -0.007127696208852239, 0.4408389392193562, 0.27494032171733973, -0.11388531912087936, 0.09092261398873158, -0.020800152000762615, -0.02229235022344706, 0.010377133442501364, -0.0013066072813796925, 1.9999999999999984, 6.834469629329247e-06, 1.795618962818708e-05, 6.480536315996722e-07, -2.2201447487133125e-06, -4.346481086001552e-06, 1.5682663843962426e-06, -1.2841769292565957e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.23176274578120978, 0.311508354101098, -0.06240667360069609, 0.1056673217513758, -0.023674667766501165, -0.0447155710858555, 0.02453589083556292, -0.0036023077191897636, 0.713292387221366, -0.1314729098994748, -0.23846936796941062, -0.038531337727739985, 0.013054313528482133, 0.0777160610080577, -0.04175611946190904, 0.006159299086920166, 1.9999999999999973, -8.875122749633135e-05, -8.907020661511403e-05, -1.1963634093205117e-05, 1.2454073561682932e-05, 2.2713838021792643e-05, -3.414443139598418e-06, -6.10088114625749e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.75, 0.09211164754947261, -0.10597744311726279, 0.03609734712908318, -0.007323089864400433, 0.006112358345294098, -0.0027930357171508016, 0.00040028091418820456, -1.2558852079154437e-16, -0.17051246670022507, 0.17851132654460566, -0.05722601440782334, 0.009451500232220827, -0.007757935558392406, 0.00414142813086971, -0.0006502634667035986, 1.9999999999999996, 0.0006981491747274284, 0.0007257072298481993, 0.00010826222929952104, -0.00013609880658854142, -0.00026765988365279087, 0.00011075883174261521, -1.1805415253387353e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.7500000000000001, -0.0009171755444989654, 0.02250135275346033, -0.0007214407730605037, -0.001702993467791604, -0.0019828895825148123, -0.0001679943537242119, 0.00014884739861800096, -1.4411686543038174e-17, 0.04246999738787247, -0.001677897216391303, 0.0065495246994704825, -0.0017159381764941513, -0.003383860915763209, -0.0005285135422602871, 0.00030420955724220214, 2.000000000000001, -0.004890957996296808, -0.004655000876745836, -0.0005765430325458346, 0.0005273159905445493, 0.0008194517151905031, 0.00011905327044856986, -7.238012721525333e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.7500000000000001, -0.09826729271198266, -0.12162349473544927, -0.037185525185655544, 0.010065721096712102, 0.05037150001583965, -0.029751845368089423, 0.004636359977622898, 4.212264794777587e-17, -0.17645490846226952, -0.19668656619202393, -0.056737232048887004, 0.01791331615813297, 0.07544191189888597, -0.04018992774606528, 0.005872364744655884, 1.9999999999999982, 0.04243270195252247, 0.05003080133532564, 0.014935705530699298, -0.004401406250867672, -0.020191090659902444, 0.011471361261503157, -0.0017455831429850056, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.23176274578121073, -0.31442953821795583, -0.09824832164016037, -0.1057334272039113, 0.02685079290412718, 0.08110127366921827, -0.044268108300538005, 0.006542967003272168, -0.7132923872213647, -0.12111561048424321, 0.22880903057420457, -0.037618409191357324, 0.005198898886815644, -0.03312725103042468, 0.020202179063027376, -0.0032472967894115272, 2.198669330795058, 0.08613223499288018, -0.0011811376194705825, 0.02997207089626215, -0.006793917195662051, -0.016947085700342573, 0.009759757264746368, -0.0014858009575514012, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.6067627457812118, -0.19712426099339936, 0.1723998724425717, -0.06571194345594024, 0.013807792480194328, -0.005945649573535832, 0.005110081989134815, -0.0009950356310198462, -0.44083893921935463, 0.28291370190531984, 0.14403159891779932, 0.09274518870025177, -0.02318596867097039, -0.08674003780071658, 0.04774687519453795, -0.007120161990133896, 2.389418342308649, 0.07613489482976694, 0.0037139190992656543, 0.02724993144769389, -0.0067036064287303515, -0.01590241765181268, 0.009094127249331188, -0.0013733583030562985, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.6067627457812111, 0.2054890800059161, 0.19161899869572444, 0.06731757833464186, -0.017653760592637357, -0.08419689985708796, 0.046963043333317855, -0.007080805455724958, 0.4408389392193553, 0.26839580622251846, -0.12066235172214, 0.08986524395489857, -0.01941919224334809, -0.01954259148283106, 0.009096251245582382, -0.001146099887240108, 2.5646424733950344, 0.0620888428369856, -0.006172698463549151, 0.023504084126871185, -0.0046204728427502, -0.011025652537430689, 0.006645575523859266, -0.0010419869857463107, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.23176274578121062, 0.323807163105392, -0.051088640337577415, 0.10704737743495366, -0.024465686767412814, -0.04575300417500136, 0.023022389069202603, -0.0031194320989971903, 0.7132923872213652, -0.08766465395272657, -0.19801764574525046, -0.03360769086646594, 0.01002199923603244, 0.0736472398592313, -0.046651675626550856, 0.007746973495501284, 2.7173560908995245, 0.09868005550960464, 0.03663112638205372, 0.02529357168730825, -0.007898822794419093, -0.01494757937483638, 0.001813686739628213, 0.0005196051346223782, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.7500000000000001, -0.016328058646621485, -0.22710618606332258, 0.017901991929629272, 0.025906627915532512, 0.06941485779260513, -0.05110738055304048, 0.009241842074157892, 3.8646925436826035e-16, -0.557021957352964, -0.25779258193612853, -0.1335050453764107, 0.1165464389756456, 0.21363672203973072, -0.15866722972453665, 0.028647181085549766, 2.8414709848078954, -0.318830303803186, -0.41252607256978535, -0.05463143193221412, 0.09693602552926639, 0.19888877635189678, -0.14761588885495402, 0.026682382840026858, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], "T": [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2], "color": [[255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [0, 0, 255], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [255, 100, 15], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [0, 0, 255]], "delay": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, "1": {"trajectory": [[2.0, 0.5000000000002818, 2.0291451062689792e-13, 3.2046564342043866e-13, 3.4976805922078125e-14, -2.75424879126694e-08, 1.98796106209478e-08, -2.6923778891283142e-09, -1.8092304705041934e-10, -7.416737925421538e-15, -1.351372108352113e-14, -2.0977562891401545e-15, -5.768526672001757e-16, -1.9865794910506904e-08, 1.4340106813737547e-08, -1.943011229424064e-09, -1.302962168519406e-10, 2.000000000000747, 8.790085694336316e-13, 1.0589357550907909e-12, 4.6159957102652416e-14, 0.21206201980678333, -0.14577453784649802, 0.029258114380575403, -0.001310263471046522, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999999223, 1.1102114162669802e-07, 1.6159850913954975e-07, 4.27471871483693e-08, -4.094752062076088e-08, -4.0717825982517225e-08, 1.2604246540775793e-08, -4.6057939597123925e-10, 1.7646255875337562e-14, 8.007224739299852e-08, 1.1654774787913343e-07, 3.083026636940012e-08, -2.952832542265739e-08, -2.935652665282484e-08, 9.08028480387078e-09, -3.3010778088616975e-10, 2.43301270189136, 0.1545815321456157, -0.4310241535614698, -0.1869345982992985, 0.1429297322833904, 0.0686279829525895, -0.054005975081331474, 0.008677519529403216, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000002502, -1.083689444666401e-06, -1.1065751098174208e-06, -1.5479100179268033e-07, 1.7916679009060808e-07, 3.432089361067655e-07, -1.0380508777378672e-07, 4.892016277361564e-09, -8.883134348449085e-15, -7.816757087666894e-07, -7.982366905562454e-07, -1.1167220647453974e-07, 1.292933092581673e-07, 2.4769585223086626e-07, -7.500269781437312e-08, 3.553814537725879e-09, 1.659913631116811, -0.23035865885877155, 0.23777957189056856, -0.07992249847571258, 0.018556525162317896, -0.015336390899066679, 0.009517755516285649, -0.0016239913207539423, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.49999999999997136, 8.083616891624431e-06, 8.095610435040863e-06, 1.1376157601401762e-06, -1.2472845777339796e-06, -2.333223279307021e-06, 5.715213765966247e-07, 3.060377309833828e-09, -3.4596612388109914e-15, 5.829956038197427e-06, 5.837953555770779e-06, 8.202128399470163e-07, -8.988419709785449e-07, -1.6810984022703577e-06, 4.1065505999504144e-07, 2.5099026218164314e-09, 1.718339970968359, 0.22844812769365525, 0.16962906936812447, 0.06847980984347855, -0.018459622662310035, -0.0785266302073654, 0.04117202474424765, -0.005960605958230521, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000000432, -6.13503675467326e-05, -6.244968329878525e-05, -9.012360512443895e-06, 1.0568670832441497e-05, 2.026085529532534e-05, -6.7238929388005085e-06, 4.490734130328899e-07, -2.815958016460885e-14, -4.42562830446258e-05, -4.505697996708256e-05, -6.504103985016839e-06, 7.63225032276823e-06, 1.4634845681683165e-05, -4.8684282855459016e-06, 3.2751297761818243e-07, 2.4654368743228585, 0.09056106311414494, -0.26889469736662913, 0.029322242721464338, -0.0023741083855306176, 0.04613658157244781, -0.026492663099106115, 0.004110222987668332, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999996257, 0.00044996590826862896, 0.000446035705640267, 6.162945903957847e-05, -6.451579686764396e-05, -0.00011851142941059201, 2.1212926852815976e-05, 2.264694228627628e-06, -1.2608729311487917e-14, 0.00032447379730405924, 0.0003215459908612992, 4.440646766066079e-05, -4.642135625447211e-05, -8.522442767909109e-05, 1.5077436031122722e-05, 1.676438909973959e-06, 2.074521133086712, -0.26340717278883036, -0.055182612566778304, -0.0813085842280231, 0.020294357939600553, 0.058057901574314875, -0.0295273175555021, 0.004156845188530839, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000000139, -0.0034842926430865705, -0.00360050405139163, -0.0005326570277976768, 0.0006572599042364324, 0.0012847132163259038, -0.0005053592524747989, 4.959272251325667e-05, -2.046191038715769e-14, -0.002513989892141214, -0.002598930372618261, -0.0003847459349526595, 0.0004753834236269714, 0.0009296669088366262, -0.0003671319090511761, 3.627058650664667e-05, 1.5013981014097704, 0.027232620025450363, 0.29550635193298264, 0.0068248389581845315, -0.006849026858818671, -0.07249536937087443, 0.03984532449349219, -0.005982612434604539, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999995298, 0.024719744270237355, 0.023794937724644395, 0.0030283950705837785, -0.0029312007772766004, -0.004802390987446339, -4.3497559106111e-05, 0.000269634123545292, -9.187177563807636e-15, 0.017818855878113157, 0.017137324851850058, 0.0021754208030800434, -0.002100097809209062, -0.0034258623803859667, -5.6554110848088576e-05, 0.00019733005176312974, 2.1473775872033714, 0.24244961815074287, -0.08498848621541874, 0.07720680483261785, -0.016214732647375624, -0.024142510535177696, 0.011877512586407712, -0.001602573136403657, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999994372, -0.2093049118527633, -0.241820072665181, -0.06848135091703378, 0.02193259039460371, 0.09301004872394317, -0.05025036520202334, 0.007402510071498919, -2.0625629588537366e-14, -0.1512534414959844, -0.17524868000019062, -0.050203685634301165, 0.015500446229754468, 0.06751534697731205, -0.03669567188793079, 0.0054346624461435325, 2.4330127018902497, -0.05876530476798411, -0.16862821049629442, -0.015250420314110925, 0.006290438992368105, 0.04845381347367456, -0.02592651072069282, 0.0038215761189762504, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.3750000000000142, -0.18746022973217275, 0.2288770087803728, -0.06727147108685343, 0.009713985532344462, -0.013455067982167114, 0.005753688435133685, -0.0007461989054598948, -0.6495190528383282, -0.16829057818166812, 0.14189958459807492, -0.053482768932379396, 0.010619087647190988, -0.004448848849954418, 0.0044179301475941665, -0.0008862874935348071, 1.9999999999998235, -0.10450802194471805, 0.11288146031638899, -0.03493345811221505, 0.005279243801042578, -0.0054388213799626325, 0.002840064255631737, -0.0004314989179356317, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.37500000000007155, -0.07435667929158538, -0.13858204357503065, -0.025043543550954304, 0.011448918289920626, 0.04120976713986083, -0.020765190185585175, 0.0029051258711238633, -0.649519052838329, 0.19260322097029492, 0.18467120630720008, 0.06406380548252145, -0.01695409018645852, -0.0806779084358099, 0.045315473257600034, -0.006860786450273862, 1.999999999999845, 0.013627235056925821, -0.013474999030885688, 0.002518523954590413, 0.00047518831731112505, 0.0004837833807088046, -0.00038720583905302, 6.401895947748068e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.7336107005503544, 0.04851925489421855, 0.2513110461209675, 0.019378546498437656, -0.008929577531939396, -0.0694493685621629, 0.03975474753224503, -0.006102715005653338, 0.15593376811331905, 0.3302286868943777, -0.026811712897536432, 0.10975005562106763, -0.02582498516525267, -0.05376315223786386, 0.028300385250738268, -0.004053572933951566, 2.000000000000004, -0.0018048042537691029, 0.0018346741645036194, -0.0004309512203465706, 5.980434689125652e-06, -8.466205664387692e-05, 5.319389127983e-05, -8.378035718856279e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.07839634745073731, 0.3435052350225651, 0.03743790331170465, 0.11320638574971617, -0.02689861280181945, -0.07199271502429258, 0.03877318861383754, -0.005673414955360451, 0.7458914215262051, 0.030204092171207328, -0.22107175360469217, 0.010684881889641737, -0.00043381400607937944, 0.04839762415010876, -0.02830278084256835, 0.004430905163734613, 2.0000000000000036, 0.00024075426223409165, -0.00024397335253403807, 4.9732593693438626e-05, 5.143343788574961e-06, 1.0073953075097201e-05, -7.073049142666797e-06, 1.1292131419711792e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.6851590932319502, 0.13432297951971026, -0.19527727405825837, 0.04490668557562183, -0.008990633715419683, 0.023179406880401248, -0.014675909866754162, 0.002431764569312474, 0.30505248230684906, -0.3170248641777304, -0.11066319859080671, -0.10401870727421123, 0.026029022786592408, 0.08474155702412389, -0.04617957540698095, 0.0068260915327096, 1.999999999999999, -1.99827239533746e-05, 4.507814392107286e-05, -5.491036102711079e-06, -2.3203941645583343e-06, -5.545672238547072e-06, 2.3525823563984787e-06, -2.530960272004788e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5018479547691412, -0.2695977856320295, -0.17533239678559379, -0.08619952879740399, 0.02314292249082405, 0.0896624766339568, -0.04843023952840125, 0.00713533399761243, -0.5573586191080456, -0.20403832621709134, 0.17328107887744473, -0.07224505081611933, 0.013975697767649318, -0.00013323520069949413, -0.00037062705143612457, 4.003752464845373e-05, 1.999999999999997, -8.516018147810322e-05, -9.267207642952094e-05, -1.1201676954598733e-05, 1.2510937384853926e-05, 2.286111193641175e-05, -3.5181252842182834e-06, -5.933474821968469e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.3750000000000012, -0.19372642644301338, 0.20758566050552976, -0.06760836939371218, 0.011846836457780436, -0.009774705245307172, 0.004983125569459417, -0.0007632944211441533, -0.6495190528383288, 0.005485309174775237, 0.0025234285673412793, -0.002648191129633109, 0.0016162295760143233, -0.0014144916156595827, 0.00034812475225706047, -2.152132219851964e-05, 1.9999999999999991, 0.000697670614169816, 0.0007261897379128136, 0.00010815566463472896, -0.00013610275528926893, -0.00026768062202847426, 0.0001107727612978467, -1.1807642426284848e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.3750000000000008, 0.037242338542587364, -0.012700614833183237, 0.006033327729047666, -0.0006351197480727115, -0.001940149999491627, -0.00037336531672956083, 0.00018900981749045184, -0.649519052838329, -0.020440852843401153, -0.018647925614670546, -0.0026499992180092752, 0.002332828339223687, 0.003409208195925467, 0.0004097299037827337, -0.0002810095910131911, 2.000000000000001, -0.004890894027958348, -0.004655065162439937, -0.0005765292098227869, 0.0005273168229735777, 0.0008194543939357842, 0.00011905141793570453, -7.237982919608843e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.37500000000000083, -0.10370576251560981, -0.10954860103352114, -0.03054658311730263, 0.010484210150555338, 0.04015568075464995, -0.019930984765405894, 0.002767357593607288, -0.6495190528383291, 0.17333046136323316, 0.20367334663945993, 0.0605723689704773, -0.01767398095833545, -0.08134423723155416, 0.04586087617883885, -0.0069513845999926525, 1.9999999999999982, 0.04243269341817141, 0.050030809929578285, 0.014935703651284275, -0.004401406336419273, -0.0201910910251203, 0.011471361509453123, -0.0017455831827198252, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.733610700550354, 0.052517148110384555, 0.24747536345442864, 0.020317018228148832, -0.008957942017320092, -0.06930739177146815, 0.03965480736033048, -0.006085950037977702, 0.15593376811331944, 0.33285383191607787, -0.029327129335824854, 0.11037584465992822, -0.025851470541504922, -0.053669330889249536, 0.02823517419790148, -0.004042635014851784, 2.198669330795058, 0.08613223613281201, -0.0011811387658868778, 0.029972071144305557, -0.0067939171820926, -0.016947085652223453, 0.009759757231698914, -0.0014858009522426166, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.07839634745074048, 0.34219473056375466, 0.03719086364862544, 0.11299496761631496, -0.026809428745867735, -0.07183756343777158, 0.03878071015135168, -0.005685701372763684, 0.7458914215262061, 0.02931488136729892, -0.22126274799635834, 0.010543108105172058, -0.0003721317183971668, 0.04850630677227467, -0.028298307488663955, 0.004422510900360615, 2.389418342308649, 0.07613489467764818, 0.003713919252403823, 0.027249931414346312, -0.0067036064303865476, -0.01590241765826735, 0.009094127253735498, -0.0013733583037622754, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.6851590932319509, 0.14069313572586953, -0.18869158174889994, 0.04593785560709106, -0.010338838416331413, 0.020493490886121218, -0.0134182401061297, 0.002273149572828458, 0.30505248230685017, -0.3126126410247247, -0.10609719106189544, -0.1033047687353021, 0.025095538785488872, 0.08288196873041605, -0.04530991544365719, 0.006716594243886573, 2.564642473395035, 0.06208884285722953, -0.006172698484012782, 0.02350408413137399, -0.0046204728423829035, -0.011025652536761557, 0.006645575523348674, -0.001041986985662291, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5018479547691436, -0.31211357416727276, -0.2145384117390011, -0.09098787247551747, 0.0259835444690696, 0.09345960609372267, -0.04343510548621156, 0.00552217550733464, -0.5573586191080461, -0.2335134344480117, 0.14609625122533818, -0.07555540364343567, 0.015966687758449415, 0.002517658283166385, 0.003036450481470006, -0.0010581268796774988, 2.717356090899525, 0.09868005550679866, 0.03663112638486725, 0.025293571686732037, -0.007898822794528242, -0.01494757937488412, 0.001813686739682031, 0.0005196051346131147, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.375, 0.18051459361187902, 0.6263845324015355, -0.0019338666604470575, -0.09931758171267406, -0.2261171744533812, 0.1676837663904024, -0.030336144864227223, -0.649519052838329, 0.265511048002512, 0.2950638506303178, 0.046165454166593574, -0.07294522683139777, -0.15132988336788944, 0.11200644798988799, -0.020232928541396328, 2.8414709848078954, -0.3188303038028445, -0.41252607257019946, -0.05463143193215339, 0.0969360255293083, 0.19888877635193952, -0.14761588885499316, 0.026682382840034116, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], "T": [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2], "color": [[255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [0, 0, 255], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [255, 100, 15], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [0, 0, 255]], "delay": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, "2": {"trajectory": [[2.0, 0.5000000000002847, 1.9430897329486794e-13, 3.3225915618091604e-13, 3.470714701119828e-14, -2.683518863796459e-08, 1.9370489250885264e-08, -2.624375898154319e-09, -1.7605626000977567e-10, 1.000000000000585, 4.185045842767569e-13, 6.679849948940655e-13, 7.068132373744566e-14, -1.0928711714921182e-08, 7.887815515509797e-09, -1.0682181470177524e-09, -7.179787525957999e-11, 2.000000000001299, 4.961211747040057e-13, 1.6539531875162678e-12, 2.1804405709886294e-13, -0.18904847689233864, 0.13064702111137755, -0.02805534570356996, 0.0016140654518579992, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999999329, 1.0816174128519413e-07, 1.5743494886522134e-07, 4.164644965416083e-08, -3.988861111554152e-08, -3.9654794292065815e-08, 1.2265975088260045e-08, -4.4597971252987617e-10, 0.9999999999998269, 4.404715928007808e-08, 6.411627686309272e-08, 1.696122165774431e-08, -1.6247101525270494e-08, -1.6150010946769944e-08, 4.996868839234882e-09, -1.8198000084983567e-10, 1.566987298107973, -0.26131462428768903, 0.26596725828841344, 0.1285143697860798, -0.11396068147246538, -0.015668675785796657, 0.025428150812011947, -0.00458088087064521, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000002487, -1.0558917521956297e-06, -1.0782609337581133e-06, -1.508468306583448e-07, 1.7464762766699042e-07, 3.3458250215071616e-07, -1.0130799174383476e-07, 4.799304276090975e-09, 1.0000000000005167, -4.299892236217264e-07, -4.39089760764767e-07, -6.14257770608035e-08, 7.111052757892942e-08, 1.3622510014939124e-07, -4.123066178404744e-08, 1.9494831583354043e-09, 1.8526224127945718, 0.2744613021741102, 0.07290730861117624, 0.09329272893514404, -0.02760503439035586, -0.06335339721034251, 0.03348148486443007, -0.004802655897073186, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999999621, 7.87518089436116e-06, 7.886015608435211e-06, 1.1079659235895891e-06, -1.2142016752886769e-06, -2.2709286718623525e-06, 5.547920220768847e-07, 3.375805406670318e-09, 0.9999999999999325, 3.207169126873762e-06, 3.211709802036434e-06, 4.512670159461245e-07, -4.946229174504067e-07, -9.251576411947254e-07, 2.2623957094673203e-07, 1.3153493792742439e-09, 2.4986018985903677, 0.01082565860439012, -0.28995731534439234, 0.005866841346285148, 0.0030064341915000403, 0.06485152173182089, -0.03598254232261271, 0.005450400374089832, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000000286, -5.9781483664275835e-05, -6.0862696259503754e-05, -8.785619786251618e-06, 1.0309258422289612e-05, 1.9767852290391517e-05, -6.575415370296507e-06, 4.4223348066673185e-07, 1.0000000000001192, -2.434405748207118e-05, -2.4782837491439294e-05, -3.577094879136327e-06, 4.196472747000894e-06, 8.0460279637472e-06, -2.6740871742086198e-06, 1.7938549025516605e-07, 1.9254788669115783, -0.2611426322923241, 0.032378785836676804, -0.08100337797633314, 0.018681216898182158, 0.03759031791048727, -0.018212363724992204, 0.0024506487398812072, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.49999999999964884, 0.0004383055948816345, 0.0004343551805832552, 5.9986829019454686e-05, -6.271176633635215e-05, -0.00011513417338282628, 2.0377456119484076e-05, 2.2624849529659093e-06, 0.9999999999993229, 0.00017850878120743326, 0.0001769183039625778, 2.44377320587467e-05, -2.5560540866891746e-05, -4.6936870904209066e-05, 8.342173941424495e-06, 9.129434085944482e-07, 1.5345631256765004, 0.10247314918781071, 0.27780181557660816, 0.030444171963175483, -0.011975780329703494, -0.0814689870865369, 0.04414112718550779, -0.006554860162973754, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5000000000000092, -0.003395877294079225, -0.0035105607509329354, -0.0005196896842326585, 0.0006420896538237278, 0.0012556549776722113, -0.0004958006057340826, 4.897156316775187e-05, 1.0000000000000628, -0.0013827561545367909, -0.0014292382403493758, -0.00021152529169566806, 0.0002612253409375316, 0.0005107515435543212, -0.00020139563327102011, 1.9846353777304316e-05, 2.2816600290317095, 0.21678560418435594, -0.15546635247666424, 0.06773710427987971, -0.013558865697312447, -0.0016048494197516876, -0.0013420390466795545, 0.0004610253025571662, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999995455, 0.02407041462572985, 0.0231505961218618, 0.0029392051097121897, -0.0028373592265301816, -0.004629656212642895, -7.571146426097494e-05, 0.0002666830119151596, 0.9999999999991093, 0.009804526682209443, 0.009432939429767487, 0.0011989959698225534, -0.0011580181621548354, -0.0018930329576593763, -2.668852547290994e-05, 0.0001084424377172525, 2.34008636888404, -0.20563837167499924, -0.2151248249975369, -0.061479876125595294, 0.018957382020335707, 0.0838849414300994, -0.04387659628115088, 0.006335796136501011, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.4999999999994392, -0.2042873436307293, -0.23664305908362668, -0.06771726481746743, 0.02099463412764338, 0.09117980216967723, -0.0495410746239074, 0.007334734395261611, 0.9999999999989226, -0.08311696655603364, -0.09614008522338365, -0.027328866587858994, 0.008674223293672075, 0.03704772088719996, -0.020077316553903657, 0.002965452171146336, 1.566987298105964, -0.07233446294196595, 0.3090402973975668, -0.026632731302326716, -0.0007660623273547469, -0.05506896880980646, 0.029507238796630104, -0.004370445275227834, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.37500000000001144, -0.22317961082915344, 0.1943923485273967, -0.07168878348793443, 0.014053809063241511, -0.0062636365543675705, 0.00559476396974701, -0.0010952294834651532, 0.6495190528383041, -0.08055309580641924, 0.08611363134754461, -0.027743677533062693, 0.004839046852518325, -0.0035836488444046547, 0.0016844060326702457, -0.00024960067361529933, 2.000000000000125, 0.12163281741149334, -0.12986100481842858, 0.03818887101986296, -0.0047460996912633635, 0.006065762694121702, -0.0033283812817016577, 0.0005118516185101649, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.3750000000000679, 0.22628722069861443, 0.20720928061863014, 0.07202995126227908, -0.01956097366587683, -0.08970244642736094, 0.048994970477532096, -0.007289277073696828, 0.6495190528381931, 0.010719747408384118, -0.014377421771802854, -0.002648668486478644, 0.00017873175638297595, 0.00612444718879888, -0.0058820599078463955, 0.0011149134561772283, 1.9999999999995814, -0.015898605208377024, 0.015779624269731767, -0.003053517452024476, -0.0004728964693218647, -0.0005885968418569784, 0.00045394833384746234, -7.456560908640688e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5018479547691432, 0.25877508949339845, -0.14587137289428231, 0.08464441997554283, -0.017884789922738845, -0.011975640106836946, 0.004718605330587939, -0.00047283224786097113, 0.5573586191080462, -0.2127730441477191, -0.19849663076571464, -0.0730189766799489, 0.020675376903116383, 0.08675886648301366, -0.048412342671984784, 0.007285730441278943, 2.0000000000000018, 0.002109935306996252, -0.0021392322376328436, 0.0004944294456300954, -3.354014694123831e-07, 9.674705670985963e-05, -6.192135541783127e-05, 9.797266616896124e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.6851590932319508, -0.14519847202074498, -0.2105690109553424, -0.04726870958801177, 0.013082099443047675, 0.07792552028677928, -0.0439089051052227, 0.006675835237131415, -0.3050524823068499, -0.3118284631572636, 0.07735648873352069, -0.10322708253580284, 0.023528004972273326, 0.03817849706694235, -0.01944892634448791, 0.002701406193720428, 2.000000000000001, -0.00028453340190695537, 0.00028169720397638886, -5.931905891232137e-05, -4.911506626887707e-06, -1.0937121058228612e-05, 8.039679924984357e-06, -1.3213834071316666e-06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.07839634745074144, -0.34176581974629167, 0.001854795190917864, -0.11254818661450346, 0.027036862394281637, 0.06179629660723389, -0.032653189960749444, 0.0046954411429165, -0.7458914215262057, 0.04208454246039066, 0.22454851771168613, 0.013096093285479636, -0.005228870785829794, -0.06244920680067686, 0.0358024419644796, -0.005519484251329909, 1.9999999999999991, 4.992758958754721e-05, -2.5509460655493466e-05, 1.0278125890945804e-05, -1.8892756012026308e-06, -2.471486531549937e-06, 3.131863840509991e-07, 7.21070809398751e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.7336107005503543, -0.04189639359821721, 0.2377249590053882, -0.019464776857538318, 0.0005319508382944016, -0.04494634122282365, 0.023893945951816063, -0.0035329606922893464, -0.15593376811332055, 0.3355112361165663, 0.06518828909196597, 0.11077638854385938, -0.027030011296131463, -0.0775828258073582, 0.04212674651334517, -0.00619933661156862, 1.9999999999999984, -9.451272583938563e-05, -8.328155145568075e-05, -1.3205753125758285e-05, 1.23770249423788e-05, 2.247320540978962e-05, -3.247655851451188e-06, -6.369321506614493e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.3749999999999997, 0.10161248173540685, -0.10160673662935682, 0.03151054554440537, -0.004523698122048112, 0.0036623870000915275, -0.002190065958869237, 0.0003630048906943079, 0.649519052838329, 0.16502715752545, -0.18103475511194694, 0.05987420553745648, -0.01106772980823516, 0.00917242717405198, -0.0044895528831267704, 0.000671784788902118, 2.0000000000000004, 0.0006989177349575528, 0.0007249331517448055, 0.00010843172265719298, -0.000136091274200585, -0.0002676269431686727, 0.00011073649805410553, -1.1801837226495222e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.3749999999999999, -0.03632177121930848, -0.009797801866914084, -0.005311374058670348, 0.00233758374108035, 0.003922032226408297, 0.0005416786570077981, -0.0003378754904309964, 0.6495190528383291, -0.02202914454447131, 0.020325822831061838, -0.0038995254814612064, -0.0006168901627295351, -2.534728016225635e-05, 0.0001187836384775507, -2.3199966229011022e-05, 2.0000000000000013, -0.004891060665152308, -0.0046548976297515485, -0.00057656535575858, 0.0005273147540193406, 0.0008194473853547909, 0.00011905624679232465, -7.238060542963462e-05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.37499999999999994, 0.2019498728684913, 0.23114908979168303, 0.0677288954060196, -0.02054651189512977, -0.09052085177214803, 0.04968152225328467, -0.0074037913325162305, 0.6495190528383292, 0.0031244470990363414, -0.006986780447435962, -0.0038351369215902814, -0.00023933519979751797, 0.0059023253326681655, -0.005670948432773566, 0.0010790198553367691, 1.999999999999999, 0.04243271565542034, 0.05003078754202692, 0.01493570853676432, -0.004401406105186851, -0.02019109007603882, 0.011471360863640303, -0.0017455830791768905, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.5018479547691432, 0.2620902101546821, -0.14904439968995253, 0.08544314367541433, -0.017925267524129464, -0.01185673423918773, 0.004636647970785209, -0.00045908223890808806, 0.5573586191080454, -0.21173822143183468, -0.19948190123837983, -0.07275743546857094, 0.02065257165468928, 0.08679658191967426, -0.04843735326092887, 0.007289931804263314, 2.1986693307950587, 0.0861322331630494, -0.0011811357786111177, 0.02997207049700896, -0.0067939172167489955, -0.016947085777775855, 0.00975975731781131, -0.0014858009660720542, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, 0.6851590932319511, -0.14634939704738315, -0.21083844890942965, -0.04745074454809094, 0.0131632414368225, 0.07806973824356614, -0.043904024349437965, 0.006664977945431916, -0.3050524823068515, -0.312228583272619, 0.07723114907855895, -0.10328829680542387, 0.023558100389367568, 0.03823373102844197, -0.019448567705874005, 0.002697651089773283, 2.3894183423086495, 0.07613489507409203, 0.0037139188534007703, 0.0272499315011581, -0.006703606426067745, -0.01590241764140384, 0.009094127242231895, -0.0013733583019177543, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.07839634745074026, -0.3359718973371977, 0.0078532732361788, -0.11161152977905418, 0.02581298762714471, 0.0593588810153276, -0.03151603230174613, 0.004552677028511254, -0.7458914215262058, 0.044216834802206235, 0.22675954278403557, 0.01343952478040353, -0.005676346542140776, -0.06333937724758501, 0.03621366419807482, -0.005570494356646467, 2.5646424733950344, 0.06208884280442685, -0.006172698430714749, 0.023504084119646794, -0.004620472843169219, -0.011025652538682632, 0.0066455755247442246, -0.0010419869858893282, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.7336107005503543, -0.08064996960846894, 0.201957489580055, -0.023818420461851647, 0.0031894276588096127, -0.041392292112460476, 0.028281675577775597, -0.0049521616154529335, -0.15593376811331935, 0.3211780884007384, 0.051921394519912324, 0.10916309450990166, -0.025988686994481862, -0.0761648981423977, 0.043615225145080865, -0.006688846615823786, 2.717356090899525, 0.09868005551401395, 0.03663112637760928, 0.02529357168821762, -0.007898822794288385, -0.0149475793747006, 0.0018136867395170796, 0.0005196051346407732, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [2.0, -0.3750000000000002, 0.44355056646458824, 0.2839593947053022, 0.09786290410066162, -0.10043821112608495, -0.19269618890405182, 0.14289347814076653, -0.025802618145451015, 0.6495190528383289, 0.29151090935045204, -0.03727126869418951, 0.08733959120981713, -0.04360121214424781, -0.06230683867184125, 0.046660781734648604, -0.008414252544153431, 2.8414709848078954, -0.31883030380368405, -0.41252607256911944, -0.05463143193233544, 0.0969360255292242, 0.1988887763518014, -0.14761588885487864, 0.026682382840013022, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], "T": [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2], "color": [[255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [0, 0, 255], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0], [255, 100, 15], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0], [0, 0, 255]], "delay": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]}, "repeat": 3} -------------------------------------------------------------------------------- /scripts/error.py: -------------------------------------------------------------------------------- 1 | #%% [markdown] 2 | # # Kalman Filter for UWB TDOA measurement 3 | # 4 | # The time difference of arrival (TDOA) measurement is given as: 5 | # 6 | # $d = |\vec{a} - \vec{n}| - |\vec{b} - \vec{n}|$ 7 | #%% 8 | 9 | import sympy 10 | sympy.init_printing() 11 | 12 | a = sympy.Matrix(sympy.symbols('a_0:3', real=True)) 13 | b = sympy.Matrix(sympy.symbols('b_0:3', real=True)) 14 | n = sympy.Matrix(sympy.symbols('n_0:3', real=True)) 15 | 16 | d = sympy.Matrix.norm(a - n) - sympy.Matrix.norm(b - n) 17 | 18 | da = sympy.symbols('da') 19 | db = sympy.symbols('db') 20 | 21 | 22 | H = sympy.Matrix([d]).jacobian(n) 23 | H.simplify() 24 | H.subs({ 25 | sympy.Matrix.norm(a - n): da, 26 | sympy.Matrix.norm(b - n): db}) 27 | 28 | 29 | #%% 30 | 31 | 32 | #%% 33 | -------------------------------------------------------------------------------- /scripts/flight_qualysis.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import enum 3 | import time 4 | import sys 5 | import json 6 | import argparse 7 | import numpy as np 8 | from typing import Dict, List 9 | 10 | import cflib.crtp 11 | from cflib.crazyflie.log import LogConfig 12 | from cflib.crazyflie.swarm import CachedCfFactory 13 | from cflib.crazyflie.swarm import Swarm 14 | from cflib.crazyflie.syncLogger import SyncLogger 15 | from cflib.crazyflie.mem import MemoryElement 16 | from cflib.crazyflie.mem import Poly4D 17 | from cflib.crazyflie.syncCrazyflie import SyncCrazyflie 18 | from cflib.crazyflie import Crazyflie 19 | 20 | import asyncio 21 | import math 22 | import xml.etree.cElementTree as ET 23 | from threading import Thread 24 | from enum import Enum 25 | import logging 26 | 27 | import qtm 28 | 29 | # set logging levels for crazyflie 30 | logger = logging.getLogger('flight_qualisys') 31 | logging.getLogger('qtm').setLevel(logging.WARN) 32 | logging.getLogger('cflib.crazyflie.log').setLevel(logging.ERROR) 33 | logging.getLogger('cflib.crazyflie').setLevel(logging.INFO) 34 | 35 | class FlightStage(Enum): 36 | INIT=0 37 | PREFLIGHT=1 38 | UPLOAD=2 39 | TAKEOFF=3 40 | GO=4 41 | LAND=5 42 | ABORT=6 43 | 44 | if sys.version_info[0] != 3: 45 | print("This script requires Python 3") 46 | exit() 47 | 48 | # Change uris and sequences according to your setup 49 | DRONES = [ 50 | 'radio://2/80/250K/E7E7E7E7E8', 51 | 52 | 'radio://0/0/250K/E7E7E7E7E0', 53 | 'radio://1/40/250K/E7E7E7E7E1', 54 | 'radio://2/80/250K/E7E7E7E7E2', 55 | 56 | 'radio://0/0/250K/E7E7E7E7E3', 57 | 'radio://1/40/250K/E7E7E7E7E4', 58 | 'radio://2/80/250K/E7E7E7E7E5', 59 | 60 | 'radio://0/0/250K/E7E7E7E7E6', 61 | 'radio://1/40/250K/E7E7E7E7E7', 62 | ] 63 | 64 | SEND_FULL_POSE = True 65 | 66 | 67 | 68 | def assignments(count): 69 | if count > len(DRONES) or count <= 0: 70 | msg = 'unknown number of drones' 71 | logger.error(msg) 72 | raise ValueError(msg) 73 | 74 | uris = DRONES[:count] 75 | n_drones = len(uris) 76 | 77 | body_names = [] 78 | led_id = [[1,3,4,2]] 79 | for i in range(n_drones): 80 | body_names.append('cf'+str(i)) 81 | if i != 0: 82 | led_id.append([i*4 + j for j in led_id[0]]) 83 | 84 | # print('Defined bodies are: ', body_names) 85 | # print('Deck ids are:', led_id) 86 | 87 | uri_to_traj_id = {uri: i for i, uri in enumerate(uris)} 88 | uri_to_led_id = {uri: led_id[i] for i, uri in enumerate(uris)} 89 | uri_to_rigid_body = {uri: body_names[i] for i, uri in enumerate(uris)} 90 | return { 91 | 'uris': uris, 92 | 'uri_to_traj_id': uri_to_traj_id, 93 | 'uri_to_rigid_body': uri_to_rigid_body, 94 | 'uri_to_led_id': uri_to_led_id, 95 | 'n_drones': n_drones, 96 | 'body_names': body_names 97 | } 98 | 99 | 100 | class UnstableException(Exception): 101 | pass 102 | 103 | 104 | class LowBatteryException(Exception): 105 | pass 106 | 107 | 108 | class QtmWrapper(Thread): 109 | def __init__(self, body_names): 110 | Thread.__init__(self) 111 | 112 | self.body_names = body_names 113 | self.on_pose = {name: None for name in body_names} 114 | self.connection = None 115 | self.qtm_6DoF_labels = [] 116 | self._stay_open = True 117 | 118 | self.last_send = time.time() 119 | self.dt_min = 0.2 # reducing send_extpose rate to 5HZ 120 | 121 | self.start() 122 | 123 | def close(self): 124 | self._stay_open = False 125 | self.join() 126 | 127 | def run(self): 128 | asyncio.run(self._life_cycle()) 129 | 130 | async def _life_cycle(self): 131 | await self._connect() 132 | while(self._stay_open): 133 | await asyncio.sleep(1) 134 | await self._close() 135 | 136 | async def _connect(self): 137 | # qtm_instance = await self._discover() 138 | # host = qtm_instance.host 139 | host = "192.168.1.2" 140 | logger.info('Connecting to QTM on %s', host) 141 | self.connection = await qtm.connect(host=host, version="1.20") # version 1.21 has weird 6DOF labels, so using 1.20 here 142 | 143 | params = await self.connection.get_parameters(parameters=['6d']) 144 | xml = ET.fromstring(params) 145 | self.qtm_6DoF_labels = [label.text for label in xml.iter('Name')] 146 | 147 | await self.connection.stream_frames( 148 | components=['6D'], 149 | on_packet=self._on_packet) 150 | logger.info('Connected to QTM') 151 | 152 | 153 | async def _discover(self): 154 | async for qtm_instance in qtm.Discover('0.0.0.0'): 155 | return qtm_instance 156 | 157 | def _on_packet(self, packet): 158 | now = time.time() 159 | dt = now - self.last_send 160 | if dt < self.dt_min: 161 | return 162 | self.last_send = time.time() 163 | # print('Hz: ', 1.0/dt) 164 | 165 | header, bodies = packet.get_6d() 166 | 167 | if bodies is None: 168 | return 169 | 170 | intersect = set(self.qtm_6DoF_labels).intersection(self.body_names) 171 | if len(intersect) < len(self.body_names) : 172 | logger.error('Missing rigid bodies') 173 | logger.error('In QTM: %s', str(self.qtm_6DoF_labels)) 174 | logger.error('Intersection: %s', str(intersect)) 175 | return 176 | else: 177 | for body_name in self.body_names: 178 | index = self.qtm_6DoF_labels.index(body_name) 179 | temp_cf_pos = bodies[index] 180 | x = temp_cf_pos[0][0] / 1000 181 | y = temp_cf_pos[0][1] / 1000 182 | z = temp_cf_pos[0][2] / 1000 183 | 184 | r = temp_cf_pos[1].matrix 185 | rot = [ 186 | [r[0], r[3], r[6]], 187 | [r[1], r[4], r[7]], 188 | [r[2], r[5], r[8]], 189 | ] 190 | 191 | if self.on_pose[body_name]: 192 | # Make sure we got a position 193 | if math.isnan(x): 194 | logger.error("======= LOST RB TRACKING : %s", body_name) 195 | continue 196 | 197 | self.on_pose[body_name]([x, y, z, rot]) 198 | 199 | async def _close(self): 200 | await self.connection.stream_frames_stop() 201 | self.connection.disconnect() 202 | 203 | def _sqrt(a): 204 | """ 205 | There might be rounding errors making 'a' slightly negative. 206 | Make sure we don't throw an exception. 207 | """ 208 | if a < 0.0: 209 | return 0.0 210 | return math.sqrt(a) 211 | 212 | def send_extpose_rot_matrix(scf: SyncCrazyflie, x, y, z, rot): 213 | """ 214 | Send the current Crazyflie X, Y, Z position and attitude as a (3x3) 215 | rotaton matrix. This is going to be forwarded to the Crazyflie's 216 | position estimator. 217 | By default the attitude is not sent. 218 | """ 219 | cf = scf.cf 220 | 221 | if SEND_FULL_POSE: 222 | trace = rot[0][0] + rot[1][1] + rot[2][2] 223 | if trace > 0: 224 | a = _sqrt(1 + trace) 225 | qw = 0.5*a 226 | b = 0.5/a 227 | qx = (rot[2][1] - rot[1][2])*b 228 | qy = (rot[0][2] - rot[2][0])*b 229 | qz = (rot[1][0] - rot[0][1])*b 230 | elif rot[0][0] > rot[1][1] and rot[0][0] > rot[2][2]: 231 | a = _sqrt(1 + rot[0][0] - rot[1][1] - rot[2][2]) 232 | qx = 0.5*a 233 | b = 0.5/a 234 | qw = (rot[2][1] - rot[1][2])*b 235 | qy = (rot[1][0] + rot[0][1])*b 236 | qz = (rot[0][2] + rot[2][0])*b 237 | elif rot[1][1] > rot[2][2]: 238 | a = _sqrt(1 - rot[0][0] + rot[1][1] - rot[2][2]) 239 | qy = 0.5*a 240 | b = 0.5/a 241 | qw = (rot[0][2] - rot[2][0])*b 242 | qx = (rot[1][0] + rot[0][1])*b 243 | qz = (rot[2][1] + rot[1][2])*b 244 | else: 245 | a = _sqrt(1 - rot[0][0] - rot[1][1] + rot[2][2]) 246 | qz = 0.5*a 247 | b = 0.5/a 248 | qw = (rot[1][0] - rot[0][1])*b 249 | qx = (rot[0][2] + rot[2][0])*b 250 | qy = (rot[2][1] + rot[1][2])*b 251 | cf.extpos.send_extpose(x, y, z, qx, qy, qz, qw) 252 | else: 253 | cf.extpos.send_extpos(x, y, z) 254 | 255 | class Uploader: 256 | def __init__(self, trajectory_mem): 257 | self._is_done = False 258 | self.trajectory_mem = trajectory_mem 259 | 260 | def upload(self): 261 | logger.info('uploading') 262 | self.trajectory_mem.write_data(self._upload_done) 263 | while not self._is_done: 264 | time.sleep(1) 265 | 266 | def _upload_done(self, mem, addr): 267 | logger.info('upload is done') 268 | self._is_done = True 269 | self.trajectory_mem.disconnect() 270 | 271 | 272 | def check_battery(scf: SyncCrazyflie, min_voltage=4): 273 | logger.info('Checking battery...') 274 | log_config = LogConfig(name='Battery', period_in_ms=500) 275 | log_config.add_variable('pm.vbat', 'float') 276 | 277 | with SyncLogger(scf, log_config) as sync_logger: 278 | for log_entry in sync_logger: 279 | log_data = log_entry[1] 280 | vbat = log_data['pm.vbat'] 281 | if vbat < min_voltage: 282 | msg = "battery too low: {:10.4f} V, for {:s}".format( 283 | vbat, scf.cf.link_uri) 284 | logger.error(msg) 285 | raise LowBatteryException(msg) 286 | else: 287 | return 288 | 289 | 290 | def check_state(scf: SyncCrazyflie): 291 | logger.info('Checking state.') 292 | log_config = LogConfig(name='State', period_in_ms=500) 293 | log_config.add_variable('stabilizer.roll', 'float') 294 | log_config.add_variable('stabilizer.pitch', 'float') 295 | log_config.add_variable('stabilizer.yaw', 'float') 296 | logger.info('Log configured.') 297 | 298 | with SyncLogger(scf, log_config) as sync_logger: 299 | for log_entry in sync_logger: 300 | log_data = log_entry[1] 301 | roll = log_data['stabilizer.roll'] 302 | pitch = log_data['stabilizer.pitch'] 303 | yaw = log_data['stabilizer.yaw'] 304 | logger.info('%s Checking roll/pitch/yaw', scf.cf.link_uri) 305 | 306 | #('yaw', yaw) 307 | if SEND_FULL_POSE: 308 | euler_checks = [('roll', roll, 5), ('pitch', pitch, 5)] 309 | else: 310 | euler_checks = [('roll', roll, 5), ('pitch', pitch, 5), ('yaw', yaw, 5)] 311 | 312 | for name, val, val_max in euler_checks: 313 | if np.abs(val) > val_max: 314 | msg = "too much {:s}, {:10.4f} deg, for {:s}".format( 315 | name, val, scf.cf.link_uri) 316 | logger.error(msg) 317 | raise UnstableException(msg) 318 | return 319 | 320 | 321 | def wait_for_position_estimator(scf: SyncCrazyflie, duration: float = 10.0): 322 | logger.info('Waiting for estimator to find position...') 323 | 324 | dt = 0.5 325 | log_config = LogConfig(name='Kalman Variance', period_in_ms=dt*1000) 326 | log_config.add_variable('kalman.varPX', 'float') 327 | log_config.add_variable('kalman.varPY', 'float') 328 | log_config.add_variable('kalman.varPZ', 'float') 329 | 330 | var_y_history = [1000] * 10 331 | var_x_history = [1000] * 10 332 | var_z_history = [1000] * 10 333 | 334 | threshold = 0.001 335 | t = 0 336 | 337 | with SyncLogger(scf.cf, log_config) as sync_logger: 338 | for log_entry in sync_logger: 339 | log_data = log_entry[1] 340 | 341 | var_x_history.append(log_data['kalman.varPX']) 342 | var_x_history.pop(0) 343 | var_y_history.append(log_data['kalman.varPY']) 344 | var_y_history.pop(0) 345 | var_z_history.append(log_data['kalman.varPZ']) 346 | var_z_history.pop(0) 347 | 348 | min_x = min(var_x_history) 349 | max_x = max(var_x_history) 350 | min_y = min(var_y_history) 351 | max_y = max(var_y_history) 352 | min_z = min(var_z_history) 353 | max_z = max(var_z_history) 354 | 355 | if (max_x - min_x) < threshold and ( 356 | max_y - min_y) < threshold and ( 357 | max_z - min_z) < threshold: 358 | break 359 | else: 360 | if t > duration: 361 | msg = "estimator failed after {} seconds: {:s}\t{:10g}\t{:10g}\t{:10g}".format( 362 | duration, scf.cf.link_uri, max_x - min_x, max_y - min_y, max_z - min_z) 363 | logger.error(msg) 364 | raise Exception(msg) 365 | t += dt 366 | 367 | 368 | def wait_for_param_download(scf: SyncCrazyflie): 369 | while not scf.cf.param.is_updated: 370 | time.sleep(1.0) 371 | logger.info('Parameters downloaded for %s', scf.cf.link_uri) 372 | 373 | 374 | def reset_estimator(scf: SyncCrazyflie): 375 | logger.info('Resetting estimator...') 376 | cf = scf.cf 377 | cf.param.set_value('kalman.resetEstimation', '1') 378 | time.sleep(0.1) 379 | cf.param.set_value('kalman.resetEstimation', '0') 380 | wait_for_position_estimator(scf, 10) 381 | 382 | def activate_kalman_estimator(cf): 383 | cf.param.set_value('stabilizer.estimator', '2') 384 | 385 | # Set the std deviation for the quaternion data pushed into the 386 | # kalman filter. The default value seems to be a bit too low. 387 | cf.param.set_value('locSrv.extQuatStdDev', 0.06) 388 | 389 | def sleep_while_checking_stable(scf: SyncCrazyflie, tf_sec, dt_sec=0.1, check_low_battery=True): 390 | if tf_sec == 0: 391 | return 392 | log_config = LogConfig(name='Roll', period_in_ms=int(dt_sec*1000.0)) 393 | log_config.add_variable('stabilizer.roll', 'float') 394 | log_config.add_variable('pm.vbat', 'float') 395 | t_sec = 0 396 | batt_lowpass = 3.7 397 | RC = 0.25 # time constant in seconds of first order low pass filter 398 | alpha = dt_sec/(RC + dt_sec) 399 | # print('sleeping {:10.4f} seconds'.format(tf_sec)) 400 | with SyncLogger(scf, log_config) as sync_logger: 401 | for log_entry in sync_logger: 402 | log_data = log_entry[1] 403 | roll = log_data['stabilizer.roll'] 404 | batt = log_data['pm.vbat'] 405 | if np.abs(roll) > 60: 406 | msg = "flip detected {:10.4f} deg, for {:s}".format(roll, scf.cf.link_uri) 407 | logger.error(msg) 408 | raise UnstableException(msg) 409 | batt_lowpass = (1 - alpha)*batt_lowpass + alpha*batt 410 | # print("battery {:10.4f} V, for {:s}".format(batt_lowpass, scf.cf.link_uri)) 411 | if batt_lowpass < 2.9 and check_low_battery: 412 | msg = "low battery {:10.4f} V, for {:s}".format(batt, scf.cf.link_uri) 413 | logger.error(msg) 414 | raise LowBatteryException(msg) 415 | t_sec += dt_sec 416 | if t_sec>tf_sec: 417 | return 418 | 419 | 420 | def upload_trajectory(scf: SyncCrazyflie, data: Dict): 421 | logger.info('uploading trajectory at flight stage: %s', data['flight_stage']) 422 | 423 | cf = scf.cf # type: Crazyflie 424 | 425 | trajectory_mem = scf.cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] 426 | 427 | TRAJECTORY_MAX_LENGTH = 31 428 | trajectory = data['traj']['trajectory'] 429 | 430 | if len(trajectory) > TRAJECTORY_MAX_LENGTH: 431 | msg = "Trajectory too long for drone {:s}".format(cf.link_uri) 432 | logger.error(msg) 433 | raise ValueError(msg) 434 | 435 | for row in trajectory: 436 | duration = row[0] 437 | x = Poly4D.Poly(row[1:9]) 438 | y = Poly4D.Poly(row[9:17]) 439 | z = Poly4D.Poly(row[17:25]) 440 | yaw = Poly4D.Poly(row[25:33]) 441 | trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw)) 442 | 443 | logger.info('%s Calling upload method', scf.cf.link_uri) 444 | uploader = Uploader(trajectory_mem) 445 | uploader.upload() 446 | 447 | logger.info('%s Defining trajectory', scf.cf.link_uri) 448 | cf.high_level_commander.define_trajectory( 449 | trajectory_id=1, offset=0, n_pieces=len(trajectory_mem.poly4Ds)) 450 | 451 | data['flight_stage'] = FlightStage.UPLOAD 452 | 453 | 454 | def preflight_sequence(scf: SyncCrazyflie, data: Dict): 455 | """ 456 | This is the preflight sequence. It calls all other subroutines before takeoff. 457 | """ 458 | logger.info('%s preflight at flight stage: %s', scf.cf.link_uri, data['flight_stage']) 459 | 460 | cf = scf.cf # type: Crazyflie 461 | 462 | # switch to Kalman filter 463 | cf.param.set_value('stabilizer.estimator', '2') 464 | cf.param.set_value('locSrv.extQuatStdDev', 0.06) 465 | 466 | # enable high level commander 467 | cf.param.set_value('commander.enHighLevel', '1') 468 | 469 | # prepare for motor shut-off 470 | cf.param.set_value('motorPowerSet.enable', '0') 471 | cf.param.set_value('motorPowerSet.m1', '0') 472 | cf.param.set_value('motorPowerSet.m2', '0') 473 | cf.param.set_value('motorPowerSet.m3', '0') 474 | cf.param.set_value('motorPowerSet.m4', '0') 475 | 476 | # ensure params are downloaded 477 | wait_for_param_download(scf) 478 | 479 | cf.param.set_value('ring.effect', '0') 480 | 481 | # set pid gains, tune down Kp to smooth trajectories 482 | cf.param.set_value('posCtlPid.xKp', '1') 483 | cf.param.set_value('posCtlPid.yKp', '1') 484 | cf.param.set_value('posCtlPid.zKp', '1') 485 | 486 | # check battery level 487 | check_battery(scf, 3.7) 488 | 489 | # reset the estimator 490 | reset_estimator(scf) 491 | 492 | # check state 493 | check_state(scf) 494 | 495 | data['flight_stage'] = FlightStage.PREFLIGHT 496 | 497 | 498 | def takeoff_sequence(scf: SyncCrazyflie, data: Dict): 499 | """ 500 | This is the takeoff sequence. It commands takeoff. 501 | """ 502 | logger.info('%s takeoff at flight stage: %s', scf.cf.link_uri, data['flight_stage']) 503 | 504 | try: 505 | cf = scf.cf # type: Crazyflie 506 | commander = cf.high_level_commander # type: cflib.HighLevelCOmmander 507 | cf.param.set_value('commander.enHighLevel', '1') 508 | cf.param.set_value('ring.effect', '7') 509 | cf.param.set_value('ring.solidRed', str(0)) 510 | cf.param.set_value('ring.solidGreen', str(255)) 511 | cf.param.set_value('ring.solidBlue', str(0)) 512 | commander.takeoff(1.5, 3.0) 513 | sleep_while_checking_stable(scf, tf_sec=10) 514 | 515 | except UnstableException as e: 516 | logger.error('%s unstable exception: %s', scf.cf.link_uri, e) 517 | kill_motor_sequence(scf, data) 518 | # raising here since we want to kill entire show if one fails early 519 | raise(e) 520 | 521 | except LowBatteryException as e: 522 | logger.error('low battery exception: %s', e) 523 | kill_motor_sequence(scf, data) 524 | 525 | except Exception as e: 526 | logger.error('%s general exception: %s', scf.cf.link_uri, e) 527 | kill_motor_sequence(scf, data) 528 | raise(e) 529 | 530 | data['flight_stage'] = FlightStage.TAKEOFF 531 | 532 | 533 | def go_sequence(scf: SyncCrazyflie, data: Dict): 534 | """ 535 | This is the go sequence. It commands the trajectory to start. 536 | """ 537 | logger.info('%s go at flight stage: %s', scf.cf.link_uri, data['flight_stage']) 538 | data['flight_stage'] = FlightStage.TAKEOFF 539 | 540 | try: 541 | cf = scf.cf # type: Crazyflie 542 | commander = cf.high_level_commander # type: cflib.HighLevelCOmmander 543 | commander.start_trajectory( 544 | trajectory_id=1, time_scale=1.0, relative=False) 545 | 546 | intensity = 1 # 0-1 547 | 548 | # initial led color 549 | cf.param.set_value('ring.effect', '7') 550 | cf.param.set_value('ring.solidRed', str(0)) 551 | cf.param.set_value('ring.solidGreen', str(255)) 552 | cf.param.set_value('ring.solidBlue', str(0)) 553 | time.sleep(0.1) 554 | 555 | traj = data['traj'] 556 | for color, delay, T in zip(traj['color'], traj['delay'], traj['T']): 557 | # change led color 558 | red = int(intensity * color[0]) 559 | green = int(intensity * color[1]) 560 | blue = int(intensity * color[2]) 561 | #print('setting color', red, blue, green) 562 | sleep_while_checking_stable(scf, tf_sec=delay) 563 | cf.param.set_value('ring.solidRed', str(red)) 564 | cf.param.set_value('ring.solidBlue', str(blue)) 565 | cf.param.set_value('ring.solidGreen', str(green)) 566 | # wait for leg to complete 567 | # print('sleeping leg duration', leg_duration) 568 | sleep_while_checking_stable(scf, tf_sec=T - delay) 569 | 570 | except UnstableException as e: 571 | logger.error('go general exceptoin: %s', e) 572 | kill_motor_sequence(scf, data) 573 | # raise if you want flight to stop if one drone crashes 574 | #raise(e) 575 | 576 | except LowBatteryException as e: 577 | logger.error('go low battery exception: %s', e) 578 | # have this vehicle land 579 | land_sequence(scf, data) 580 | 581 | except Exception as e: 582 | logger.error('%s go general exception: %s', scf.cf.link_uri, e) 583 | land_sequence(scf, data) 584 | # raising here since we don't know what this exception is 585 | raise(e) 586 | 587 | 588 | def kill_motor_sequence(scf: Crazyflie, data: Dict): 589 | logger.info('%s killing motors at flight stage: %s', scf.cf.link_uri, data['flight_stage']) 590 | cf = scf.cf 591 | cf.param.set_value('commander.enHighLevel', '0') 592 | cf.param.set_value('motorPowerSet.enable', '1') 593 | data['flight_stage'] = FlightStage.ABORT 594 | 595 | 596 | def land_sequence(scf: Crazyflie, data: Dict): 597 | logger.info('%slanding at flight stage: %s', scf.cf.link_uri, data['flight_stage']) 598 | flight_stage = data['flight_stage'] 599 | if flight_stage == FlightStage.ABORT or \ 600 | flight_stage == FlightStage.LAND or \ 601 | flight_stage.value < FlightStage.TAKEOFF.value: 602 | kill_motor_sequence(scf, data) 603 | logger.error('land called, but not flying, killing motors') 604 | return 605 | 606 | try: 607 | cf = scf.cf # type: Crazyflie 608 | commander = cf.high_level_commander # type: cflib.HighLevelCOmmander 609 | commander.land(0.2, 5.0) 610 | logger.info('%s Landing...', scf.cf.link_uri) 611 | sleep_while_checking_stable(scf, tf_sec=5, check_low_battery=False) 612 | 613 | # disable led to save battery 614 | cf.param.set_value('ring.effect', '0') 615 | commander.stop() 616 | kill_motor_sequence(scf, data) 617 | 618 | except UnstableException as e: 619 | logger.error('%s land unstable exception: %s', scf.cf.link_uri, e) 620 | kill_motor_sequence(scf, data) 621 | 622 | except Exception as e: 623 | logger.error('%s land general exception: %s', scf.cf.link_uri, e) 624 | kill_motor_sequence(scf, data) 625 | 626 | data['flight_stage'] = FlightStage.LAND 627 | 628 | 629 | def send6DOF(scf: SyncCrazyflie, data: Dict): 630 | """ 631 | This relays mocap data to each crazyflie 632 | """ 633 | try: 634 | data['qtmWrapper'].on_pose[data['rigid_body']] = lambda pose: send_extpose_rot_matrix(scf, pose[0], pose[1], pose[2], pose[3]) 635 | except Exception as e: 636 | logger.error('send6DOF general exception: %s', e) 637 | logger.error('Could not relay mocap data') 638 | land_sequence(scf, data) 639 | 640 | def id_update(scf: SyncCrazyflie, id: List): 641 | cf = scf.cf 642 | cf.param.set_value('activeMarker.mode', '3') # qualisys mode 643 | time.sleep(1) 644 | 645 | # default id 646 | # [f, b, l, r] 647 | # [1, 3, 4, 2] 648 | 649 | # disable led to save battery 650 | cf.param.set_value('ring.effect', '0') 651 | t_sleep = 1 # seconds 652 | cf.param.set_value('activeMarker.front', str(id[0])) 653 | time.sleep(t_sleep) 654 | cf.param.set_value('activeMarker.back', str(id[1])) 655 | time.sleep(t_sleep) 656 | cf.param.set_value('activeMarker.left', str(id[2])) 657 | time.sleep(t_sleep) 658 | cf.param.set_value('activeMarker.right', str(id[3])) 659 | time.sleep(t_sleep) 660 | 661 | logger.info('ID update done! | %s', scf._link_uri) 662 | 663 | def swarm_id_update(args): 664 | cflib.crtp.init_drivers(enable_debug_driver=False) 665 | assign = assignments(int(args.count)) 666 | factory = CachedCfFactory(rw_cache='./cache') 667 | swarm_args = { 668 | uri: [assign['uri_to_led_id'][uri]] 669 | for uri in assign['uris'] 670 | } 671 | 672 | with Swarm(assign['uris'], factory=factory) as swarm: 673 | logger.info('Starting ID update...') 674 | swarm.sequential(id_update, swarm_args) # parallel update has some issue with more than 3 drones, using sequential update here. 675 | 676 | def hover(args): 677 | cflib.crtp.init_drivers(enable_debug_driver=False) 678 | assign = assignments(int(args.count)) 679 | factory = CachedCfFactory(rw_cache='./cache') 680 | qtmWrapper = QtmWrapper(assign['body_names']) 681 | swarm_args = {} 682 | for uri in assign['uris']: 683 | swarm_args[uri] = [{ 684 | 'qtmWrapper': qtmWrapper, 685 | 'rigid_body': assign['uri_to_rigid_body'][uri], 686 | 'flight_stage': FlightStage.INIT 687 | }] 688 | 689 | with Swarm(assign['uris'], factory=factory) as swarm: 690 | 691 | logger.info('swarm links created') 692 | 693 | try: 694 | logger.info('Starting mocap data relay...') 695 | swarm.parallel_safe(send6DOF, swarm_args) 696 | 697 | logger.info('Preflight sequence...') 698 | swarm.parallel_safe(preflight_sequence, swarm_args) 699 | 700 | logger.info('Takeoff sequence...') 701 | swarm.parallel(takeoff_sequence, swarm_args) 702 | 703 | except KeyboardInterrupt: 704 | pass 705 | 706 | except Exception as e: 707 | print(e) 708 | 709 | logger.info('Land sequence...') 710 | swarm.parallel(land_sequence, swarm_args) 711 | 712 | logger.info('Closing QTM connection...') 713 | qtmWrapper.close() 714 | 715 | def run(args): 716 | assign = assignments(int(args.count)) 717 | cflib.crtp.init_drivers(enable_debug_driver=False) 718 | factory = CachedCfFactory(rw_cache='./cache') 719 | with open(args.json, 'r') as f: 720 | traj_data = json.load(f) 721 | qtmWrapper = QtmWrapper(assign['body_names']) 722 | swarm_args = {} 723 | for uri in assign['uris']: 724 | swarm_args[uri] = [{ 725 | 'qtmWrapper': qtmWrapper, 726 | 'rigid_body': assign['uri_to_rigid_body'][uri], 727 | 'flight_stage': FlightStage.INIT, 728 | 'traj': traj_data[str(assign['uri_to_traj_id'][uri])] 729 | }] 730 | 731 | with Swarm(assign['uris'], factory=factory) as swarm: 732 | 733 | try: 734 | logger.info('Starting mocap data relay...') 735 | swarm.parallel_safe(send6DOF, swarm_args) 736 | 737 | logger.info('Preflight sequence...') 738 | swarm.parallel_safe(preflight_sequence, swarm_args) 739 | 740 | logger.info('Upload sequence...') 741 | swarm.parallel_safe(upload_trajectory, swarm_args) 742 | 743 | logger.info('Takeoff sequence...') 744 | swarm.parallel(takeoff_sequence, swarm_args) 745 | 746 | # repeat = int(data['repeat']) 747 | repeat = 1 748 | for trajectory_count in range(repeat): 749 | logger.info('Go sequence...') 750 | swarm.parallel(go_sequence, swarm_args) 751 | 752 | except KeyboardInterrupt: 753 | pass 754 | 755 | except Exception as e: 756 | logger.error('swarm general exception: %s', e) 757 | 758 | logger.info('Land sequence...') 759 | swarm.parallel(land_sequence, swarm_args) 760 | 761 | logger.info('Closing QTM connection...') 762 | qtmWrapper.close() 763 | 764 | if __name__ == "__main__": 765 | parser = argparse.ArgumentParser() 766 | parser.add_argument('mode') 767 | 768 | parser.add_argument('--json') 769 | parser.add_argument('--count') 770 | args = parser.parse_args() 771 | 772 | if args.mode == 'id': 773 | swarm_id_update(args) 774 | elif args.mode == 'traj': 775 | run(args) 776 | elif args.mode == 'hover': 777 | hover(args) 778 | else: 779 | logger.error('Not a valid input!!!') 780 | -------------------------------------------------------------------------------- /scripts/flight_script.py: -------------------------------------------------------------------------------- 1 | import time 2 | import signal 3 | import sys 4 | import json 5 | import argparse 6 | import numpy as np 7 | from typing import Dict 8 | 9 | import cflib.crtp 10 | from cflib.crazyflie.log import LogConfig 11 | from cflib.crazyflie.swarm import CachedCfFactory 12 | from cflib.crazyflie.swarm import Swarm 13 | from cflib.crazyflie.syncLogger import SyncLogger 14 | from cflib.crazyflie.mem import MemoryElement 15 | from cflib.crazyflie.mem import Poly4D 16 | from typing import List, Dict 17 | from cflib.crazyflie.syncCrazyflie import SyncCrazyflie 18 | from cflib.crazyflie import Crazyflie 19 | 20 | if sys.version_info[0] != 3: 21 | print("This script requires Python 3") 22 | exit() 23 | 24 | # Change uris and sequences according to your setup 25 | DRONE0 = 'radio://0/70/2M/E7E7E7E701' 26 | DRONE1 = 'radio://0/70/2M/E7E7E7E702' 27 | DRONE2 = 'radio://0/70/2M/E7E7E7E703' 28 | DRONE3 = 'radio://0/70/2M/E7E7E7E704' 29 | DRONE4 = 'radio://0/70/2M/E7E7E7E705' 30 | DRONE5 = 'radio://0/70/2M/E7E7E7E706' 31 | DRONE6 = 'radio://0/80/2M/E7E7E7E707' 32 | DRONE7 = 'radio://0/80/2M/E7E7E7E708' 33 | DRONE8 = 'radio://0/80/2M/E7E7E7E709' 34 | DRONE9 = 'radio://0/80/2M/E7E7E7E710' 35 | DRONE10 = 'radio://0/80/2M/E7E7E7E711' 36 | DRONE11 = 'radio://0/90/2M/E7E7E7E712' 37 | DRONE12 = 'radio://0/90/2M/E7E7E7E713' 38 | DRONE13 = 'radio://0/90/2M/E7E7E7E714' 39 | DRONE14 = 'radio://0/90/2M/E7E7E7E715' 40 | DRONE15 = 'radio://0/100/2M/E7E7E7E716' 41 | DRONE16 = 'radio://0/100/2M/E7E7E7E717' 42 | DRONE17 = 'radio://0/100/2M/E7E7E7E718' 43 | DRONE18 = 'radio://0/100/2M/E7E7E7E719' 44 | DRONE19 = 'radio://0/100/2M/E7E7E7E720' 45 | DRONE20 = 'radio://0/100/2M/E7E7E7E721' 46 | 47 | # List of URIs, comment the one you do not want to fly 48 | # DRONE4 ## Faulty Drone // Does not work 49 | trajectory_assignment = { 50 | 0: DRONE15, 51 | 1: DRONE19, 52 | 2: DRONE2, 53 | 3: DRONE9, 54 | 4: DRONE16, 55 | 5: DRONE12, 56 | # 6: DRONE7, 57 | # 7: DRONE17, 58 | # 8: DRONE18, 59 | # 9: DRONE10, 60 | # 10: DRONE11, 61 | # 11: DRONE12, 62 | # 12: DRONE13, 63 | } 64 | 65 | class Uploader: 66 | def __init__(self, trajectory_mem): 67 | self._is_done = False 68 | self.trajectory_mem = trajectory_mem 69 | 70 | def upload(self): 71 | print('upload started') 72 | self.trajectory_mem.write_data(self._upload_done) 73 | while not self._is_done: 74 | print('uploading...') 75 | time.sleep(1) 76 | 77 | def _upload_done(self, mem, addr): 78 | print('upload is done') 79 | self._is_done = True 80 | self.trajectory_mem.disconnect() 81 | 82 | 83 | def check_battery(scf: SyncCrazyflie, min_voltage=4.0): 84 | print('Checking battery...') 85 | log_config = LogConfig(name='Battery', period_in_ms=500) 86 | log_config.add_variable('pm.vbat', 'float') 87 | 88 | with SyncLogger(scf, log_config) as logger: 89 | for log_entry in logger: 90 | log_data = log_entry[1] 91 | vbat = log_data['pm.vbat'] 92 | if log_data['pm.vbat'] < min_voltage: 93 | msg = "battery too low: {:10.4f} V, for {:s}".format( 94 | vbat, scf.cf.link_uri) 95 | raise Exception(msg) 96 | else: 97 | return 98 | 99 | 100 | def check_state(scf: SyncCrazyflie, min_voltage=4.0): 101 | print('Checking state.') 102 | log_config = LogConfig(name='State', period_in_ms=500) 103 | log_config.add_variable('stabilizer.roll', 'float') 104 | log_config.add_variable('stabilizer.pitch', 'float') 105 | log_config.add_variable('stabilizer.yaw', 'float') 106 | print('Log configured.') 107 | 108 | with SyncLogger(scf, log_config) as logger: 109 | for log_entry in logger: 110 | log_data = log_entry[1] 111 | roll = log_data['stabilizer.roll'] 112 | pitch = log_data['stabilizer.pitch'] 113 | yaw = log_data['stabilizer.yaw'] 114 | print('Checking roll/pitch/yaw.') 115 | 116 | for name, val in [('roll', roll), ('pitch', pitch), ('yaw', yaw)]: 117 | 118 | if np.abs(val) > 20: 119 | print('exceeded') 120 | msg = "too much {:s}, {:10.4f} deg, for {:s}".format( 121 | name, val, scf.cf.link_uri) 122 | print(msg) 123 | raise Exception(msg) 124 | return 125 | 126 | 127 | def wait_for_position_estimator(scf: SyncCrazyflie): 128 | print('Waiting for estimator to find position...',) 129 | 130 | log_config = LogConfig(name='Kalman Variance', period_in_ms=500) 131 | log_config.add_variable('kalman.varPX', 'float') 132 | log_config.add_variable('kalman.varPY', 'float') 133 | log_config.add_variable('kalman.varPZ', 'float') 134 | 135 | var_y_history = [1000] * 10 136 | var_x_history = [1000] * 10 137 | var_z_history = [1000] * 10 138 | 139 | threshold = 0.001 140 | 141 | with SyncLogger(scf.cf, log_config) as logger: 142 | for log_entry in logger: 143 | log_data = log_entry[1] 144 | 145 | var_x_history.append(log_data['kalman.varPX']) 146 | var_x_history.pop(0) 147 | var_y_history.append(log_data['kalman.varPY']) 148 | var_y_history.pop(0) 149 | var_z_history.append(log_data['kalman.varPZ']) 150 | var_z_history.pop(0) 151 | 152 | min_x = min(var_x_history) 153 | max_x = max(var_x_history) 154 | min_y = min(var_y_history) 155 | max_y = max(var_y_history) 156 | min_z = min(var_z_history) 157 | max_z = max(var_z_history) 158 | 159 | if (max_x - min_x) < threshold and ( 160 | max_y - min_y) < threshold and ( 161 | max_z - min_z) < threshold: 162 | break 163 | else: 164 | print("{:s}\t{:10g}\t{:10g}\t{:10g}". 165 | format(scf.cf.link_uri, max_x - min_x, max_y - min_y, max_z - min_z)) 166 | 167 | 168 | def wait_for_param_download(scf: SyncCrazyflie): 169 | while not scf.cf.param.is_updated: 170 | time.sleep(1.0) 171 | print('Parameters downloaded for', scf.cf.link_uri) 172 | 173 | 174 | def reset_estimator(scf: SyncCrazyflie): 175 | print('Resetting estimator...') 176 | cf = scf.cf 177 | cf.param.set_value('kalman.resetEstimation', '1') 178 | time.sleep(0.1) 179 | cf.param.set_value('kalman.resetEstimation', '0') 180 | wait_for_position_estimator(scf) 181 | 182 | 183 | def upload_trajectory(scf: SyncCrazyflie, data: Dict): 184 | try: 185 | cf = scf.cf # type: Crazyflie 186 | 187 | print('Starting upload') 188 | trajectory_mem = scf.cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0] 189 | 190 | TRAJECTORY_MAX_LENGTH = 31 191 | trajectory = data['trajectory'] 192 | 193 | if len(trajectory) > TRAJECTORY_MAX_LENGTH: 194 | raise ValueError("Trajectory too long for drone {:s}".format(cf.link_uri)) 195 | 196 | for row in trajectory: 197 | duration = row[0] 198 | x = Poly4D.Poly(row[1:9]) 199 | y = Poly4D.Poly(row[9:17]) 200 | z = Poly4D.Poly(row[17:25]) 201 | yaw = Poly4D.Poly(row[25:33]) 202 | trajectory_mem.poly4Ds.append(Poly4D(duration, x, y, z, yaw)) 203 | 204 | print('Calling upload method') 205 | uploader = Uploader(trajectory_mem) 206 | uploader.upload() 207 | 208 | print('Defining trajectory.') 209 | cf.high_level_commander.define_trajectory( 210 | trajectory_id=1, offset=0, n_pieces=len(trajectory_mem.poly4Ds)) 211 | 212 | except Exception as e: 213 | print(e) 214 | land_sequence(scf) 215 | raise(e) 216 | 217 | def preflight_sequence(scf: Crazyflie): 218 | """ 219 | This is the preflight sequence. It calls all other subroutines before takeoff. 220 | """ 221 | cf = scf.cf # type: Crazyflie 222 | 223 | try: 224 | # enable high level commander 225 | cf.param.set_value('commander.enHighLevel', '1') 226 | 227 | # ensure params are downloaded 228 | wait_for_param_download(scf) 229 | 230 | # make sure not already flying 231 | land_sequence(scf) 232 | 233 | # disable LED to save battery 234 | cf.param.set_value('ring.effect', '0') 235 | 236 | # set pid gains, tune down Kp to deal with UWB noise 237 | cf.param.set_value('posCtlPid.xKp', '1') 238 | cf.param.set_value('posCtlPid.yKp', '1') 239 | cf.param.set_value('posCtlPid.zKp', '1') 240 | 241 | # check battery level 242 | check_battery(scf, 4.0) 243 | 244 | # reset the estimator 245 | reset_estimator(scf) 246 | 247 | # check state 248 | check_state(scf) 249 | 250 | except Exception as e: 251 | print(e) 252 | land_sequence(scf) 253 | raise(e) 254 | 255 | 256 | def takeoff_sequence(scf: Crazyflie): 257 | """ 258 | This is the takeoff sequence. It commands takeoff. 259 | """ 260 | try: 261 | cf = scf.cf # type: Crazyflie 262 | commander = cf.high_level_commander # type: cflib.HighLevelCOmmander 263 | cf.param.set_value('commander.enHighLevel', '1') 264 | cf.param.set_value('ring.effect', '7') 265 | cf.param.set_value('ring.solidRed', str(0)) 266 | cf.param.set_value('ring.solidGreen', str(0)) 267 | cf.param.set_value('ring.solidBlue', str(0)) 268 | commander.takeoff(2, 3.0) 269 | time.sleep(3.0) 270 | except Exception as e: 271 | print(e) 272 | land_sequence(scf) 273 | 274 | 275 | def go_sequence(scf: Crazyflie, data: Dict): 276 | """ 277 | This is the go sequence. It commands the trajectory to start and runs the lighting. 278 | """ 279 | try: 280 | cf = scf.cf # type: Crazyflie 281 | commander = cf.high_level_commander # type: cflib.HighLevelCOmmander 282 | commander.start_trajectory( 283 | trajectory_id=1, time_scale=1.0, relative=False) 284 | intensity = 1 # 0-1 285 | 286 | # initial led color 287 | cf.param.set_value('ring.effect', '7') 288 | cf.param.set_value('ring.solidRed', str(0)) 289 | cf.param.set_value('ring.solidGreen', str(255)) 290 | cf.param.set_value('ring.solidBlue', str(0)) 291 | time.sleep(0.1) 292 | 293 | for color, delay, T in zip(data['color'], data['delay'], data['T']): 294 | # change led color 295 | red = int(intensity * color[0]) 296 | green = int(intensity * color[1]) 297 | blue = int(intensity * color[2]) 298 | #print('setting color', red, blue, green) 299 | time.sleep(delay) 300 | cf.param.set_value('ring.solidRed', str(red)) 301 | cf.param.set_value('ring.solidBlue', str(blue)) 302 | cf.param.set_value('ring.solidGreen', str(green)) 303 | # wait for leg to complete 304 | # print('sleeping leg duration', leg_duration) 305 | time.sleep(T - delay) 306 | 307 | except Exception as e: 308 | print(e) 309 | land_sequence(scf) 310 | 311 | 312 | def land_sequence(scf: Crazyflie): 313 | try: 314 | cf = scf.cf # type: Crazyflie 315 | commander = cf.high_level_commander # type: cflib.HighLevelCOmmander 316 | commander.land(0.0, 3.0) 317 | print('Landing...') 318 | time.sleep(3) 319 | # disable led to save battery 320 | cf.param.set_value('ring.effect', '0') 321 | commander.stop() 322 | except Exception as e: 323 | print(e) 324 | land_sequence(scf) 325 | 326 | def run(args): 327 | 328 | # logging.basicConfig(level=logging.DEBUG) 329 | cflib.crtp.init_drivers(enable_debug_driver=False) 330 | 331 | factory = CachedCfFactory(rw_cache='./cache') 332 | uris = {trajectory_assignment[key] for key in trajectory_assignment.keys()} 333 | with open(args.json, 'r') as f: 334 | data = json.load(f) 335 | swarm_args = {trajectory_assignment[drone_pos]: [data[str(drone_pos)]] 336 | for drone_pos in trajectory_assignment.keys()} 337 | 338 | with Swarm(uris, factory=factory) as swarm: 339 | 340 | def signal_handler(sig, frame): 341 | print('You pressed Ctrl+C!') 342 | swarm.parallel(land_sequence) 343 | sys.exit(0) 344 | 345 | signal.signal(signal.SIGINT, signal_handler) 346 | print('Press Ctrl+C to land.') 347 | 348 | print('Preflight sequence...') 349 | swarm.parallel_safe(preflight_sequence) 350 | 351 | #print('Takeoff sequence...') 352 | swarm.parallel_safe(takeoff_sequence) 353 | 354 | print('Upload sequence...') 355 | trajectory_count = 0 356 | 357 | repeat = int(data['repeat']) 358 | 359 | for trajectory_count in range(repeat): 360 | if trajectory_count == 0: 361 | print('Uploading Trajectory') 362 | swarm.parallel(upload_trajectory, args_dict=swarm_args) 363 | 364 | print('Go...') 365 | swarm.parallel_safe(go_sequence, args_dict=swarm_args) 366 | 367 | print('Land sequence...') 368 | swarm.parallel(land_sequence) 369 | 370 | 371 | if __name__ == "__main__": 372 | parser = argparse.ArgumentParser() 373 | parser.add_argument('json') 374 | args = parser.parse_args() 375 | run(args) 376 | -------------------------------------------------------------------------------- /scripts/flyMeToTheMoon.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | sys.path.insert(0, os.curdir) 5 | 6 | from mpl_toolkits import mplot3d 7 | import matplotlib.pyplot as plt 8 | from matplotlib.animation import FuncAnimation 9 | 10 | import numpy as np 11 | import bswarm.trajectory as tgen 12 | import bswarm.formation as formation 13 | import bswarm 14 | import json 15 | 16 | def plot_formation(F, name): 17 | plt.figure() 18 | ax = plt.axes(projection='3d') 19 | ax.plot3D(F[0, :], F[1, :], F[2, :], 'ro') 20 | plt.title(name) 21 | ax.set_xlabel('x, m') 22 | ax.set_ylabel('y, m') 23 | ax.set_zlabel('z, m') 24 | plt.show() 25 | 26 | def scale_formation(form, scale): 27 | formNew = np.copy(form) 28 | for i in range(3): 29 | formNew[i, :] *= scale[i] 30 | return formNew 31 | 32 | #%% parameters 33 | 34 | # defines how the drones are ordered for circles etc. 35 | rotation_order = [4, 3, 2, 5, 0, 1] 36 | 37 | # scaling for formation 38 | form_scale = np.array([1.8, 1.8, 1]) 39 | 40 | #%% takeoff 41 | formTakeoff = np.array([ 42 | [-0.5, -1, 0], 43 | [-0.5, 0, 0], 44 | [-0.5, 1, 0], 45 | [0.5, -1, 0], 46 | [0.5, 0, 0], 47 | [0.5, 1, 0], 48 | ]).T 49 | n_drones = formTakeoff.shape[1] 50 | plot_formation(formTakeoff, 'takeoff') 51 | 52 | #%% circle 53 | points = [] 54 | for i_drone in rotation_order: 55 | theta = i_drone*2*np.pi/n_drones 56 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 57 | formCircle = scale_formation(np.array(points).T, form_scale) 58 | formCircle[2, 0] = 0.8 59 | formCircle[2, 1] = 1 60 | formCircle[2, 2] = 0.8 61 | formCircle[2, 3] = 0.2 62 | formCircle[2, 4] = 0 63 | formCircle[2, 5] = 0.2 64 | 65 | plot_formation(formCircle, 'circle') 66 | 67 | #%% heart # 68 | formHeart = np.array([ 69 | [-1, -0.7, 1], 70 | [-0.1, 0, 0.6], 71 | [-1, 0.7, 1], 72 | [0, -1.25, 0.5], 73 | [1.25, 0, 0], 74 | [0, 1.25, 0.5], 75 | ]).T 76 | plot_formation(formHeart, 'heart') 77 | 78 | #%% triangle 79 | points = [] 80 | for i_drone in rotation_order: 81 | theta = i_drone*2*np.pi/n_drones 82 | if i_drone % 2 == 0: 83 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), -0.5]) 84 | else: 85 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0.5]) 86 | formTriangle = scale_formation(np.array(points).T, form_scale) 87 | plot_formation(formTriangle, 'triangle') 88 | 89 | 90 | #%% 91 | 92 | class Geometry: 93 | 94 | rgb = { 95 | 'black': [0, 0, 0], 96 | 'gold': [255, 100, 15], 97 | 'red': [255, 0, 0], 98 | 'green': [0, 255, 0], 99 | 'blue': [0, 0, 255], 100 | 'white': [255, 255, 255] 101 | } 102 | 103 | def __init__(self): 104 | self.waypoints = [formTakeoff] 105 | self.T = [] 106 | self.delays = [] 107 | self.colors = [] 108 | 109 | @staticmethod 110 | def sin_z(form, t): 111 | new_form = np.array(form) 112 | n_drones = form.shape[1] 113 | for i in rotation_order: 114 | new_form[2, i] = 0.5*np.sin(t + i*2*np.pi/n_drones) 115 | return new_form 116 | 117 | def sin_wave(self, form, n, duration, color): 118 | for t in np.linspace(0, duration, n): 119 | formSin = self.sin_z(form, t*np.pi/4) 120 | self.waypoints.append(formSin) 121 | self.T.append(duration/n) 122 | self.colors.append(self.rgb[color]) 123 | n_drones = form.shape[1] 124 | self.delays.append(np.zeros(n_drones).tolist()) 125 | 126 | def spiral(self, form, z, n, duration, color): 127 | for t in np.linspace(0, 1, n): 128 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 129 | shift = np.array([[0, 0, z*np.sin(t)]]).T 130 | self.waypoints.append(rot_form + shift) 131 | self.T.append(duration/n) 132 | self.colors.append(self.rgb[color]) 133 | n_drones = form.shape[1] 134 | self.delays.append(np.zeros(n_drones).tolist()) 135 | 136 | def rotate(self, form, n, duration, color): 137 | for t in np.linspace(0, 1, n): 138 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 139 | self.waypoints.append(rot_form) 140 | self.T.append(duration/n) 141 | self.colors.append(self.rgb[color]) 142 | n_drones = form.shape[1] 143 | self.delays.append(np.zeros(n_drones).tolist()) 144 | 145 | def goto(self, form, duration, color): 146 | self.waypoints.append(form) 147 | self.T.append(duration) 148 | self.colors.append(self.rgb[color]) 149 | n_drones = form.shape[1] 150 | self.delays.append(np.zeros(n_drones).tolist()) 151 | 152 | def plan_trajectory(self): 153 | trajectories = [] 154 | origin = np.array([1.5, 2, 2]) 155 | waypoints = np.array(self.waypoints) 156 | for drone in range(waypoints.shape[2]): 157 | pos_wp = waypoints[:, :, drone] + origin 158 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 159 | traj = tgen.min_deriv_4d(4, 160 | np.hstack([pos_wp, yaw_wp]), self.T, stop=False) 161 | trajectories.append(traj) 162 | 163 | traj_json = tgen.trajectories_to_json(trajectories) 164 | data = {} 165 | for key in traj_json.keys(): 166 | data[key] = { 167 | 'trajectory': traj_json[key], 168 | 'T': self.T, 169 | 'color': g.colors, 170 | 'delay': [d[key] for d in g.delays] 171 | } 172 | data['repeat'] = 2 173 | assert len(trajectories) < 32 174 | assert len(self.T) < 32 175 | return trajectories, data 176 | 177 | # create trajectory waypoints 178 | g = Geometry() 179 | #ooga chaka 180 | g.goto(form=formTakeoff, duration=4, color='blue') 181 | g.goto(form=formCircle, duration=4, color='black') 182 | # moon 8 183 | g.goto(form=formCircle, duration=3, color='white') 184 | g.goto(form=formTriangle, duration=3, color='black') 185 | # stars 14 186 | g.goto(form=formTriangle, duration=2, color='white') 187 | g.rotate(form=formTriangle, n=5, duration=12, color='white') 188 | # hand 28 189 | g.goto(form=formHeart, duration=2, color='black') # change to heart 190 | g.rotate(form=formHeart, n=6, duration=9, color='red') # change to heart 191 | g.goto(form=formTakeoff, duration=2, color='black') 192 | # song 41 193 | g.sin_wave(form=formTakeoff, n=4, duration=8, color='blue') 194 | g.goto(form=formTakeoff, duration=2, color='black') 195 | # adore 54 196 | g.goto(form=formHeart, duration=2, color='black') # change to heart 197 | g.rotate(form=formHeart, n=6, duration=10, color='red') # change to heart 198 | g.goto(form=formTakeoff, duration=2, color='black') 199 | 200 | #%% plan trajectories 201 | trajectories, data = g.plan_trajectory() 202 | 203 | with open('scripts/data/flyMe.json', 'w') as f: 204 | json.dump(data, f) 205 | 206 | tgen.plot_trajectories(trajectories) 207 | tgen.animate_trajectories('flyMe.mp4', trajectories, 1) 208 | 209 | #%% 210 | plt.figure() 211 | tgen.plot_trajectories_time_history(trajectories) 212 | plt.show() 213 | 214 | #%% 215 | plt.figure() 216 | tgen.plot_trajectories_magnitudes(trajectories) 217 | plt.show() 218 | 219 | #%% 220 | print('number of segments', len(trajectories[0].coef_array())) 221 | #%% 222 | plt.figure() 223 | plt.title('durations') 224 | plt.bar(range(len(g.T)), g.T) 225 | plt.show() -------------------------------------------------------------------------------- /scripts/geometry_3drone.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | sys.path.insert(0, os.getcwd()) 5 | 6 | from mpl_toolkits import mplot3d 7 | import matplotlib.pyplot as plt 8 | from matplotlib.animation import FuncAnimation 9 | 10 | import numpy as np 11 | import bswarm.trajectory as tgen 12 | import bswarm.formation as formation 13 | import bswarm 14 | import json 15 | 16 | def plot_formation(F, name): 17 | plt.figure() 18 | ax = plt.axes(projection='3d') 19 | ax.plot3D(F[0, :], F[1, :], F[2, :], 'ro') 20 | plt.title(name) 21 | ax.set_xlabel('x, m') 22 | ax.set_ylabel('y, m') 23 | ax.set_zlabel('z, m') 24 | plt.show() 25 | 26 | def scale_formation(form, scale): 27 | formNew = np.copy(form) 28 | for i in range(3): 29 | formNew[i, :] *= scale[i] 30 | return formNew 31 | 32 | #%% parameters 33 | 34 | # defines how the drones are ordered for circles etc. 35 | # rotation_order = [4, 3, 2, 5, 0, 1] 36 | rotation_order = [0, 2, 1] 37 | 38 | # scaling for formation 39 | form_scale = np.array([1.5, 1.5, 1]) 40 | 41 | # offset = np.array([ 42 | # [0.156,-3.343,0] 43 | # ]) 44 | 45 | # the takeoff formation 46 | formTakeoff = np.array([ 47 | [0.5, -1, 0], 48 | [0.5, 0, 0], 49 | [0.5, 1, 0], 50 | ]).T 51 | n_drones = formTakeoff.shape[1] 52 | 53 | plot_formation(formTakeoff, 'takeoff') 54 | points = [] 55 | for i_drone in rotation_order: 56 | theta = i_drone*2*np.pi/n_drones 57 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 58 | formCircle = scale_formation(np.array(points).T, form_scale) 59 | plot_formation(formCircle, 'circle') 60 | 61 | points = [] 62 | for i_drone in rotation_order: 63 | theta = i_drone*2*np.pi/n_drones 64 | if i_drone % 2 == 0: 65 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 66 | else: 67 | points.append([0.25*np.cos(theta), 0.25*np.sin(theta), 0]) 68 | formTriangle = scale_formation(np.array(points).T, form_scale) 69 | plot_formation(formTriangle, 'triangle') 70 | 71 | 72 | class Geometry: 73 | 74 | rgb = { 75 | 'black': [0, 0, 0], 76 | 'gold': [255, 100, 15], 77 | 'red': [255, 0, 0], 78 | 'green': [0, 255, 0], 79 | 'blue': [0, 0, 255], 80 | 'white': [255, 255, 255] 81 | } 82 | 83 | def __init__(self): 84 | self.waypoints = [formTakeoff] 85 | self.T = [] 86 | self.delays = [] 87 | self.colors = [] 88 | 89 | @staticmethod 90 | def sin_z(form, t): 91 | new_form = np.array(form) 92 | n_drones = form.shape[1] 93 | for i in rotation_order: 94 | new_form[2, i] = 0.5*np.sin(t + i*2*np.pi/n_drones) 95 | return new_form 96 | 97 | def sin_wave(self, form, n, duration, color): 98 | for t in np.linspace(0, duration, n): 99 | formSin = self.sin_z(form, t*np.pi/4) 100 | self.waypoints.append(formSin) 101 | self.T.append(duration/n) 102 | self.colors.append(self.rgb[color]) 103 | n_drones = form.shape[1] 104 | self.delays.append(np.zeros(n_drones).tolist()) 105 | 106 | def spiral(self, form, z, n, duration, color): 107 | for t in np.linspace(0, 1, n): 108 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 109 | shift = np.array([[0, 0, z*np.sin(t)]]).T 110 | self.waypoints.append(rot_form + shift) 111 | self.T.append(duration/n) 112 | self.colors.append(self.rgb[color]) 113 | n_drones = form.shape[1] 114 | self.delays.append(np.zeros(n_drones).tolist()) 115 | 116 | def rotate(self, form, n, duration, color): 117 | for t in np.linspace(0, 1, n): 118 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 119 | self.waypoints.append(rot_form) 120 | self.T.append(duration/n) 121 | self.colors.append(self.rgb[color]) 122 | n_drones = form.shape[1] 123 | self.delays.append(np.zeros(n_drones).tolist()) 124 | 125 | def goto(self, form, duration, color): 126 | self.waypoints.append(form) 127 | self.T.append(duration) 128 | self.colors.append(self.rgb[color]) 129 | n_drones = form.shape[1] 130 | self.delays.append(np.zeros(n_drones).tolist()) 131 | 132 | def plan_trajectory(self): 133 | trajectories = [] 134 | origin = np.array([0, 0, 2]) 135 | self.waypoints = np.array(self.waypoints) 136 | for drone in range(self.waypoints.shape[2]): 137 | pos_wp = self.waypoints[:, :, drone] + origin 138 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 139 | traj = tgen.min_deriv_4d(4, 140 | np.hstack([pos_wp, yaw_wp]), self. 141 | T, stop=False) 142 | trajectories.append(traj) 143 | 144 | traj_json = tgen.trajectories_to_json(trajectories) 145 | data = {} 146 | for key in traj_json.keys(): 147 | data[key] = { 148 | 'trajectory': traj_json[key], 149 | 'T': self.T, 150 | 'color': g.colors, 151 | 'delay': [d[key] for d in g.delays] 152 | } 153 | data['repeat'] = 3 154 | assert len(trajectories) < 32 155 | return trajectories, data 156 | 157 | # create trajectory waypoints 158 | g = Geometry() 159 | g.sin_wave(form=formTakeoff, n=8, duration=16, color='red') 160 | g.goto(form=formCircle, duration=2, color='blue') 161 | g.rotate(form=formCircle, n=6, duration=12, color='green') 162 | # g.rotate(form=formTriangle, n=6, duration=12, color='white') 163 | g.goto(form=formCircle, duration=2, color='gold') 164 | g.spiral(form=formCircle, z=1, n=6, duration=12, color='red') 165 | g.goto(formTakeoff, 2, color='blue') 166 | 167 | 168 | 169 | #%% plan trajectories 170 | trajectories, data = g.plan_trajectory() 171 | 172 | with open('scripts/data/geometry_3drone.json', 'w') as f: 173 | json.dump(data, f) 174 | 175 | tgen.plot_trajectories(trajectories) 176 | # tgen.animate_trajectories('geometry.mp4', trajectories, 1) 177 | 178 | #%% 179 | plt.figure() 180 | tgen.plot_trajectories_time_history(trajectories) 181 | plt.show() 182 | 183 | #%% 184 | plt.figure() 185 | tgen.plot_trajectories_magnitudes(trajectories) 186 | plt.show() 187 | 188 | #%% 189 | print('number of segments', len(trajectories[0].coef_array())) 190 | #%% 191 | plt.figure() 192 | plt.title('durations') 193 | plt.bar(range(len(g.T)), g.T) 194 | plt.show() 195 | 196 | 197 | #%% 198 | -------------------------------------------------------------------------------- /scripts/geometry_6drone.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | sys.path.insert(0, os.curdir) 5 | sys.path.insert(0, os.pardir) 6 | 7 | from mpl_toolkits import mplot3d 8 | import matplotlib.pyplot as plt 9 | from matplotlib.animation import FuncAnimation 10 | 11 | import numpy as np 12 | import bswarm.trajectory as tgen 13 | import bswarm.formation as formation 14 | import bswarm 15 | import json 16 | 17 | def plot_formation(F, name): 18 | plt.figure() 19 | ax = plt.axes(projection='3d') 20 | ax.plot3D(F[0, :], F[1, :], F[2, :], 'ro') 21 | plt.title(name) 22 | ax.set_xlabel('x, m') 23 | ax.set_ylabel('y, m') 24 | ax.set_zlabel('z, m') 25 | plt.show() 26 | 27 | def scale_formation(form, scale): 28 | formNew = np.copy(form) 29 | for i in range(3): 30 | formNew[i, :] *= scale[i] 31 | return formNew 32 | 33 | #%% parameters 34 | 35 | # defines how the drones are ordered for circles etc. 36 | rotation_order = [4, 3, 2, 5, 0, 1] 37 | 38 | # scaling for formation 39 | form_scale = np.array([1.5, 1.5, 1]) 40 | 41 | # offset = np.array([ 42 | # [0.156,-3.343,0] 43 | # ]) 44 | 45 | # the takeoff formation 46 | formTakeoff = np.array([ 47 | [-0.5, -1, 0], 48 | [-0.5, 0, 0], 49 | [-0.5, 1, 0], 50 | [0.5, -1, 0], 51 | [0.5, 0, 0], 52 | [0.5, 1, 0], 53 | ]).T 54 | n_drones = formTakeoff.shape[1] 55 | 56 | plot_formation(formTakeoff, 'takeoff') 57 | points = [] 58 | for i_drone in rotation_order: 59 | theta = i_drone*2*np.pi/n_drones 60 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 61 | formCircle = scale_formation(np.array(points).T, form_scale) 62 | plot_formation(formCircle, 'circle') 63 | 64 | points = [] 65 | for i_drone in rotation_order: 66 | theta = i_drone*2*np.pi/n_drones 67 | if i_drone % 2 == 0: 68 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 69 | else: 70 | points.append([0.25*np.cos(theta), 0.25*np.sin(theta), 0]) 71 | formTriangle = scale_formation(np.array(points).T, form_scale) 72 | plot_formation(formTriangle, 'triangle') 73 | 74 | 75 | class Geometry: 76 | 77 | rgb = { 78 | 'black': [0, 0, 0], 79 | 'gold': [255, 100, 15], 80 | 'red': [255, 0, 0], 81 | 'green': [0, 255, 0], 82 | 'blue': [0, 0, 255], 83 | 'white': [255, 255, 255] 84 | } 85 | 86 | def __init__(self): 87 | self.waypoints = [formTakeoff] 88 | self.T = [] 89 | self.delays = [] 90 | self.colors = [] 91 | 92 | @staticmethod 93 | def sin_z(form, t): 94 | new_form = np.array(form) 95 | n_drones = form.shape[1] 96 | for i in rotation_order: 97 | new_form[2, i] = 0.5*np.sin(t + i*2*np.pi/n_drones) 98 | return new_form 99 | 100 | def sin_wave(self, form, n, duration, color): 101 | for t in np.linspace(0, duration, n): 102 | formSin = self.sin_z(form, t*np.pi/4) 103 | self.waypoints.append(formSin) 104 | self.T.append(duration/n) 105 | self.colors.append(self.rgb[color]) 106 | n_drones = form.shape[1] 107 | self.delays.append(np.zeros(n_drones).tolist()) 108 | 109 | def spiral(self, form, z, n, duration, color): 110 | for t in np.linspace(0, 1, n): 111 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 112 | shift = np.array([[0, 0, z*np.sin(t)]]).T 113 | self.waypoints.append(rot_form + shift) 114 | self.T.append(duration/n) 115 | self.colors.append(self.rgb[color]) 116 | n_drones = form.shape[1] 117 | self.delays.append(np.zeros(n_drones).tolist()) 118 | 119 | def rotate(self, form, n, duration, color): 120 | for t in np.linspace(0, 1, n): 121 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 122 | self.waypoints.append(rot_form) 123 | self.T.append(duration/n) 124 | self.colors.append(self.rgb[color]) 125 | n_drones = form.shape[1] 126 | self.delays.append(np.zeros(n_drones).tolist()) 127 | 128 | def goto(self, form, duration, color): 129 | self.waypoints.append(form) 130 | self.T.append(duration) 131 | self.colors.append(self.rgb[color]) 132 | n_drones = form.shape[1] 133 | self.delays.append(np.zeros(n_drones).tolist()) 134 | 135 | def plan_trajectory(self): 136 | trajectories = [] 137 | origin = np.array([0, 0, 2]) 138 | self.waypoints = np.array(self.waypoints) 139 | for drone in range(self.waypoints.shape[2]): 140 | pos_wp = self.waypoints[:, :, drone] + origin 141 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 142 | traj = tgen.min_deriv_4d(4, 143 | np.hstack([pos_wp, yaw_wp]), self. 144 | T, stop=False) 145 | trajectories.append(traj) 146 | 147 | traj_json = tgen.trajectories_to_json(trajectories) 148 | data = {} 149 | for key in traj_json.keys(): 150 | data[key] = { 151 | 'trajectory': traj_json[key], 152 | 'T': self.T, 153 | 'color': g.colors, 154 | 'delay': [d[key] for d in g.delays] 155 | } 156 | data['repeat'] = 3 157 | assert len(trajectories) < 32 158 | return trajectories, data 159 | 160 | # create trajectory waypoints 161 | g = Geometry() 162 | g.sin_wave(form=formTakeoff, n=8, duration=16, color='red') 163 | g.goto(form=formCircle, duration=2, color='blue') 164 | g.rotate(form=formCircle, n=6, duration=12, color='green') 165 | g.rotate(form=formTriangle, n=6, duration=12, color='red') 166 | g.goto(form=formCircle, duration=2, color='blue') 167 | g.spiral(form=formCircle, z=1, n=6, duration=12, color='green') 168 | g.goto(formTakeoff, 2, color='blue') 169 | 170 | 171 | 172 | #%% plan trajectories 173 | trajectories, data = g.plan_trajectory() 174 | 175 | with open('data/geometry.json', 'w') as f: 176 | json.dump(data, f) 177 | 178 | tgen.plot_trajectories(trajectories) 179 | tgen.animate_trajectories('geometry.mp4', trajectories, 1) 180 | 181 | #%% 182 | plt.figure() 183 | tgen.plot_trajectories_time_history(trajectories) 184 | plt.show() 185 | 186 | #%% 187 | plt.figure() 188 | tgen.plot_trajectories_magnitudes(trajectories) 189 | plt.show() 190 | 191 | #%% 192 | print('number of segments', len(trajectories[0].coef_array())) 193 | #%% 194 | plt.figure() 195 | plt.title('durations') 196 | plt.bar(range(len(g.T)), g.T) 197 | plt.show() -------------------------------------------------------------------------------- /scripts/geometry_9drone.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | from typing import OrderedDict 5 | 6 | from numpy.core.defchararray import equal 7 | sys.path.insert(0, os.getcwd()) 8 | if 'pybswarm/scripts' in os.getcwd(): 9 | os.chdir('..') 10 | 11 | from mpl_toolkits import mplot3d 12 | import matplotlib.pyplot as plt 13 | from matplotlib.animation import FuncAnimation 14 | 15 | import numpy as np 16 | import bswarm.trajectory as tgen 17 | import bswarm.formation as formation 18 | import bswarm 19 | import json 20 | 21 | def plot_formation(F, name): 22 | plt.figure() 23 | ax = plt.axes(projection='3d') 24 | ax.plot3D(F[0, :], F[1, :], F[2, :], 'ro') 25 | plt.title(name) 26 | ax.set_xlabel('x, m') 27 | ax.set_ylabel('y, m') 28 | ax.set_zlabel('z, m') 29 | plt.show() 30 | 31 | def scale_formation(form, scale): 32 | formNew = np.copy(form) 33 | for i in range(3): 34 | formNew[i, :] *= scale[i] 35 | return formNew 36 | 37 | #%% parameters 38 | 39 | # defines how the drones are ordered for circles etc. 40 | circle_order = [5, 4, 3, 6, 8, 2, 7, 0, 1] 41 | 42 | # scaling for formation 43 | form_scale = np.array([1.5, 1.5, 1]) 44 | 45 | # the takeoff formation 46 | formTakeoff = np.array([ 47 | [-1, -1, 0], 48 | [-1, 0, 0], 49 | [-1, 1, 0], 50 | [0, -1, 0], 51 | [0, 0, 0], 52 | [0, 1, 0], 53 | [1, -1, 0], 54 | [1, 0, 0], 55 | [1, 1, 0], 56 | ]).T 57 | n_drones = formTakeoff.shape[1] 58 | 59 | plot_formation(formTakeoff, 'takeoff') 60 | points = [] 61 | radius = 0.75 62 | for i, order in enumerate(circle_order): 63 | if i == 4: 64 | points.append([0, 0, 0.5]) 65 | else: 66 | theta = (order)*2*np.pi/(n_drones - 1) 67 | points.append([radius*np.cos(theta), radius*np.sin(theta), 0]) 68 | 69 | formCone = scale_formation(np.array(points).T, form_scale) 70 | plot_formation(formCone, 'cone') 71 | # %% 72 | points = [] 73 | radius = 1 74 | 75 | for i, order in enumerate(circle_order): 76 | theta = order*2*np.pi/(n_drones) 77 | points.append([radius*np.cos(theta), radius*np.sin(theta), 0]) 78 | formCircle = scale_formation(np.array(points).T, form_scale) 79 | plot_formation(formCircle, 'circle') 80 | 81 | formCircleVert = formation.rotate_points_x(formCircle, np.pi/2) 82 | plot_formation(formCircleVert, 'circle_vert') 83 | # %% 84 | points = [] 85 | triangle_oder = [5, 2, 8, 1, 4, 0, 3, 6, 7] 86 | for i_drone in triangle_oder: 87 | theta = i_drone*2*np.pi/n_drones 88 | if i_drone % 2 == 0: 89 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 90 | else: 91 | points.append([0.25*np.cos(theta), 0.25*np.sin(theta), 0]) 92 | formTriangle = scale_formation(np.array(points).T, form_scale) 93 | plot_formation(formTriangle, 'triangle') 94 | 95 | #%% 96 | class Geometry: 97 | 98 | rgb = { 99 | 'black': [0, 0, 0], 100 | 'gold': [255, 100, 15], 101 | 'red': [255, 0, 0], 102 | 'green': [0, 255, 0], 103 | 'blue': [0, 0, 255], 104 | 'white': [255, 255, 255] 105 | } 106 | 107 | def __init__(self): 108 | self.waypoints = [formTakeoff] 109 | self.T = [] 110 | self.delays = [] 111 | self.colors = [] 112 | 113 | @staticmethod 114 | def sin_z(form, t): 115 | new_form = np.array(form) 116 | n_drones = form.shape[1] 117 | rotation_order = circle_order 118 | for i in rotation_order: 119 | new_form[2, i] = 0.5*np.sin(t + i*2*np.pi/n_drones) 120 | return new_form 121 | 122 | def sin_wave(self, form, n, duration, color): 123 | for t in np.linspace(0, duration, n): 124 | formSin = self.sin_z(form, t*np.pi/4) 125 | self.waypoints.append(formSin) 126 | self.T.append(duration/n) 127 | self.colors.append(self.rgb[color]) 128 | n_drones = form.shape[1] 129 | self.delays.append(np.zeros(n_drones).tolist()) 130 | 131 | def spiral(self, form, z, n, duration, color): 132 | for t in np.linspace(0, 1, n): 133 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 134 | shift = np.array([[0, 0, z*np.sin(t)]]).T 135 | self.waypoints.append(rot_form + shift) 136 | self.T.append(duration/n) 137 | self.colors.append(self.rgb[color]) 138 | n_drones = form.shape[1] 139 | self.delays.append(np.zeros(n_drones).tolist()) 140 | 141 | def rotate_z(self, form, n, duration, color): 142 | for t in np.linspace(0, 1, n): 143 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 144 | self.waypoints.append(rot_form) 145 | self.T.append(duration/n) 146 | self.colors.append(self.rgb[color]) 147 | n_drones = form.shape[1] 148 | self.delays.append(np.zeros(n_drones).tolist()) 149 | 150 | def rotate_x(self, form, n, duration, color): 151 | for t in np.linspace(0, 1, n): 152 | rot_form = formation.rotate_points_x(form, t*2*np.pi) 153 | self.waypoints.append(rot_form) 154 | self.T.append(duration/n) 155 | self.colors.append(self.rgb[color]) 156 | n_drones = form.shape[1] 157 | self.delays.append(np.zeros(n_drones).tolist()) 158 | 159 | def goto(self, form, duration, color): 160 | self.waypoints.append(form) 161 | self.T.append(duration) 162 | self.colors.append(self.rgb[color]) 163 | n_drones = form.shape[1] 164 | self.delays.append(np.zeros(n_drones).tolist()) 165 | 166 | def plan_trajectory(self): 167 | trajectories = [] 168 | origin = np.array([0, 0, 2]) 169 | self.waypoints = np.array(self.waypoints) 170 | for drone in range(self.waypoints.shape[2]): 171 | pos_wp = self.waypoints[:, :, drone] + origin 172 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 173 | traj = tgen.min_deriv_4d(4, 174 | np.hstack([pos_wp, yaw_wp]), self. 175 | T, stop=False) 176 | trajectories.append(traj) 177 | 178 | traj_json = tgen.trajectories_to_json(trajectories) 179 | data = {} 180 | for key in traj_json.keys(): 181 | data[key] = { 182 | 'trajectory': traj_json[key], 183 | 'T': self.T, 184 | 'color': g.colors, 185 | 'delay': [d[key] for d in g.delays] 186 | } 187 | data['repeat'] = 3 188 | assert len(trajectories) < 32 189 | return trajectories, data 190 | # %% 191 | # create trajectory waypoints 192 | g = Geometry() 193 | # g.sin_wave(form=formTakeoff, n=8, duration=16, color='red') 194 | g.goto(form=formCone, duration=2, color='blue') 195 | g.rotate_z(form=formCone, n=6, duration=12, color='green') 196 | g.goto(form=formTakeoff, duration=2, color='red') 197 | g.goto(form=formCircle, duration=2, color='blue') 198 | # g.rotate_x(form=formCircle, n=4, duration=5, color='blue') 199 | # g.rotate(form=formTriangle, n=6, duration=12, color='red') 200 | # g.goto(form=formCircle, duration=2, color='blue') 201 | # g.spiral(form=formCircle, z=1, n=6, duration=12, color='green') 202 | # g.goto(formTakeoff, 2, color='blue') 203 | 204 | 205 | 206 | #%% plan trajectories 207 | trajectories, data = g.plan_trajectory() 208 | 209 | with open('scripts/data/geometry_9drone.json', 'w') as f: 210 | json.dump(data, f) 211 | 212 | tgen.plot_trajectories(trajectories) 213 | tgen.animate_trajectories('geometry.mp4', trajectories, 1) 214 | 215 | #%% 216 | plt.figure() 217 | tgen.plot_trajectories_time_history(trajectories) 218 | plt.show() 219 | 220 | #%% 221 | plt.figure() 222 | tgen.plot_trajectories_magnitudes(trajectories) 223 | plt.show() 224 | 225 | #%% 226 | print('number of segments', len(trajectories[0].coef_array())) 227 | #%% 228 | plt.figure() 229 | plt.title('durations') 230 | plt.bar(range(len(g.T)), g.T) 231 | plt.show() 232 | 233 | 234 | #%% 235 | -------------------------------------------------------------------------------- /scripts/hookedonafeeling.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | sys.path.insert(0, os.getcwd()) 5 | 6 | from mpl_toolkits import mplot3d 7 | import matplotlib.pyplot as plt 8 | from matplotlib.animation import FuncAnimation 9 | 10 | import numpy as np 11 | import bswarm.trajectory as tgen 12 | import bswarm.formation as formation 13 | import bswarm 14 | import json 15 | 16 | def plot_formation(F, name): 17 | plt.figure() 18 | ax = plt.axes(projection='3d') 19 | ax.plot3D(F[0, :], F[1, :], F[2, :], 'ro') 20 | plt.title(name) 21 | ax.set_xlabel('x, m') 22 | ax.set_ylabel('y, m') 23 | ax.set_zlabel('z, m') 24 | plt.show() 25 | 26 | def scale_formation(form, scale): 27 | formNew = np.copy(form) 28 | for i in range(3): 29 | formNew[i, :] *= scale[i] 30 | return formNew 31 | 32 | #%% parameters 33 | 34 | # defines how the drones are ordered for circles etc. 35 | rotation_order = [4, 3, 2, 5, 0, 1] 36 | 37 | # scaling for formation 38 | form_scale = np.array([1.8, 1.8, 1]) 39 | 40 | # the takeoff formation 41 | formTakeoff = np.array([ 42 | [-0.5, -1, 0], 43 | [-0.5, 0, 0], 44 | [-0.5, 1, 0], 45 | [0.5, -1, 0], 46 | [0.5, 0, 0], 47 | [0.5, 1, 0], 48 | ]).T 49 | n_drones = formTakeoff.shape[1] 50 | #%% 51 | 52 | plot_formation(formTakeoff, 'takeoff') 53 | points = [] 54 | for i_drone in rotation_order: 55 | theta = i_drone*2*np.pi/n_drones 56 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0]) 57 | formCircle = scale_formation(np.array(points).T, form_scale) 58 | plot_formation(formCircle, 'circle') 59 | 60 | #%% 61 | points = [] 62 | for i_drone in rotation_order: 63 | theta = i_drone*2*np.pi/n_drones 64 | if i_drone % 2 == 0: 65 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), -0.5]) 66 | else: 67 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0.5]) 68 | formTriangle = scale_formation(np.array(points).T, form_scale) 69 | plot_formation(formTriangle, 'triangle') 70 | 71 | 72 | #%% 73 | points = [] 74 | for i_drone in rotation_order: 75 | theta = i_drone*2*np.pi/n_drones 76 | if i_drone % 2 == 0: 77 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), -0.5]) 78 | else: 79 | points.append([0.5*np.cos(theta), 0.5*np.sin(theta), 0.5]) 80 | formTriangle = scale_formation(np.array(points).T, form_scale) 81 | plot_formation(formTriangle, 'triangle') 82 | 83 | 84 | #%% 85 | 86 | class Geometry: 87 | 88 | rgb = { 89 | 'black': [0, 0, 0], 90 | 'gold': [255, 100, 15], 91 | 'red': [255, 0, 0], 92 | 'green': [0, 255, 0], 93 | 'blue': [0, 0, 255], 94 | 'white': [255, 255, 255] 95 | } 96 | 97 | def __init__(self): 98 | self.waypoints = [formTakeoff] 99 | self.T = [] 100 | self.delays = [] 101 | self.colors = [] 102 | 103 | @staticmethod 104 | def sin_z(form, t): 105 | new_form = np.array(form) 106 | n_drones = form.shape[1] 107 | for i in rotation_order: 108 | new_form[2, i] = 0.5*np.sin(t + i*2*np.pi/n_drones) 109 | return new_form 110 | 111 | def sin_wave(self, form, n, duration, color): 112 | for t in np.linspace(0, duration, n): 113 | formSin = self.sin_z(form, t*np.pi/4) 114 | self.waypoints.append(formSin) 115 | self.T.append(duration/n) 116 | self.colors.append(self.rgb[color]) 117 | n_drones = form.shape[1] 118 | self.delays.append(np.zeros(n_drones).tolist()) 119 | 120 | def spiral(self, form, z, n, duration, color): 121 | for t in np.linspace(0, 1, n): 122 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 123 | shift = np.array([[0, 0, z*np.sin(t)]]).T 124 | self.waypoints.append(rot_form + shift) 125 | self.T.append(duration/n) 126 | self.colors.append(self.rgb[color]) 127 | n_drones = form.shape[1] 128 | self.delays.append(np.zeros(n_drones).tolist()) 129 | 130 | def rotate(self, form, n, duration, color): 131 | for t in np.linspace(0, 1, n): 132 | rot_form = formation.rotate_points_z(form, t*2*np.pi) 133 | self.waypoints.append(rot_form) 134 | self.T.append(duration/n) 135 | self.colors.append(self.rgb[color]) 136 | n_drones = form.shape[1] 137 | self.delays.append(np.zeros(n_drones).tolist()) 138 | 139 | def goto(self, form, duration, color): 140 | self.waypoints.append(form) 141 | self.T.append(duration) 142 | self.colors.append(self.rgb[color]) 143 | n_drones = form.shape[1] 144 | self.delays.append(np.zeros(n_drones).tolist()) 145 | 146 | def plan_trajectory(self): 147 | trajectories = [] 148 | origin = np.array([1.5, 2, 2]) 149 | waypoints = np.array(self.waypoints) 150 | for drone in range(waypoints.shape[2]): 151 | pos_wp = waypoints[:, :, drone] + origin 152 | yaw_wp = np.zeros((pos_wp.shape[0], 1)) 153 | traj = tgen.min_deriv_4d(4, 154 | np.hstack([pos_wp, yaw_wp]), self.T, stop=False) 155 | trajectories.append(traj) 156 | 157 | traj_json = tgen.trajectories_to_json(trajectories) 158 | data = {} 159 | for key in traj_json.keys(): 160 | data[key] = { 161 | 'trajectory': traj_json[key], 162 | 'T': self.T, 163 | 'color': g.colors, 164 | 'delay': [d[key] for d in g.delays] 165 | } 166 | data['repeat'] = 3 167 | assert len(trajectories) < 32 168 | return trajectories, data 169 | 170 | # create trajectory waypoints 171 | g = Geometry() 172 | #ooga chaka 173 | g.goto(form=formTakeoff, duration=2, color='blue') 174 | g.sin_wave(form=formTakeoff, n=4, duration=7, color='gold') 175 | #IIIIIIIIIIIIIIIIIIIIIIIIIIIIIIIII gotta feeling 176 | g.goto(form=formCircle, duration=2, color='red') 177 | g.spiral(form=formCircle, z=1, n=6, duration=12, color='red') 178 | #less popular part of chorus 179 | g.goto(form=formCircle, duration=2, color='green') 180 | g.rotate(form=formTriangle, n=6, duration=17, color='white') 181 | #before alcohol 182 | g.rotate(form=formCircle, n=6, duration=12, color='gold') 183 | g.goto(form=formTakeoff, duration=2, color='blue') 184 | 185 | 186 | #g.rotate(form=formCircle, n=7, duration=20) 187 | #g.rotate(form=formTriangle, n=4, duration=12) 188 | #g.spiral(form=formCircle, z=1, n=4, duration=10) 189 | #g.goto(form=formCircle, duration=2) 190 | #g.spiral(form=formCircle, z=1, n=7, duration=28) 191 | #g.goto(form=formCircle, duration=2) 192 | #g.rotate(form=formTriangle, n=5, duration=18) 193 | g.goto(formTakeoff, duration=2, color='green') 194 | 195 | ##a 196 | #18 secs for next segment of song 197 | ## 198 | 199 | #%% plan trajectories 200 | trajectories, data = g.plan_trajectory() 201 | 202 | with open('scripts/data/HookedOnAFeeling.json', 'w') as f: 203 | json.dump(data, f) 204 | 205 | tgen.plot_trajectories(trajectories) 206 | tgen.animate_trajectories('hooked.mp4', trajectories, 1) 207 | 208 | #%% 209 | plt.figure() 210 | tgen.plot_trajectories_time_history(trajectories) 211 | plt.show() 212 | 213 | #%% 214 | plt.figure() 215 | tgen.plot_trajectories_magnitudes(trajectories) 216 | plt.show() 217 | 218 | #%% 219 | print('number of segments', len(trajectories[0].coef_array())) 220 | #%% 221 | plt.figure() 222 | plt.title('durations') 223 | plt.bar(range(len(g.T)), g.T) 224 | plt.show() 225 | 226 | 227 | #%% 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | -------------------------------------------------------------------------------- /scripts/set_tx_power.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # || ____ _ __ 4 | # +------+ / __ )(_) /_______________ _____ ___ 5 | # | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 6 | # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 7 | # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 8 | # 9 | # Copyright (C) 2018 Bitcraze AB 10 | # 11 | # This program is free software; you can redistribute it and/or 12 | # modify it under the terms of the GNU General Public License 13 | # as published by the Free Software Foundation; either version 2 14 | # of the License, or (at your option) any later version. 15 | # 16 | # This program is distributed in the hope that it will be useful, 17 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | # GNU General Public License for more details. 20 | # You should have received a copy of the GNU General Public License 21 | # along with this program; if not, write to the Free Software 22 | # Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, 23 | # MA 02110-1301, USA. 24 | """ 25 | Sets the transmit power of the UWB radio chip in LPS Anchors. 26 | 27 | Requires a system in TDoA mode and a Crazyflie with the LPS deck. 28 | The script will broadcast power settings through the Crazyflie over and over 29 | again. 30 | 31 | IMPORTANT: There are regulations on how much energy a radio source is allowed 32 | to transmit, and the regulations are different from country to country. It 33 | might be possible that some configurations that this script can set are not 34 | within limits in some countries. If you choose to run this script and modify 35 | the transmit power, make sure it is within limits where you will use the 36 | anchor. 37 | 38 | 39 | """ 40 | import logging 41 | import time 42 | import struct 43 | import sys 44 | 45 | import cflib.crtp 46 | from cflib.crazyflie import Crazyflie 47 | from cflib.crazyflie.syncCrazyflie import SyncCrazyflie 48 | import argparse 49 | 50 | 51 | def to_power_byte(db): 52 | if db <= 0 or db > 33.5: 53 | raise Exception("Out of range") 54 | 55 | coarse = 0 56 | fine = 0 57 | 58 | while ((6 - coarse) * 3) > db and coarse <= 6: 59 | coarse += 1 60 | 61 | while ((6 - coarse) * 3) + (0.5 * fine) < db: 62 | fine += 1 63 | 64 | return (coarse << 5) + fine 65 | 66 | 67 | def generate_power_conf(power): 68 | power_byte = to_power_byte(power) 69 | return power_byte + 256 * ( 70 | power_byte + 256 * (power_byte + 256 * power_byte)) 71 | 72 | 73 | def generate_lpp_packet(power_conf, force, smart): 74 | flags = 0 75 | 76 | if smart: 77 | flags |= 0x01 78 | if force: 79 | flags |= 0x02 80 | 81 | LPP_UWB = 0x04 82 | return struct.pack(" 255: 87 | raise Exception("Valid anchor ids are 0 - 255") 88 | 89 | 90 | def generate_config(power): 91 | DEFAULT_CONFIG = 0x07274767 92 | 93 | power_conf = DEFAULT_CONFIG 94 | force = False 95 | smart = True 96 | 97 | if power != 'default': 98 | power_conf = generate_power_conf(float(power)) 99 | force = True 100 | smart = False 101 | 102 | return (power_conf, force, smart) 103 | 104 | 105 | def main(): 106 | parser = argparse.ArgumentParser( 107 | description='Set transmit power of LPS Anchors. A new power ' 108 | 'configuration is sent to anchors using a Crazyflie with ' 109 | 'an LPS deck as a bridge. IMPORTANT: There are ' 110 | 'regulations on how much energy a ' 111 | 'radio source is allowed to transmit, and the regulations ' 112 | 'are different from country to country. It might be ' 113 | 'possible that some configurations that this script can ' 114 | 'set are not within limits in some countries. If you ' 115 | 'choose to run this script and modify the transmit power, ' 116 | 'make sure it is within limits where you will use the ' 117 | 'anchor.') 118 | 119 | parser.add_argument('-u', '--uri', 120 | dest='uri', 121 | default='radio://0/80/2M/E7E7E7E7E7', 122 | help='The URI of the Crazyflie (default ' 123 | 'radio://0/80/2M/E7E7E7E7E7)') 124 | 125 | parser.add_argument('-b', '--begin', 126 | dest='anchor_id_lower', 127 | default=0, 128 | type=int, 129 | help='The lower id in the anchor range to send to ' 130 | '(default 0)') 131 | 132 | parser.add_argument('-e', '--end', 133 | dest='anchor_id_upper', 134 | default=7, 135 | type=int, 136 | help='The upper id in the anchor range to send to ' 137 | '(default 7)') 138 | 139 | valid_powers = list(map(lambda x: ('%.1f' % (x / 2)).replace(".0", ""), 140 | range(1, 68))) 141 | valid_powers.append('default') 142 | parser.add_argument('power', 143 | type=str, 144 | choices=valid_powers, 145 | help='The transmit power to use. Valid values are ' 146 | 'between 0.5 and 33.5 (dB) in steps of 0.5 or ' 147 | '"default". "default" will set the power to the ' 148 | 'default power as well as configure the anchor ' 149 | 'to use smart power.') 150 | 151 | args = parser.parse_args() 152 | 153 | unit = 'dB' 154 | if args.power == 'default': 155 | unit = ' (smart)' 156 | 157 | validate_anchor_id(args.anchor_id_lower) 158 | validate_anchor_id(args.anchor_id_upper) 159 | if args.anchor_id_lower > args.anchor_id_upper: 160 | raise Exception("(%i to %i) is not a valid anchor id range" % ( 161 | args.anchor_id_lower, args.anchor_id_upper 162 | )) 163 | 164 | (power_conf, force, smart) = generate_config(args.power) 165 | power_pack = generate_lpp_packet(power_conf, force, smart) 166 | 167 | print('Sending anchor transmit power configuration to anchors (%i to %i) ' 168 | 'using %s. Setting transmit power to %s%s. Anchors will reset' 169 | 'when configured.' % ( 170 | args.anchor_id_lower, 171 | args.anchor_id_upper, 172 | args.uri, 173 | args.power, unit)) 174 | 175 | logging.basicConfig(level=logging.ERROR) 176 | cflib.crtp.init_drivers(enable_debug_driver=False) 177 | cf = Crazyflie(rw_cache='./cache') 178 | with SyncCrazyflie(args.uri, cf=cf) as scf: 179 | print('Starting transmission. Terminate with CTRL+C.') 180 | while True: 181 | for id in range(args.anchor_id_lower, args.anchor_id_upper + 1): 182 | print('Anchor %i' % id) 183 | for _ in range(7): 184 | scf.cf.loc.send_short_lpp_packet(id, power_pack) 185 | time.sleep(0.2) 186 | time.sleep(0.5) 187 | 188 | 189 | main() 190 | -------------------------------------------------------------------------------- /scripts/traj_example.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import sys 3 | import os 4 | sys.path.insert(0, os.getcwd()) 5 | import bswarm.trajectory as tgen 6 | import numpy as np 7 | import matplotlib.pyplot as plt 8 | 9 | waypoints = np.array([ 10 | # x, y, z, yaw 11 | [0, 0, 1, 0], 12 | [0, 1, 1, 0], 13 | [0, 1, 1, 0], 14 | [1, 1, 1, 0], 15 | [1, 0, 1, 0], 16 | [0, 0, 1, 0] 17 | ]) 18 | T = 2*np.ones(len(waypoints) - 1) 19 | traj = tgen.min_snap_4d(waypoints, T, stop=True) 20 | res = traj.compute_inputs() 21 | print(res.max_data()) 22 | 23 | #%% 24 | plt.figure() 25 | tgen.plot_trajectory_3d(traj) 26 | plt.show() 27 | 28 | #%% 29 | plt.figure() 30 | tgen.plot_trajectory_derivatives(traj) 31 | plt.show() 32 | 33 | #%% 34 | check_with_other_library = True 35 | if check_with_other_library: 36 | try: 37 | import bswarm.third_party.plot_trajectory as other 38 | traj.write_csv('/tmp/data.csv') 39 | other.plot_uav_trajectory('/tmp/data.csv') 40 | plt.show() 41 | except ImportError: 42 | print('requires plot_uav_trajectory module') 43 | 44 | #%% 45 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [metadata] 2 | # This includes the license file(s) in the wheel. 3 | # https://wheel.readthedocs.io/en/stable/user_guide.html#including-license-files-in-the-generated-wheel-file 4 | license_files = LICENSE.txt 5 | 6 | [bdist_wheel] 7 | # This flag says to generate wheels that support both Python 2 and Python 8 | # 3. If your code will not run unchanged on both Python 2 and 3, you will 9 | # need to generate separate wheels for each Python version that you 10 | # support. Removing this line (or setting universal to 0) will prevent 11 | # bdist_wheel from trying to make a universal wheel. For more see: 12 | # https://packaging.python.org/guides/distributing-packages-using-setuptools/#wheels 13 | universal=0 14 | 15 | # Tell make setup.py test use pytest 16 | [aliases] 17 | test=pytest -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | """Drone swarm control 2 | 3 | See: 4 | https://github.com/jgoppert/bswarm 5 | """ 6 | 7 | # Always prefer setuptools over distutils 8 | from setuptools import setup, find_packages 9 | from os import path 10 | # io.open is needed for projects that support Python 2.7 11 | # It ensures open() defaults to text mode with universal newlines, 12 | # and accepts an argument to specify the text encoding 13 | # Python 3 only projects can skip this import 14 | from io import open 15 | 16 | here = path.abspath(path.dirname(__file__)) 17 | 18 | # Get the long description from the README file 19 | with open(path.join(here, 'README.md'), encoding='utf-8') as f: 20 | long_description = f.read() 21 | 22 | # Arguments marked as "Required" below must be included for upload to PyPI. 23 | # Fields marked as "Optional" may be commented out. 24 | 25 | setup( 26 | # This is the name of your project. The first time you publish this 27 | # package, this name will be registered for you. It will determine how 28 | # users can install this project, e.g.: 29 | # 30 | # $ pip install sampleproject 31 | # 32 | # And where it will live on PyPI: https://pypi.org/project/sampleproject/ 33 | # 34 | # There are some restrictions on what makes a valid project name 35 | # specification here: 36 | # https://packaging.python.org/specifications/core-metadata/#name 37 | name='bswarm', # Required 38 | 39 | # Versions should comply with PEP 440: 40 | # https://www.python.org/dev/peps/pep-0440/ 41 | # 42 | # For a discussion on single-sourcing the version across setup.py and the 43 | # project code, see 44 | # https://packaging.python.org/en/latest/single_source_version.html 45 | version='0.0.0', # Required 46 | 47 | # This is a one-line description or tagline of what your project does. This 48 | # corresponds to the "Summary" metadata field: 49 | # https://packaging.python.org/specifications/core-metadata/#summary 50 | description='Drone swarm control', # Optional 51 | 52 | # This is an optional longer description of your project that represents 53 | # the body of text which users will see when they visit PyPI. 54 | # 55 | # Often, this is the same as your README, so you can just read it in from 56 | # that file directly (as we have already done above) 57 | # 58 | # This field corresponds to the "Description" metadata field: 59 | # https://packaging.python.org/specifications/core-metadata/#description-optional 60 | long_description=long_description, # Optional 61 | 62 | # Denotes that our long_description is in Markdown; valid values are 63 | # text/plain, text/x-rst, and text/markdown 64 | # 65 | # Optional if long_description is written in reStructuredText (rst) but 66 | # required for plain-text or Markdown; if unspecified, "applications should 67 | # attempt to render [the long_description] as text/x-rst; charset=UTF-8 and 68 | # fall back to text/plain if it is not valid rst" (see link below) 69 | # 70 | # This field corresponds to the "Description-Content-Type" metadata field: 71 | # https://packaging.python.org/specifications/core-metadata/#description-content-type-optional 72 | long_description_content_type='text/markdown', # Optional (see note above) 73 | 74 | # This should be a valid link to your project's main homepage. 75 | # 76 | # This field corresponds to the "Home-Page" metadata field: 77 | # https://packaging.python.org/specifications/core-metadata/#home-page-optional 78 | url='https://github.com/jgoppert/pybswarm', # Optional 79 | 80 | # This should be your name or the name of the organization which owns the 81 | # project. 82 | author='James Goppert', # Optional 83 | 84 | # This should be a valid email address corresponding to the author listed 85 | # above. 86 | author_email='james.goppert@gmail.com', # Optional 87 | 88 | # Classifiers help users find your project by categorizing it. 89 | # 90 | # For a list of valid classifiers, see https://pypi.org/classifiers/ 91 | classifiers=[ # Optional 92 | # How mature is this project? Common values are 93 | # 3 - Alpha 94 | # 4 - Beta 95 | # 5 - Production/Stable 96 | 'Development Status :: 3 - Alpha', 97 | 98 | # Indicate who your project is intended for 99 | 'Intended Audience :: Developers', 100 | 'Topic :: Software Development :: Build Tools', 101 | 102 | # Pick your license as you wish 103 | 'License :: OSI Approved :: MIT License', 104 | 105 | # Specify the Python versions you support here. In particular, ensure 106 | # that you indicate whether you support Python 2, Python 3 or both. 107 | # These classifiers are *not* checked by 'pip install'. See instead 108 | # 'python_requires' below. 109 | #'Programming Language :: Python :: 2', 110 | #'Programming Language :: Python :: 2.7', 111 | 'Programming Language :: Python :: 3', 112 | #'Programming Language :: Python :: 3.4', 113 | #'Programming Language :: Python :: 3.5', 114 | 'Programming Language :: Python :: 3.6', 115 | 'Programming Language :: Python :: 3.7', 116 | ], 117 | 118 | # This field adds keywords for your project which will appear on the 119 | # project page. What does your project relate to? 120 | # 121 | # Note that this is a string of words separated by whitespace, not a list. 122 | keywords='drone swarm', # Optional 123 | 124 | # You can just specify package directories manually here if your project is 125 | # simple. Or you can use find_packages(). 126 | # 127 | # Alternatively, if you just want to distribute a single Python file, use 128 | # the `py_modules` argument instead as follows, which will expect a file 129 | # called `my_module.py` to exist: 130 | # 131 | # py_modules=["my_module"], 132 | # 133 | packages=find_packages(exclude=['contrib', 'docs', 'tests']), # Required 134 | 135 | # Specify which Python versions you support. In contrast to the 136 | # 'Programming Language' classifiers above, 'pip install' will check this 137 | # and refuse to install the project if the version does not match. If you 138 | # do not support Python 2, you can simplify this to '>=3.5' or similar, see 139 | # https://packaging.python.org/guides/distributing-packages-using-setuptools/#python-requires 140 | python_requires='>=3.6', 141 | 142 | # This field lists other packages that your project depends on to run. 143 | # Any package you put here will be installed by pip when your project is 144 | # installed, so they must be valid existing projects. 145 | # 146 | # For an analysis of "install_requires" vs pip's requirements files see: 147 | # https://packaging.python.org/en/latest/requirements.html 148 | install_requires=['numpy', 'matplotlib'], # Optional 149 | 150 | # Testing package 151 | setup_requires=['pytest-runner'], 152 | tests_require=['pytest'], 153 | 154 | # List additional groups of dependencies here (e.g. development 155 | # dependencies). Users will be able to install these using the "extras" 156 | # syntax, for example: 157 | # 158 | # $ pip install bswarm[dev] 159 | # 160 | # Similar to `install_requires` above, these must be valid existing 161 | # projects. 162 | extras_require={ # Optional 163 | 'dev': ['check-manifest'], 164 | 'test': ['coverage'], 165 | }, 166 | 167 | # If there are data files included in your packages that need to be 168 | # installed, specify them here. 169 | # 170 | # If using Python 2.6 or earlier, then these have to be included in 171 | # MANIFEST.in as well. 172 | package_data={ # Optional 173 | 'bswarm': ['data/package_data.dat'], 174 | }, 175 | 176 | # Although 'package_data' is the preferred approach, in some case you may 177 | # need to place data files outside of your packages. See: 178 | # http://docs.python.org/3.4/distutils/setupscript.html#installing-additional-files 179 | # 180 | # In this case, 'data_file' will be installed into '/my_data' 181 | #data_files=[('my_data', ['data/data_file'])], # Optional 182 | 183 | # To provide executable scripts, use entry points in preference to the 184 | # "scripts" keyword. Entry points provide cross-platform support and allow 185 | # `pip` to create the appropriate form of executable for the target 186 | # platform. 187 | # 188 | # For example, the following would provide a command called `bswarm` which 189 | # executes the function `main` from this package when invoked: 190 | entry_points={ # Optional 191 | 'console_scripts': [ 192 | 'bswarm=bswarm:main', 193 | ], 194 | }, 195 | 196 | # List additional URLs that are relevant to your project as a dict. 197 | # 198 | # This field corresponds to the "Project-URL" metadata fields: 199 | # https://packaging.python.org/specifications/core-metadata/#project-url-multiple-use 200 | # 201 | # Examples listed include a pattern for specifying where the package tracks 202 | # issues, where the source is hosted, where to say thanks to the package 203 | # maintainers, and where to support the project financially. The key is 204 | # what's used to render the link text on PyPI. 205 | project_urls={ # Optional 206 | 'Bug Reports': 'https://github.com/jgoppert/pybswarm/issues', 207 | #'Funding': 'https://donate.pypi.org', 208 | #'Say Thanks!': 'http://saythanks.io/to/example', 209 | 'Source': 'https://github.com/jgoppert/pybswarm/', 210 | }, 211 | ) 212 | -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- 1 | # the inclusion of the tests module is not meant to offer best practices for 2 | # testing in general, but rather to support the `find_packages` example in 3 | # setup.py that excludes installing the "tests" package 4 | -------------------------------------------------------------------------------- /tests/test_simple.py: -------------------------------------------------------------------------------- 1 | #%% 2 | import os, sys 3 | os.chdir('../') 4 | print(os.getcwd()) 5 | #%% 6 | import bswarm 7 | import bswarm.trajectory as traj 8 | import bswarm.formation 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | 12 | #%% 13 | def test_load(): 14 | bswarm.load_file() 15 | assert True 16 | 17 | 18 | def test_trajectory(): 19 | res = traj.min_snap_1d([1, 2, 3], [1, 2], False) 20 | plt.figure(figsize=(6, 5)) 21 | traj.plot_trajectories_time_history([res]) 22 | plt.show(block=False) 23 | plt.pause(1) 24 | plt.close() 25 | 26 | 27 | def test_rotation(): 28 | P = np.array([ 29 | [1, 2, 3], 30 | [2, 3, 4], 31 | [6, 3, 1] 32 | ]).T 33 | P_rot = bswarm.formation.rotate_points_z(P, 2*np.pi) 34 | assert np.allclose(P, P_rot) 35 | 36 | def test_trajectory_2d(): 37 | # continous 38 | T = [1, 2] 39 | waypoints = np.array([ 40 | [0, 0], 41 | [1, 2], 42 | [3, 4] 43 | ]) 44 | trajx = traj.min_accel_1d(waypoints[:, 0], T, False) 45 | trajy = traj.min_accel_1d(waypoints[:, 1], T, False) 46 | print('x coef', trajx.coef_array()) 47 | print('y coef', trajy.coef_array()) 48 | 49 | # %% 50 | -------------------------------------------------------------------------------- /tox.ini: -------------------------------------------------------------------------------- 1 | # this file is *not* meant to cover or endorse the use of tox or pytest or 2 | # testing in general, 3 | # 4 | # It's meant to show the use of: 5 | # 6 | # - check-manifest 7 | # confirm items checked into vcs are in your sdist 8 | # - python setup.py check 9 | # confirm required package meta-data in setup.py 10 | # - readme_renderer (when using a ReStructuredText README) 11 | # confirms your long_description will render correctly on PyPI. 12 | # 13 | # and also to help confirm pull requests to this project. 14 | 15 | [tox] 16 | envlist = py{36,37} 17 | 18 | [testenv] 19 | basepython = 20 | py36: python3.6 21 | py37: python3.7 22 | deps = 23 | check-manifest 24 | # If your project uses README.rst, uncomment the following: 25 | # readme_renderer 26 | flake8 27 | pytest 28 | commands = 29 | check-manifest --ignore tox.ini,tests* 30 | # This repository uses a Markdown long_description, so the -r flag to 31 | # `setup.py check` is not needed. If your project contains a README.rst, 32 | # use `python setup.py check -m -r -s` instead. 33 | python setup.py check -m -s 34 | flake8 . 35 | py.test tests 36 | [flake8] 37 | exclude = .tox,*.egg,build,data 38 | select = E,W,F 39 | --------------------------------------------------------------------------------