├── .gitignore ├── .vscode └── settings.json ├── DroneController ├── Drone.py ├── EKF.py ├── GUI.py ├── Quadrotor.py ├── TrajFollower.py ├── ViconClient.py ├── __init__.py └── utilities.py ├── PathGen ├── L1.csv ├── L2.csv └── interpolatedPath.py ├── README.md ├── __pycache__ └── Quadrotor.cpython-36.pyc ├── arrows ├── down.png ├── left.png ├── right.png └── up.png ├── backup ├── config.ini ├── main.py ├── main_rosetta copy.py ├── main_rosetta.py ├── main_vicon.py ├── vicon.py └── window_v0.ui ├── conda_init.sh ├── drone.png ├── param copy.yaml ├── param.yaml ├── requirements.txt ├── run.sh ├── trajs ├── .~lock.traj_2obs_vicon.csv# ├── .~lock.traj_L_vicon.csv# ├── interpolate.py ├── spiral8.csv ├── spiral82.csv ├── spiral84.csv ├── spiral85.csv ├── subsample.py ├── traj_2obs_vicon.csv └── traj_L_vicon.csv ├── window.ui └── workspace.py /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | .pyc 3 | .venv 4 | __pycache__ 5 | .vscode 6 | 'python=3.6' 7 | pyvicon-datatstream-module 8 | DroneController/__pycache__ -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "ros.distro": "humble" 3 | } -------------------------------------------------------------------------------- /DroneController/Drone.py: -------------------------------------------------------------------------------- 1 | from dronekit import connect, VehicleMode 2 | from pymavlink import mavutil 3 | from PyQt5.QtCore import QTimer 4 | import time 5 | from PyQt5.QtCore import QObject, pyqtSignal, pyqtSlot 6 | 7 | 8 | from .utilities import State, DynamicAttributes 9 | 10 | import logging 11 | logging.basicConfig( 12 | format='%(asctime)s %(levelname)-8s %(message)s', 13 | level=logging.INFO, 14 | datefmt='[Drone %H:%M:%S]') 15 | 16 | 17 | class djiDrone(QObject): 18 | updateFlightModeGUI = pyqtSignal(str) 19 | 20 | def __init__(self, parent: QObject, config:DynamicAttributes) -> None: 21 | super().__init__(parent) 22 | self.__config = config 23 | self.state = State.LAND 24 | self.default_cmd_vel = self.__config.default_cmd_vel 25 | self.default_dt = self.__config.default_dt 26 | self.mavlink_connect(self.__config.mavlink) 27 | 28 | 29 | def mavlink_connect(self, connection_string): 30 | self.vehicle = connect(connection_string, wait_ready=True) 31 | self.state = State.HOVER 32 | 33 | # Display Flight Mode 34 | def modeCallback(vehicle, name, mode): 35 | # print(vehicle, name, mode) 36 | index, modeName = str(mode).split(':') 37 | self.updateFlightModeGUI.emit(modeName) 38 | self.addObserverAndInit( 39 | 'mode' 40 | , modeCallback 41 | ) 42 | 43 | self.vehicle.mode = VehicleMode("GUIDED") 44 | 45 | 46 | self.viz_timer = QTimer() 47 | self.viz_timer.timeout.connect(self.updateModeGUI) 48 | self.viz_timer.start(1000) 49 | 50 | def updateModeGUI(self): 51 | index, modeName = str(self.vehicle.mode).split(':') 52 | self.updateFlightModeGUI.emit(modeName) 53 | logging.info(modeName) 54 | self.viz_timer.stop() 55 | 56 | 57 | def addObserverAndInit(self, name, cb): 58 | """We go ahead and call our observer once at startup to get an initial value""" 59 | self.vehicle.add_attribute_listener(name, cb) 60 | 61 | 62 | 63 | 64 | ############### MAV LINK communication ########################################################################## 65 | def publish_cmd_vel(self, velocity_x, velocity_y, velocity_z): 66 | msg = self.vehicle.message_factory.set_position_target_local_ned_encode( 67 | 0, # time_boot_ms (not used) 68 | 0, 0, # target system, target component 69 | mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame 70 | 0b0000111111000111, # type_mask (only speeds enabled) 71 | 0, 0, 0, # x, y, z positions (not used) 72 | velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s 73 | 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) 74 | 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 75 | self.vehicle.send_mavlink(msg) 76 | 77 | def send_ned_velocity(self, velocity_x, velocity_y, velocity_z, duration): 78 | # send command to vehicle on x Hz cycle 79 | elapsed_time = 0.0 80 | while elapsed_time < duration: 81 | self.publish_cmd_vel(velocity_x, velocity_y, velocity_z) 82 | time.sleep(self.default_dt) 83 | elapsed_time += self.default_dt 84 | ############### Joystick communication ########################################################################## 85 | def vehicle_validation(self, function): 86 | if self.vehicle.mode == "GUIDED": 87 | logging.info(f'button clicked {function.__name__}') 88 | function() 89 | @pyqtSlot() 90 | def west_click(self): 91 | if self.state != State.HOVER: 92 | logging.warning("[Invalid request]: moving west is not possible") 93 | return 94 | @self.vehicle_validation 95 | def west_wrapped(): 96 | self.send_ned_velocity(0, self.default_cmd_vel, 0, 1) 97 | # self.send_ned_velocity(0, 0, 0, 1) 98 | @pyqtSlot() 99 | def east_click(self): 100 | if self.state != State.HOVER: 101 | logging.warning("[Invalid request]: moving east is not possible") 102 | return 103 | @self.vehicle_validation 104 | def east_wrapped(): 105 | self.send_ned_velocity(0, -self.default_cmd_vel, 0, 1) 106 | # self.send_ned_velocity(0, 0, 0, 1) 107 | 108 | @pyqtSlot() 109 | def north_click(self): 110 | if self.state != State.HOVER: 111 | logging.warning("[Invalid request]: moving north is not possible") 112 | return 113 | @self.vehicle_validation 114 | def north_wrapped(): 115 | self.send_ned_velocity(-self.default_cmd_vel, 0, 0, 1) 116 | # self.send_ned_velocity(0, 0, 0, 1) 117 | @pyqtSlot() 118 | def south_click(self): 119 | if self.state != State.HOVER: 120 | logging.warning("[Invalid request]: moving south is not possible") 121 | return 122 | @self.vehicle_validation 123 | def south_wrapped(): 124 | self.send_ned_velocity(self.default_cmd_vel, 0, 0, 1) 125 | # self.send_ned_velocity(0, 0, 0, 1) 126 | 127 | @pyqtSlot() 128 | def rtl_click(self): 129 | if self.state == State.LAND or self.state == State.INITIALIZED: 130 | logging.warning("[Invalid request]: landing is not possible") 131 | return 132 | @self.vehicle_validation 133 | def rtl_wrapped(): 134 | self.vehicle.mode = VehicleMode("LAND") 135 | self.state = State.LAND 136 | 137 | @pyqtSlot() 138 | def up_click(self): 139 | if self.state != State.HOVER: 140 | logging.warning("[Invalid request]: moving up is not possible") 141 | return 142 | @self.vehicle_validation 143 | def up_wrapped(): 144 | alt = self.vehicle.location.global_relative_frame.alt 145 | if alt < 3: 146 | self.send_ned_velocity(0, 0, 0.5 * self.default_cmd_vel, 1) 147 | # self.send_ned_velocity(0, 0, 0, 1) 148 | 149 | @pyqtSlot() 150 | def down_click(self): 151 | if self.state != State.HOVER: 152 | logging.warning("[Invalid request]: moving down is not possible") 153 | return 154 | @self.vehicle_validation 155 | def down_wrapped(): 156 | alt = self.vehicle.location.global_relative_frame.alt 157 | if alt > 0.5: 158 | self.send_ned_velocity(0, 0, -0.5 * self.default_cmd_vel, 1) 159 | # self.send_ned_velocity(0, 0, 0, 1) 160 | 161 | 162 | 163 | -------------------------------------------------------------------------------- /DroneController/EKF.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | # Covariance for EKF simulation 4 | Q = np.diag([ 5 | 0.1, # variance of location on x-axis 6 | 0.1, # variance of location on y-axis 7 | 0.08, # variance of velocity on x-axis 8 | 0.08, # variance of velocity on y-axis 9 | 0.06, # variance of acceleration on x-axis 10 | 0.06 # variance of acceleration on y-axis 11 | ]) ** 2 # predict state covariance 12 | R = np.diag([0.01, 0.01]) ** 2 # Observation x,y position covariance 13 | 14 | def motion_model(x, u, DT): 15 | F = np.array([[1.0, 0, 0, 0, 0.5 * DT * DT, 0], 16 | [0, 1.0, 0, 0, 0, 0.5 * DT * DT], 17 | [0, 0, 0.0, 0, 0, 0], 18 | [0, 0, 0, 0, 0, 0], 19 | [0, 0, 0, 0, 0, 0], 20 | [0, 0, 0, 0, 0, 0] 21 | ], dtype=np.float32) 22 | 23 | B = np.array([[DT, 0], 24 | [DT, 0], 25 | [1.0, 0.0], 26 | [1.0, 0.0], 27 | [1.0 / DT, 0.0], 28 | [1.0 / DT, 0.0] 29 | ], dtype=np.float32) 30 | 31 | x = F @ x + B @ u 32 | 33 | return x 34 | 35 | 36 | def observation_model(x): 37 | H = np.array([ 38 | [1, 0, 0, 0, 0, 0], 39 | [0, 1, 0, 0, 0, 0] 40 | ], dtype=np.float32) 41 | z = H @ x 42 | 43 | return z 44 | 45 | 46 | def jacob_f(x, u, DT): 47 | """ 48 | Jacobian of Motion Model 49 | 50 | motion model 51 | """ 52 | vx = u[0, 0] 53 | vy = u[1, 0] 54 | ax = x[4, 0] 55 | ay = x[5, 0] 56 | 57 | jF = np.array([ 58 | [1.0, 0.0, DT * vx, 0.0, 0.5 * DT * DT * ax, 0.0], 59 | [0.0, 1.0, 0.0, DT * vy, 0.0, 0.5 * DT * DT * ay], 60 | [0.0, 0.0, 1.0, 0.0, 0.0, 0.0], 61 | [0.0, 0.0, 0.0, 1.0, 0.0, 0.0], 62 | [0.0, 0.0, 0.0, 0.0, 1.0, 0.0], 63 | [0.0, 0.0, 0.0, 0.0, 0.0, 1.0] 64 | ], dtype=np.float32) 65 | return jF 66 | 67 | 68 | def jacob_h(): 69 | jH = np.array([[1, 0, 0, 0, 0, 0], 70 | [0, 1, 0, 0, 0, 0]], dtype=np.float32) 71 | return jH 72 | 73 | 74 | def ekf_estimation(xEst, PEst, z, u, DT): 75 | # Predict 76 | xPred = motion_model(xEst, u, DT) 77 | jF = jacob_f(xEst, u, DT) 78 | PPred = jF @ PEst @ jF.T + Q 79 | 80 | # Update 81 | jH = jacob_h() 82 | zPred = observation_model(xPred) 83 | y = z - zPred 84 | S = jH @ PPred @ jH.T + R 85 | K = PPred @ jH.T @ np.linalg.inv(S) 86 | xEst = xPred + K @ y 87 | PEst = (np.eye(len(xEst)) - K @ jH) @ PPred 88 | return xEst, PEst -------------------------------------------------------------------------------- /DroneController/GUI.py: -------------------------------------------------------------------------------- 1 | from dronekit import VehicleMode 2 | 3 | from PyQt5.QtWidgets import QMainWindow 4 | from PyQt5 import uic 5 | from PyQt5.QtCore import QTimer, QObject, pyqtSignal, pyqtSlot 6 | 7 | 8 | import time 9 | from .Quadrotor import Quadrotor 10 | import numpy as np 11 | from PyQt5.QtCore import QTimer, QObject, pyqtSignal, pyqtSlot 12 | 13 | from .ViconClient import ViconClient 14 | from .Drone import djiDrone 15 | from .TrajFollower import TrajFollower 16 | from .utilities import ConfigParser 17 | 18 | import logging 19 | logging.basicConfig( 20 | format='%(asctime)s %(levelname)-8s %(message)s', 21 | level=logging.INFO, 22 | datefmt='[GUI %H:%M:%S]') 23 | 24 | 25 | 26 | class MainWindow(QMainWindow): 27 | 28 | def __init__(self, config:ConfigParser): 29 | super().__init__() 30 | self.__config = config.GUI 31 | ui_path = self.__config.ui_path 32 | uic.loadUi(ui_path, self) 33 | self.coord = np.zeros(4) # x, y, z, yaw 34 | 35 | # add visualizer 36 | self.quad = Quadrotor(size=0.5) 37 | self.viewer.addWidget(self.quad.canvas) 38 | 39 | 40 | self.vicon = ViconClient(self, config.Vicon) 41 | self.vicon.pose_signal.connect(self.pose_callback) 42 | 43 | # viz timer 44 | self.viz_timer = QTimer() 45 | self.viz_timer.timeout.connect(self.updatePoseGUI) 46 | self.viz_timer.start(self.__config.viz_dt) 47 | 48 | 49 | # establish mavlink connection 50 | self.dji = djiDrone(self, config.Drone) 51 | self.dji.updateFlightModeGUI.connect(self.updateFlightModeGUI) 52 | 53 | self.btnWest.clicked.connect(self.dji.west_click) 54 | self.btnEast.clicked.connect(self.dji.east_click) 55 | self.btnNorth.clicked.connect(self.dji.north_click) 56 | self.btnSouth.clicked.connect(self.dji.south_click) 57 | self.btnRTL.clicked.connect(self.dji.rtl_click) 58 | self.btnUp.clicked.connect(self.dji.up_click) 59 | self.btnDown.clicked.connect(self.dji.down_click) 60 | 61 | 62 | # trajectory follower 63 | self.traj = TrajFollower(self.coord, config.Trajectory, self.dji, self) 64 | self.btnSendTraj.clicked.connect(self.sendTrajectory) 65 | self.numPointsText.textChanged.connect(self.updateNumPoints) 66 | self.traj.maxVelSignal.connect(self.updateMaxVel) 67 | 68 | def sendTrajectory(self): 69 | self.traj.sendTrajectory() 70 | self.quad.setPath(self.traj.traj) 71 | 72 | 73 | def updateNumPoints(self): 74 | text = self.numPointsText.text() 75 | try: 76 | numPoints = int(text) 77 | self.traj.updateTrajPoints(numPoints) 78 | except: 79 | pass 80 | 81 | @pyqtSlot(dict) 82 | def pose_callback(self, data): 83 | posistion = data['position'] 84 | yaw = data['orientation'] 85 | 86 | x, y, z = posistion 87 | 88 | 89 | self.coord[0] = x 90 | self.coord[1] = y 91 | self.coord[2] = z 92 | self.coord[3] = yaw 93 | 94 | 95 | self.lblLongValue.setText(f"{x:.4f}") 96 | self.lblLatValue.setText(f"{y:.4f}") 97 | self.lblAltValue.setText(f"{z:.4f}") 98 | logging.debug(f'location: {x}, {y}, {z}') 99 | 100 | @pyqtSlot(str) 101 | def updateFlightModeGUI(self, value): 102 | logging.info(f'flight mode change to {value}') 103 | # index, mode = str(value).split(':') 104 | self.lblFlightModeValue.setText(value) 105 | self.progressBar.setValue(100) 106 | 107 | def updatePoseGUI(self): 108 | heading = np.deg2rad(45) + self.coord[3] 109 | self.quad.update_pose( self.coord[0], self.coord[1], self.coord[2],0,0, heading) 110 | 111 | @pyqtSlot(float) 112 | def updateMaxVel(self, value): 113 | self.lbMaxVal.setText(f"{value:.3f} m/s") -------------------------------------------------------------------------------- /DroneController/Quadrotor.py: -------------------------------------------------------------------------------- 1 | """ 2 | Class for plotting a quadrotor 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | from math import cos, sin 8 | import numpy as np 9 | 10 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg, NavigationToolbar2QT 11 | from matplotlib.figure import Figure 12 | 13 | 14 | class Quadrotor: 15 | def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True): 16 | self.p1 = np.array([size / 2, 0, 0, 1]).T 17 | self.p2 = np.array([-size / 2, 0, 0, 1]).T 18 | self.p3 = np.array([0, size / 2, 0, 1]).T 19 | self.p4 = np.array([0, -size / 2, 0, 1]).T 20 | 21 | self.x_data = [] 22 | self.y_data = [] 23 | self.z_data = [] 24 | self.show_animation = show_animation 25 | self.path = None 26 | 27 | if self.show_animation: 28 | # plt.ion() 29 | # fig = plt.figure() 30 | # # for stopping simulation with the esc key. 31 | # fig.canvas.mpl_connect('key_release_event', 32 | # lambda event: [exit(0) if event.key == 'escape' else None]) 33 | self.fig = Figure() 34 | self.ax = self.fig.add_subplot(111, projection='3d') 35 | self.canvas = FigureCanvasQTAgg(self.fig) 36 | 37 | 38 | self.update_pose(x, y, z, roll, pitch, yaw) 39 | 40 | def setPath(self, path:np.ndarray): 41 | self.path = path 42 | 43 | 44 | def update_pose(self, x, y, z, roll, pitch, yaw): 45 | self.x = x 46 | self.y = y 47 | self.z = z 48 | self.roll = roll 49 | self.pitch = pitch 50 | self.yaw = yaw 51 | self.x_data.append(x) 52 | self.y_data.append(y) 53 | self.z_data.append(z) 54 | 55 | if self.show_animation: 56 | self.plot() 57 | 58 | def transformation_matrix(self): 59 | x = self.x 60 | y = self.y 61 | z = self.z 62 | roll = self.roll 63 | pitch = self.pitch 64 | yaw = self.yaw 65 | return np.array( 66 | [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x], 67 | [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) 68 | * sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y], 69 | [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(roll), z] 70 | ]) 71 | 72 | def plot(self): # pragma: no cover 73 | T = self.transformation_matrix() 74 | 75 | p1_t = np.matmul(T, self.p1) 76 | p2_t = np.matmul(T, self.p2) 77 | p3_t = np.matmul(T, self.p3) 78 | p4_t = np.matmul(T, self.p4) 79 | 80 | # plt.cla() 81 | self.ax.cla() 82 | 83 | self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]], 84 | [p1_t[1], p2_t[1], p3_t[1], p4_t[1]], 85 | [p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.') 86 | 87 | self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]], 88 | [p1_t[2], p2_t[2]], 'r-') 89 | self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]], 90 | [p3_t[2], p4_t[2]], 'g-') 91 | 92 | self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:') 93 | 94 | if self.path is not None: 95 | self.ax.plot(self.path[:, 0], self.path[:, 1], np.ones(len(self.path)), 'r') 96 | 97 | self.ax.set_xlim(-3.0, 3.0) 98 | self.ax.set_ylim(-3.0, 3.0) 99 | self.ax.set_zlim(0, 3) 100 | 101 | # plt.pause(0.001) 102 | self.canvas.draw() 103 | 104 | if __name__ == '__main__': 105 | quad = Quadrotor(size=0.5) 106 | alt = 0 107 | while True: 108 | alt += 0.1 109 | quad.update_pose(0,0,alt,0,0,0) 110 | quad.plot() 111 | if alt > 5.0: 112 | break -------------------------------------------------------------------------------- /DroneController/TrajFollower.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import os 3 | from PyQt5.QtCore import QTimer, QObject, pyqtSignal, pyqtSlot 4 | from .utilities import State, DynamicAttributes 5 | from .Drone import djiDrone 6 | from .EKF import ekf_estimation 7 | import logging 8 | logging.basicConfig( 9 | format='%(asctime)s %(levelname)-8s %(message)s', 10 | level=logging.INFO, 11 | datefmt='[TrajFollower %H:%M:%S]') 12 | 13 | 14 | 15 | def generate_spiral_eight(num_points, x_scale=3.5, y_scale=2.0, t0=0.0): 16 | """ 17 | Generates coordinates for points on a circle. 18 | 19 | Args: 20 | num_points (int): Number of points on the circle. 21 | x_scale (float, optional): Scaling factor for x-coordinate. Defaults to 2.0. 22 | y_scale (float, optional): Scaling factor for y-coordinate. Defaults to 2.0. 23 | t0 (float, optional): Offset value for the angle. Defaults to 0.0. 24 | 25 | Returns: 26 | tuple: A tuple containing two NumPy arrays (x_coordinates, y_coordinates). 27 | """ 28 | theta = np.linspace(t0, t0 + 2*np.pi, num_points) # Create angles for all points 29 | x = x_scale * np.cos(theta) * np.sin(theta) 30 | y = y_scale * np.sin(theta) 31 | return x, y 32 | 33 | def calculate_derivaties(waypoints): 34 | """ 35 | Calculates velocities for a given set of waypoints. 36 | 37 | Args: 38 | waypoints (list): A list of waypoints, where each waypoint is a tuple (x, y). 39 | 40 | Returns: 41 | list: A list of velocities, where each velocity is a tuple (vx, vy). 42 | """ 43 | 44 | num_points = len(waypoints) 45 | velocities = [] 46 | for i in range(1, num_points): 47 | # Get current and previous waypoints 48 | current_point = waypoints[i] 49 | prev_point = waypoints[i-1] 50 | 51 | # Calculate relative distance (assuming Euclidean distance) 52 | dx = current_point[0] - prev_point[0] 53 | dy = current_point[1] - prev_point[1] 54 | distance = np.sqrt(dx**2 + dy**2) 55 | if distance != 0.0: 56 | # Velocity is assumed to be constant between waypoints 57 | # (adjust this logic if you have additional information about speed) 58 | velocity = (dx / distance, dy / distance) 59 | else: 60 | velocity = (0.0, 0.0) 61 | 62 | velocities.append(velocity) 63 | velocity = (0.0, 0.0) 64 | velocities.append(velocity) 65 | 66 | return np.array(velocities) 67 | 68 | 69 | class TrajFollower(QObject): 70 | 71 | emergencyLand = pyqtSignal() 72 | maxVelSignal = pyqtSignal(float) 73 | def __init__(self, coord:np.array, config:DynamicAttributes, drone: djiDrone, parent: QObject) -> None: 74 | super().__init__(parent) 75 | self.__config = config 76 | self.traj_dt = self.__config.traj_dt 77 | self.coord = coord 78 | self.drone = drone 79 | self.traj_path = self.__config.traj_path 80 | self.emergencyLand.connect(self.drone.rtl_click) 81 | self.fence = self.__config.fence 82 | self.num_points = 500 83 | self.traj = None 84 | 85 | 86 | def checkWithinGeoFence(self): 87 | x, y = self.coord[0], self.coord[1] 88 | 89 | if x >= self.fence[0] and x <= self.fence[1]: 90 | if y >= self.fence[2] and y <= self.fence[3]: 91 | return True 92 | return False 93 | 94 | 95 | def updateTrajPoints(self, num_points): 96 | self.num_points = num_points 97 | logging.info(f"[TrajFollower] update num of geom points {self.num_points}") 98 | 99 | 100 | def traj_callback(self): 101 | if self.drone.state == State.LAND: 102 | logging.info("trajectory timer canceled") 103 | self.traj_timer.stop() 104 | 105 | if not self.checkWithinGeoFence(): 106 | self.drone.publish_cmd_vel(0, 0, 0) 107 | logging.info("geofence enforce emergency landing ..") 108 | self.emergencyLand.emit() 109 | return 110 | 111 | self.traj_index += 1 112 | if self.traj_index < len(self.traj): 113 | # target = self.traj[self.traj_index][1:] 114 | # vel = target - self.coord[:3] 115 | # vx, vy, vz = vel 116 | 117 | target = self.traj[self.traj_index] 118 | target_vel = self.traj_vel[self.traj_index] 119 | target_acc = self.traj_acc[self.traj_index] 120 | dt = self.traj_dt / 1000.0 121 | 122 | setpoint = np.vstack((target, target_vel, target_acc)).flatten() 123 | current = np.squeeze(self.xEst) 124 | error = setpoint - current 125 | K = np.array([ 126 | [1, 0, dt, 0, dt * dt / 2.0, 0], 127 | [0, 1, 0, dt, 0, dt * dt / 2.0] 128 | ]) 129 | vx, vy = K @ error 130 | 131 | logging.debug(f"[Traj] vx = {vx:.3f}, vy = {vy:.3f} ") 132 | self.drone.publish_cmd_vel(-vy, -vx, 0.0) 133 | 134 | # update ekf 135 | z = np.array([[self.coord[0]], [self.coord[1]]]) 136 | u = np.array([[vx], [vy]]) 137 | self.xEst, self.PEst = ekf_estimation(self.xEst, self.PEst, z, u, dt) 138 | self.VMAX = max(self.VMAX, np.sqrt(self.xEst[2, 0] ** 2 + self.xEst[3, 0] ** 2)) 139 | logging.debug(f"VMAX = {self.VMAX}") 140 | self.maxVelSignal.emit(self.VMAX) 141 | 142 | 143 | # elif self.traj_index - 10 < len(self.traj): 144 | # self.drone.publish_cmd_vel(0, 0, 0) 145 | else: 146 | logging.info("trajectory timer stopped") 147 | self.traj_timer.stop() 148 | 149 | # @pyqtSlot() 150 | def sendTrajectory(self): 151 | 152 | # if not os.path.exists(self.traj_path): 153 | # logging.error(f'{self.traj_path} does not exist!') 154 | # return 155 | 156 | 157 | if self.drone.state != State.HOVER: 158 | return 159 | 160 | 161 | self.xEst = np.zeros((6, 1), dtype=np.float32) 162 | self.PEst = np.eye(6, dtype=np.float32) 163 | self.xEst[0, 0] = self.coord[0] 164 | self.xEst[1, 0] = self.coord[1] 165 | self.VMAX = 0.0 166 | 167 | 168 | # self.traj_path = "trajs/traj_2obs_vicon.csv" 169 | # self.traj_path = 'trajs/traj_L_vicon.csv' 170 | 171 | # # # comment out the below 3 lines for different traj 172 | # self.traj = np.loadtxt(self.traj_path, delimiter=",")[:, :2] 173 | # # self.traj = np.flipud(self.traj) 174 | # # self.traj = np.fliplr(self.traj) 175 | 176 | # compute trajectory 177 | xx, yy = generate_spiral_eight(num_points=self.num_points) 178 | self.traj = np.column_stack((xx, yy)) 179 | self.traj_vel = calculate_derivaties(self.traj) 180 | self.traj_acc = calculate_derivaties(self.traj_vel) 181 | logging.info(f'sending trajectory points = {self.traj.shape}, vel = {self.traj_vel.shape}, acc = {self.traj_acc.shape}') 182 | 183 | 184 | 185 | self.traj_timer = QTimer() 186 | self.traj_index = 0 187 | self.traj_timer.timeout.connect(self.traj_callback) 188 | self.traj_timer.start(self.traj_dt) -------------------------------------------------------------------------------- /DroneController/ViconClient.py: -------------------------------------------------------------------------------- 1 | import time 2 | import numpy as np 3 | from PyQt5.QtCore import QTimer, QObject, pyqtSignal, pyqtSlot 4 | from pyvicon_datastream import tools 5 | from .utilities import State, DynamicAttributes 6 | 7 | import logging 8 | logging.basicConfig( 9 | format='%(asctime)s %(levelname)-8s %(message)s', 10 | level=logging.INFO, 11 | datefmt='[Vicon %H:%M:%S]') 12 | 13 | 14 | class ViconClient(QObject): 15 | pose_signal = pyqtSignal(dict) 16 | 17 | def __init__(self, parent: QObject, config:DynamicAttributes) -> None: 18 | super().__init__(parent) 19 | self.__config = config 20 | self.IP_ADDR = self.__config.IP_ADDR 21 | self.tracker = tools.ObjectTracker(self.IP_ADDR) 22 | self.obj_name = self.__config.obj_name 23 | if self.tracker.is_connected: 24 | self.state = State.VICON_CONNECTED 25 | logging.info('vicon connected ...') 26 | else: 27 | self.state = State.VICON_DISCONNECTED 28 | logging.error('vicon cannot be connected') 29 | self.connection_ressolve() 30 | 31 | 32 | 33 | def pose_callback(self): 34 | 35 | self.state = State.VICON_CONNECTED if self.tracker.is_connected else State.VICON_DISCONNECTED 36 | if self.state == State.VICON_DISCONNECTED: 37 | self.connection_ressolve() 38 | return 39 | 40 | latency, frameno, position = self.tracker.get_position(self.obj_name) 41 | if position != []: 42 | xyz_position = position[0][2:5] # get x,y,z only 43 | # xyz_position[0] *= -1 44 | # xyz_position[1] *= -1 45 | orientation = position[0][7] # get rotation around z axis 46 | data = {"position": np.array(xyz_position) / 1000.0, "orientation" : orientation} 47 | self.pose_signal.emit(data) 48 | 49 | def connection_ressolve(self): 50 | while self.state == State.VICON_DISCONNECTED: 51 | logging.warn('waiting for vicon to connect ...[!]') 52 | self.state = State.VICON_CONNECTED if self.tracker.connect(self.IP_ADDR) else State.VICON_DISCONNECTED 53 | time.sleep(1) 54 | logging.info('vicon connection ressolved ...') 55 | self.vicon_timer = QTimer() 56 | self.vicon_timer.timeout.connect(self.pose_callback) 57 | self.vicon_timer.start(self.__config.vicon_dt) 58 | 59 | 60 | -------------------------------------------------------------------------------- /DroneController/__init__.py: -------------------------------------------------------------------------------- 1 | from .GUI import MainWindow 2 | from .utilities import ConfigParser -------------------------------------------------------------------------------- /DroneController/utilities.py: -------------------------------------------------------------------------------- 1 | from enum import Enum 2 | import yaml 3 | 4 | class State(Enum): 5 | INITIALIZED = 0 6 | ARMED = 1 7 | TAKEOFF = 2 8 | HOVER = 3 9 | LAND = 4 10 | POSE_ESTIMATOR = 5 11 | VISUALIZATION = 6 12 | TRAJ_FOLLOWER = 7 13 | VICON_CONNECTED = 8 14 | VICON_DISCONNECTED = 9 15 | 16 | 17 | class DynamicAttributes: 18 | def __init__(self, attributes_dict): 19 | for key, value in attributes_dict.items(): 20 | setattr(self, key, value) 21 | 22 | class ConfigParser: 23 | def __init__(self, param_file) -> None: 24 | with open(param_file) as file: 25 | param = yaml.safe_load(file) 26 | 27 | self.GUI = DynamicAttributes(param["GUI"]) 28 | self.Trajectory = DynamicAttributes(param["Trajectory"]) 29 | self.Vicon = DynamicAttributes(param["Vicon"]) 30 | self.Drone = DynamicAttributes(param["Drone"]) -------------------------------------------------------------------------------- /PathGen/L1.csv: -------------------------------------------------------------------------------- 1 | 1.540398999999999852e+00,1.320082000000000200e+00 2 | 1.543049391304347706e+00,1.254956913043478561e+00 3 | 1.545699782608695561e+00,1.189831826086956923e+00 4 | 1.548350173913043193e+00,1.124706739130434840e+00 5 | 1.551000565217391047e+00,1.059581652173913202e+00 6 | 1.553650956521738902e+00,9.944565652173915637e-01 7 | 1.556301347826086534e+00,9.293314782608697033e-01 8 | 1.558951739130434611e+00,8.642063913043479539e-01 9 | 1.561602130434782243e+00,7.990813043478262045e-01 10 | 1.564252521739130319e+00,7.339562173913045662e-01 11 | 1.566902913043478396e+00,6.688311304347828168e-01 12 | 1.569553304347826028e+00,6.037060434782609564e-01 13 | 1.572203695652173661e+00,5.385809565217393180e-01 14 | 1.574854086956521737e+00,4.734558695652175686e-01 15 | 1.577504478260869369e+00,4.083307826086957082e-01 16 | 1.580154869565217002e+00,3.432056956521740143e-01 17 | 1.582805260869565078e+00,2.780806086956522649e-01 18 | 1.585455652173912933e+00,2.129555217391305433e-01 19 | 1.588106043478260787e+00,1.478304347826088772e-01 20 | 1.590756434782608641e+00,8.270534782608701674e-02 21 | 1.593406826086956496e+00,1.758026086956529510e-02 22 | 1.596057217391304128e+00,-4.754482608695641266e-02 23 | 1.598707608695651983e+00,-1.126699130434782870e-01 24 | 1.601357999999999837e+00,-1.777949999999999808e-01 25 | 1.601357999999999837e+00,-1.777949999999999808e-01 26 | 1.538633463414633917e+00,-1.772236097560975498e-01 27 | 1.475908926829267998e+00,-1.766522195121951189e-01 28 | 1.413184390243902078e+00,-1.760808292682926601e-01 29 | 1.350459853658536602e+00,-1.755094390243902291e-01 30 | 1.287735317073170682e+00,-1.749380487804877982e-01 31 | 1.225010780487804762e+00,-1.743666585365853672e-01 32 | 1.162286243902438843e+00,-1.737952682926829084e-01 33 | 1.099561707317073145e+00,-1.732238780487804775e-01 34 | 1.036837170731707225e+00,-1.726524878048780465e-01 35 | 9.741126341463413052e-01,-1.720810975609756155e-01 36 | 9.113880975609753854e-01,-1.715097073170731568e-01 37 | 8.486635609756094656e-01,-1.709383170731706980e-01 38 | 7.859390243902436568e-01,-1.703669268292682670e-01 39 | 7.232144878048777370e-01,-1.697955365853658360e-01 40 | 6.604899512195121503e-01,-1.692241463414634051e-01 41 | 5.977654146341462305e-01,-1.686527560975609741e-01 42 | 5.350408780487803106e-01,-1.680813658536585153e-01 43 | 4.723163414634146684e-01,-1.675099756097561121e-01 44 | 4.095918048780487486e-01,-1.669385853658536534e-01 45 | 3.468672682926828843e-01,-1.663671951219511946e-01 46 | 2.841427317073169645e-01,-1.657958048780487914e-01 47 | 2.214181951219510447e-01,-1.652244146341463327e-01 48 | 1.586936585365851249e-01,-1.646530243902438739e-01 49 | 9.596912195121920508e-02,-1.640816341463414707e-01 50 | 3.324458536585361834e-02,-1.635102439024390120e-01 51 | -2.947995121951230146e-02,-1.629388536585365810e-01 52 | -9.220448780487822127e-02,-1.623674634146341500e-01 53 | -1.549290243902441411e-01,-1.617960731707316913e-01 54 | -2.176535609756100054e-01,-1.612246829268292603e-01 55 | -2.803780975609756476e-01,-1.606532926829268293e-01 56 | -3.431026341463414564e-01,-1.600819024390243983e-01 57 | -4.058271707317073762e-01,-1.595105121951219673e-01 58 | -4.685517073170732960e-01,-1.589391219512195086e-01 59 | -5.312762439024392158e-01,-1.583677317073170776e-01 60 | -5.940007804878048026e-01,-1.577963414634146189e-01 61 | -6.567253170731707224e-01,-1.572249512195121879e-01 62 | -7.194498536585366422e-01,-1.566535609756097569e-01 63 | -7.821743902439025620e-01,-1.560821707317073259e-01 64 | -8.448989268292684818e-01,-1.555107804878048949e-01 65 | -9.076234634146340685e-01,-1.549393902439024362e-01 66 | -9.703479999999999883e-01,-1.543680000000000052e-01 67 | -------------------------------------------------------------------------------- /PathGen/L2.csv: -------------------------------------------------------------------------------- 1 | -9.703479999999999883e-01,-1.543680000000000052e-01 2 | -9.092070588235293682e-01,-1.551153970588235453e-01 3 | -8.480661176470588591e-01,-1.558627941176470577e-01 4 | -7.869251764705882390e-01,-1.566101911764705978e-01 5 | -7.257842352941176189e-01,-1.573575882352941102e-01 6 | -6.646432941176471099e-01,-1.581049852941176503e-01 7 | -6.035023529411764898e-01,-1.588523823529411627e-01 8 | -5.423614117647058697e-01,-1.595997794117647028e-01 9 | -4.812204705882353051e-01,-1.603471764705882152e-01 10 | -4.200795294117646850e-01,-1.610945735294117553e-01 11 | -3.589385882352941204e-01,-1.618419705882352955e-01 12 | -2.977976470588235003e-01,-1.625893676470588356e-01 13 | -2.366567058823528802e-01,-1.633367647058823202e-01 14 | -1.755157647058824821e-01,-1.640841617647058881e-01 15 | -1.143748235294118620e-01,-1.648315588235294282e-01 16 | -5.323388235294113091e-02,-1.655789558823529406e-01 17 | 7.907058823529378166e-03,-1.663263529411764807e-01 18 | 6.904799999999999827e-02,-1.670737500000000209e-01 19 | 1.301889411764706184e-01,-1.678211470588235610e-01 20 | 1.913298823529411274e-01,-1.685685441176470734e-01 21 | 2.524708235294118586e-01,-1.693159411764705857e-01 22 | 3.136117647058824787e-01,-1.700633382352941259e-01 23 | 3.747527058823529877e-01,-1.708107352941176660e-01 24 | 4.358936470588237189e-01,-1.715581323529411784e-01 25 | 4.970345882352941169e-01,-1.723055294117646907e-01 26 | 5.581755294117645150e-01,-1.730529264705882309e-01 27 | 6.193164705882351351e-01,-1.738003235294117710e-01 28 | 6.804574117647056442e-01,-1.745477205882353111e-01 29 | 7.415983529411763753e-01,-1.752951176470588512e-01 30 | 8.027392941176471064e-01,-1.760425147058823359e-01 31 | 8.638802352941176155e-01,-1.767899117647058760e-01 32 | 9.250211764705880135e-01,-1.775373088235294161e-01 33 | 9.861621176470587447e-01,-1.782847058823529562e-01 34 | 1.047303058823529476e+00,-1.790321029411764686e-01 35 | 1.108443999999999985e+00,-1.797795000000000087e-01 36 | 1.169584941176470272e+00,-1.805268970588235211e-01 37 | 1.230725882352941225e+00,-1.812742941176470612e-01 38 | 1.291866823529411512e+00,-1.820216911764706014e-01 39 | 1.353007764705882243e+00,-1.827690882352941415e-01 40 | 1.414148705882352530e+00,-1.835164852941176539e-01 41 | 1.475289647058823483e+00,-1.842638823529411662e-01 42 | 1.536430588235293992e+00,-1.850112794117647064e-01 43 | 1.597571529411764946e+00,-1.857586764705882465e-01 44 | 1.658712470588235233e+00,-1.865060735294117589e-01 45 | 1.719853411764706186e+00,-1.872534705882353268e-01 46 | 1.780994352941176473e+00,-1.880008676470588391e-01 47 | 1.842135294117647426e+00,-1.887482647058823515e-01 48 | 1.903276235294117713e+00,-1.894956617647058916e-01 49 | 1.964417176470588000e+00,-1.902430588235294040e-01 50 | 2.025558117647058509e+00,-1.909904558823529719e-01 51 | 2.086699058823529018e+00,-1.917378529411764843e-01 52 | 2.147839999999999971e+00,-1.924852499999999966e-01 53 | 2.208980941176470036e+00,-1.932326470588235368e-01 54 | 2.270121882352940990e+00,-1.939800441176470769e-01 55 | 2.331262823529411499e+00,-1.947274411764706170e-01 56 | 2.392403764705882452e+00,-1.954748382352941294e-01 57 | 2.453544705882352961e+00,-1.962222352941176418e-01 58 | 2.514685647058823470e+00,-1.969696323529411819e-01 59 | 2.575826588235293979e+00,-1.977170294117647220e-01 60 | 2.636967529411764932e+00,-1.984644264705882621e-01 61 | 2.698108470588235441e+00,-1.992118235294117745e-01 62 | 2.759249411764705950e+00,-1.999592205882353146e-01 63 | 2.820390352941176015e+00,-2.007066176470588548e-01 64 | 2.881531294117646969e+00,-2.014540147058823671e-01 65 | 2.942672235294117478e+00,-2.022014117647059073e-01 66 | 3.003813176470587987e+00,-2.029488088235294196e-01 67 | 3.064954117647058496e+00,-2.036962058823529598e-01 68 | 3.126095058823529005e+00,-2.044436029411764999e-01 69 | 3.187235999999999958e+00,-2.051910000000000123e-01 70 | 3.187235999999999958e+00,-2.051910000000000123e-01 71 | 3.156340875000000157e+00,-2.596481875000000161e-01 72 | 3.125445749999999911e+00,-3.141053750000000200e-01 73 | 3.094550625000000110e+00,-3.685625624999999683e-01 74 | 3.063655500000000309e+00,-4.230197499999999722e-01 75 | 3.032760375000000064e+00,-4.774769374999999760e-01 76 | 3.001865249999999818e+00,-5.319341250000000354e-01 77 | 2.970970125000000017e+00,-5.863913124999999837e-01 78 | 2.940074999999999772e+00,-6.408484999999999321e-01 79 | 2.909179874999999971e+00,-6.953056874999999915e-01 80 | 2.878284749999999725e+00,-7.497628749999999398e-01 81 | 2.847389624999999924e+00,-8.042200624999999992e-01 82 | 2.816494500000000123e+00,-8.586772499999999475e-01 83 | 2.785599375000000322e+00,-9.131344374999998958e-01 84 | 2.754704250000000076e+00,-9.675916249999999552e-01 85 | 2.723809124999999831e+00,-1.022048812499999793e+00 86 | 2.692914000000000030e+00,-1.076505999999999963e+00 87 | 2.662018875000000229e+00,-1.130963187499999911e+00 88 | 2.631123750000000427e+00,-1.185420375000000082e+00 89 | 2.600228624999999738e+00,-1.239877562499999808e+00 90 | 2.569333499999999937e+00,-1.294334749999999756e+00 91 | 2.538438375000000136e+00,-1.348791937499999927e+00 92 | 2.507543250000000334e+00,-1.403249124999999875e+00 93 | 2.476648125000000089e+00,-1.457706312499999823e+00 94 | 2.445753000000000288e+00,-1.512163499999999994e+00 95 | 2.414857875000000043e+00,-1.566620687499999942e+00 96 | 2.383962750000000241e+00,-1.621077874999999890e+00 97 | 2.353067624999999996e+00,-1.675535062499999839e+00 98 | 2.322172500000000195e+00,-1.729992249999999787e+00 99 | 2.291277374999999950e+00,-1.784449437499999735e+00 100 | 2.260382250000000148e+00,-1.838906624999999684e+00 101 | 2.229487124999999903e+00,-1.893363812499999854e+00 102 | 2.198592000000000102e+00,-1.947820999999999803e+00 103 | -------------------------------------------------------------------------------- /PathGen/interpolatedPath.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def generate_interpolated_points(p1, p2, num_points): 4 | p1 = np.array(p1) 5 | p2 = np.array(p2) 6 | 7 | # Generate evenly spaced values from 0 to 1 8 | t = np.linspace(0, 1, num_points) 9 | 10 | # Interpolate between p1 and p2 11 | interpolated_points = p1[np.newaxis, :] * (1 - t)[:, np.newaxis] + p2[np.newaxis, :] * t[:, np.newaxis] 12 | 13 | return interpolated_points.tolist() 14 | 15 | L1 = np.array([[1540.399, 1320.082], [1601.358, -177.795], [-970.348, -154.368]]) / 1000.0 16 | L2 = np.array([[-970.348, -154.368], [3187.236, -205.191], [2198.592, -1947.821]]) / 1000.0 17 | 18 | def main(path, V): 19 | interpolated_path = [] 20 | 21 | for i, p in enumerate(path): 22 | if i == 0: 23 | continue 24 | N = int(np.linalg.norm(path[i-1] - path[i]) / V) 25 | points = generate_interpolated_points(path[i-1], path[i], N) 26 | interpolated_path.extend(points) 27 | return interpolated_path 28 | 29 | if __name__=="__main__": 30 | dt = 0.03 # s 31 | v = 2 # m/s 32 | path = main(L2, v * dt) 33 | 34 | np.savetxt('L2.csv', path, delimiter=',') 35 | 36 | print(len(path)) 37 | 38 | # from pprint import pprint 39 | # pprint(path) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Introduction 2 | 3 | This program shows that drone can be controlled through common commecical mobile connectivity such as wibro and LTE, and it does not need any RF receiver and transmitter as well. 4 | 5 | You can get most of information related to this in [the site](http://tech.tigiminsight.com/2016/06/12/how-to-use-dronekit.html) even if those are written in Korean, because google translator can help you. :) 6 | 7 | ## Open Source List 8 | 9 | There are many open source used in this project 10 | 11 | - Raspberry Pi3 12 | - Pixhawk 13 | - PyQT 14 | - Dronekit 15 | - Mavproxy 16 | 17 | ## How to test 18 | 19 | [This video](https://www.youtube.com/watch?v=6sRNNMlCmjM) shows how it works 20 | 21 | First, power up your [drone](https://pixhawk.org/platforms/multicopters/dji_flamewheel_450) with pixhawk annd push safety button before testing 22 | 23 | On drone, companion computer also powers up, and mavproxy runs as a bridge to both app and gcs like APM Planner2 24 | 25 | mavproxy.py --udpout 127.0.0.1:14550 --tcpout 127.0.0.1:14550 26 | 27 | Also, in your laptop, you download this source code and run while gcs needs to connect, because it lets you know whole status of drone. Of course, environment settings are required as described [here](https://supertigim.github.io/2016/06/12/how-to-use-dronekit.html) 28 | 29 | ## License 30 | 31 | MIT License -------------------------------------------------------------------------------- /__pycache__/Quadrotor.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supertigim/droneController/84670adf09e1a913fbe61050ab86670ba6fbf5a3/__pycache__/Quadrotor.cpython-36.pyc -------------------------------------------------------------------------------- /arrows/down.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supertigim/droneController/84670adf09e1a913fbe61050ab86670ba6fbf5a3/arrows/down.png -------------------------------------------------------------------------------- /arrows/left.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supertigim/droneController/84670adf09e1a913fbe61050ab86670ba6fbf5a3/arrows/left.png -------------------------------------------------------------------------------- /arrows/right.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supertigim/droneController/84670adf09e1a913fbe61050ab86670ba6fbf5a3/arrows/right.png -------------------------------------------------------------------------------- /arrows/up.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supertigim/droneController/84670adf09e1a913fbe61050ab86670ba6fbf5a3/arrows/up.png -------------------------------------------------------------------------------- /backup/config.ini: -------------------------------------------------------------------------------- 1 | [DEFAULT] 2 | TAKEOFF_ALTITUDE = 1 3 | DT = 0.05 4 | CMD_VEL = 0.4 5 | 6 | [DRONEKIT] 7 | SIM = 0 8 | 9 | [Trajectory] 10 | traj_path = trajs/spiral8.csv 11 | traj_dt = 30 12 | -------------------------------------------------------------------------------- /backup/main.py: -------------------------------------------------------------------------------- 1 | from dronekit import connect, VehicleMode 2 | from pymavlink import mavutil 3 | 4 | from PyQt5.QtWidgets import QApplication, QTableWidgetItem, QMainWindow 5 | import sys 6 | from PyQt5 import uic 7 | from PyQt5.QtCore import QTimer 8 | from threading import Thread 9 | import PyQt5 10 | 11 | import configparser 12 | from enum import Enum 13 | from collections import defaultdict 14 | import time 15 | from DroneController.Quadrotor import Quadrotor 16 | import numpy as np 17 | import os 18 | import logging 19 | logging.basicConfig( 20 | format='%(asctime)s %(levelname)-8s %(message)s', 21 | level=logging.INFO, 22 | datefmt='%H:%M:%S') 23 | 24 | 25 | class Config: 26 | def __init__(self): 27 | config = configparser.ConfigParser() 28 | config.read('config.ini') 29 | self.traj_path = f"{str(config['Trajectory']['traj_path'])}" 30 | self.traj_dt = int(config['Trajectory']['traj_dt']) 31 | self.default_takeoff_alt = float(config['DEFAULT']['TAKEOFF_ALTITUDE']) 32 | self.default_cmd_vel = float(config['DEFAULT']['CMD_VEL']) 33 | self.default_dt = float(config['DEFAULT']['DT']) 34 | self.is_sim = int(config['DRONEKIT']['SIM']) 35 | assert self.is_sim in [0, 1] 36 | self.ip_addr = "0.0.0.0:18990" if self.is_sim == 0 else "" 37 | 38 | if not os.path.exists(self.traj_path): 39 | logging.error(f'{self.traj_path} does not exist!') 40 | return 41 | 42 | 43 | 44 | class State(Enum): 45 | INITIALIZED = 0 46 | ARMED = 1 47 | TAKEOFF = 2 48 | HOVER = 3 49 | LAND = 4 50 | 51 | class Action(Enum): 52 | IS_ARMABLE = 0 53 | IS_ARMED = 1 54 | HAS_ARRIVED = 2 55 | DISARMED = 3 56 | 57 | 58 | class MainWindow(QMainWindow): 59 | def __init__(self): 60 | 61 | super().__init__() 62 | uic.loadUi('window.ui', self) 63 | self.config = Config() 64 | self.state = State.LAND 65 | self.coord = np.zeros(3) 66 | self.progressBar.setValue(0) 67 | self.thread = Thread(target=self.connect, args=(self.config.ip_addr,)) 68 | self.thread.start() 69 | 70 | self.transition = defaultdict(dict) 71 | self.transition[State.INITIALIZED][Action.IS_ARMABLE] = State.ARMED 72 | self.transition[State.ARMED][Action.IS_ARMED] = State.TAKEOFF 73 | self.transition[State.TAKEOFF][Action.HAS_ARRIVED] = State.HOVER 74 | self.transition[State.LAND][Action.DISARMED] = State.INITIALIZED 75 | 76 | self.launchAlt = self.config.default_takeoff_alt 77 | self.btnLaunch.clicked.connect(self.launch_click) 78 | 79 | self.btnWest.clicked.connect(self.west_click) 80 | self.btnEast.clicked.connect(self.east_click) 81 | self.btnNorth.clicked.connect(self.north_click) 82 | self.btnSouth.clicked.connect(self.south_click) 83 | self.btnRTL.clicked.connect(self.rtl_click) 84 | 85 | self.btnUp.clicked.connect(self.up_click) 86 | self.btnDown.clicked.connect(self.down_click) 87 | self.btnSendTraj.clicked.connect(self.sendTrajectory) 88 | 89 | # intialize buttons 90 | self.btnLaunch.setEnabled(False) 91 | # self.btnSendTraj.setEnabled(False) 92 | 93 | # add visualizer 94 | self.quad = Quadrotor(size=0.5) 95 | 96 | self.viewer.addWidget(self.quad.canvas) 97 | 98 | 99 | def traj_callback(self): 100 | self.traj_index += 1 101 | if self.traj_index < len(self.traj): 102 | target = self.traj[self.traj_index][1:] 103 | vel = target - self.coord 104 | vx, vy, vz = vel 105 | logging.debug(f"[Traj] vx = {vx:.3f}, vy = {vy:.3f} ") 106 | self.publish_cmd_vel(vy, vx, -vz) 107 | elif self.traj_index - 10 < len(self.traj): 108 | self.publish_cmd_vel(0, 0, 0) 109 | else: 110 | logging.debug("trajectory timer stopped") 111 | self.traj_timer.stop() 112 | 113 | def sendTrajectory(self): 114 | 115 | if not os.path.exists(self.config.traj_path): 116 | logging.error(f'{self.config.traj_path} does not exist!') 117 | return 118 | logging.info('sending trajectory') 119 | 120 | if self.state == State.HOVER: 121 | self.traj = np.loadtxt(self.config.traj_path, delimiter=",") 122 | self.traj_timer = QTimer() 123 | self.traj_index = 0 124 | self.traj_timer.timeout.connect(self.traj_callback) 125 | self.traj_timer.start(self.config.traj_dt) 126 | 127 | def connect(self, ip_addr): 128 | self.connection_string = ip_addr 129 | self.sitl = None 130 | 131 | # Start SITL if no connection string specified 132 | if len(self.connection_string) < 3: 133 | import dronekit_sitl 134 | self.sitl = dronekit_sitl.start_default() 135 | self.connection_string = self.sitl.connection_string() 136 | 137 | # Connect to the Vehicle 138 | logging.info('Connecting to vehicle on: %s' % self.connection_string) 139 | 140 | self.vehicle = connect(self.connection_string, wait_ready=True) 141 | self.progressBar.setValue(25) 142 | 143 | # Display Flight Mode 144 | self.updateFlightModeGUI(self.vehicle.mode) 145 | self.addObserverAndInit( 146 | 'mode' 147 | , lambda vehicle, name, mode: self.updateFlightModeGUI(mode)) 148 | 149 | # Display Location Info 150 | self.updateLocationGUI(self.vehicle.location) 151 | self.addObserverAndInit( 152 | 'location' 153 | , lambda vehicle, name, location: self.updateLocationGUI(location)) 154 | 155 | # change state 156 | if self.config.is_sim == 0: 157 | self.state = State.HOVER 158 | self.vehicle.mode = VehicleMode("GUIDED") 159 | else: 160 | self.state = State.INITIALIZED 161 | self.btnLaunch.setEnabled(True) 162 | 163 | def updateFlightModeGUI(self, value): 164 | logging.info(f'flight mode change to {value}') 165 | index, mode = str(value).split(':') 166 | self.lblFlightModeValue.setText(mode) 167 | 168 | def updateLocationGUI(self, location): 169 | # self.lblLongValue.setText(str(location.global_frame.lon)) 170 | # self.lblLatValue.setText(str(location.global_frame.lat)) 171 | # location.local_frame. 172 | x = location.local_frame.east 173 | y = location.local_frame.north 174 | z = location.local_frame.down 175 | 176 | if x is None or y is None or z is None: 177 | x, y, z = 0, 0, 0 178 | z = max(-z, 0.0) 179 | logging.debug(f'location: {x}, {y}, {z}') 180 | self.coord[0] = x 181 | self.coord[1] = y 182 | self.coord[2] = z 183 | 184 | heading = np.deg2rad(45) 185 | self.lblLongValue.setText(f"{x:.4f}") 186 | self.lblLatValue.setText(f"{y:.4f}") 187 | self.lblAltValue.setText(f"{z:.4f}") 188 | 189 | self.quad.update_pose(x,y,z,0,0,heading) 190 | 191 | def addObserverAndInit(self, name, cb): 192 | """We go ahead and call our observer once at startup to get an initial value""" 193 | self.vehicle.add_attribute_listener(name, cb) 194 | 195 | def getAction(self, state:State): 196 | """get an action based on the state of the vehicle""" 197 | if state == State.INITIALIZED and self.vehicle.is_armable: 198 | return Action.IS_ARMABLE 199 | elif state == State.ARMED and self.vehicle.armed and self.vehicle.mode.name == 'GUIDED': 200 | return Action.IS_ARMED 201 | elif state == State.TAKEOFF and self.vehicle.location.global_relative_frame.alt >= self.launchAlt * 0.95: 202 | return Action.HAS_ARRIVED 203 | elif state == State.LAND and not self.vehicle.armed: 204 | return Action.DISARMED 205 | def timerCallback(self): 206 | """ 207 | complete the launch process, update the state machine 208 | """ 209 | action = self.getAction(self.state) 210 | if action is None: 211 | return 212 | 213 | self.state = self.transition[self.state][action] 214 | logging.info("[State]: {} | Action = {}".format(self.state.name, self.vehicle.system_status.state)) 215 | 216 | if self.state == State.ARMED: 217 | self.vehicle.mode = VehicleMode("GUIDED") 218 | self.vehicle.armed = True 219 | self.progressBar.setValue(50) 220 | 221 | elif self.state == State.TAKEOFF: 222 | self.vehicle.simple_takeoff(self.launchAlt) 223 | self.progressBar.setValue(75) 224 | 225 | elif self.state == State.HOVER: 226 | logging.info("vehicle reached to hovering position") 227 | self.progressBar.setValue(100) 228 | self.btnSendTraj.setEnabled(True) 229 | 230 | elif self.state == State.INITIALIZED: 231 | logging.info("vehicle landed") 232 | self.timer.stop() 233 | 234 | ############### MAV LINK communication ########################################################################## 235 | 236 | def publish_cmd_vel(self, velocity_x, velocity_y, velocity_z): 237 | msg = self.vehicle.message_factory.set_position_target_local_ned_encode( 238 | 0, # time_boot_ms (not used) 239 | 0, 0, # target system, target component 240 | mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame 241 | 0b0000111111000111, # type_mask (only speeds enabled) 242 | 0, 0, 0, # x, y, z positions (not used) 243 | velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s 244 | 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) 245 | 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 246 | self.vehicle.send_mavlink(msg) 247 | 248 | def send_ned_velocity(self, velocity_x, velocity_y, velocity_z, duration): 249 | # send command to vehicle on x Hz cycle 250 | elapsed_time = 0.0 251 | while elapsed_time < duration: 252 | self.publish_cmd_vel(velocity_x, velocity_y, velocity_z) 253 | time.sleep(self.config.default_dt) 254 | elapsed_time += self.config.default_dt 255 | 256 | 257 | ############### Joystick communication ########################################################################## 258 | def vehicle_validation(self, function): 259 | if self.vehicle.mode == "GUIDED": 260 | logging.debug('button clicked ', function.__name__) 261 | function() 262 | 263 | def west_click(self): 264 | if self.state != State.HOVER: 265 | logging.warning("[Invalid request]: moving west is not possible") 266 | return 267 | @self.vehicle_validation 268 | def west_wrapped(): 269 | self.send_ned_velocity(0, -self.config.default_cmd_vel, 0, 1) 270 | # self.send_ned_velocity(0, 0, 0, 1) 271 | 272 | def east_click(self): 273 | if self.state != State.HOVER: 274 | logging.warning("[Invalid request]: moving east is not possible") 275 | return 276 | @self.vehicle_validation 277 | def east_wrapped(): 278 | self.send_ned_velocity(0, self.config.default_cmd_vel, 0, 1) 279 | # self.send_ned_velocity(0, 0, 0, 1) 280 | 281 | def north_click(self): 282 | if self.state != State.HOVER: 283 | logging.warning("[Invalid request]: moving north is not possible") 284 | return 285 | @self.vehicle_validation 286 | def north_wrapped(): 287 | self.send_ned_velocity(self.config.default_cmd_vel, 0, 0, 1) 288 | # self.send_ned_velocity(0, 0, 0, 1) 289 | 290 | def south_click(self): 291 | if self.state != State.HOVER: 292 | logging.warning("[Invalid request]: moving south is not possible") 293 | return 294 | @self.vehicle_validation 295 | def south_wrapped(): 296 | self.send_ned_velocity(-self.config.default_cmd_vel, 0, 0, 1) 297 | # self.send_ned_velocity(0, 0, 0, 1) 298 | 299 | def rtl_click(self): 300 | if self.state == State.LAND or self.state == State.INITIALIZED: 301 | logging.warning("[Invalid request]: landing is not possible") 302 | return 303 | @self.vehicle_validation 304 | def rtl_wrapped(): 305 | self.vehicle.mode = VehicleMode("LAND") 306 | self.state = State.LAND 307 | 308 | 309 | def up_click(self): 310 | if self.state != State.HOVER: 311 | logging.warning("[Invalid request]: moving up is not possible") 312 | return 313 | @self.vehicle_validation 314 | def up_wrapped(): 315 | alt = self.vehicle.location.global_relative_frame.alt 316 | if alt < 3: 317 | self.send_ned_velocity(0, 0, -0.5 * self.config.default_cmd_vel, 1) 318 | # self.send_ned_velocity(0, 0, 0, 1) 319 | 320 | def down_click(self): 321 | if self.state != State.HOVER: 322 | logging.warning("[Invalid request]: moving down is not possible") 323 | return 324 | @self.vehicle_validation 325 | def down_wrapped(): 326 | alt = self.vehicle.location.global_relative_frame.alt 327 | if alt > 0.5: 328 | self.send_ned_velocity(0, 0, 0.5 * self.config.default_cmd_vel, 1) 329 | # self.send_ned_velocity(0, 0, 0, 1) 330 | 331 | 332 | def launch_click(self): 333 | """ 334 | Arms vehicle and fly to self.alt 335 | """ 336 | logging.debug('launch button pressed') 337 | if self.state == State.INITIALIZED: 338 | logging.info('initializeing taking off ...') 339 | self.timer = QTimer() 340 | self.timer.timeout.connect(self.timerCallback) 341 | self.timer.start(1000) 342 | else: 343 | logging.warning("launch has not been initialized !") 344 | 345 | 346 | 347 | 348 | if __name__ == '__main__': 349 | app = QApplication(sys.argv) 350 | window = MainWindow() 351 | window.show() 352 | sys.exit(app.exec()) -------------------------------------------------------------------------------- /backup/main_rosetta copy.py: -------------------------------------------------------------------------------- 1 | from dronekit import connect, VehicleMode 2 | from pymavlink import mavutil 3 | import time 4 | 5 | from PyQt5.QtWidgets import QApplication, QTableWidgetItem, QMainWindow 6 | import sys 7 | from PyQt5 import uic 8 | from PyQt5.QtCore import QTimer, QObject, pyqtSignal, pyqtSlot 9 | from threading import Thread 10 | from pyvicon_datastream import tools 11 | from queue import Queue 12 | 13 | import configparser 14 | from enum import Enum 15 | from collections import defaultdict 16 | import time 17 | from DroneController.Quadrotor import Quadrotor 18 | import numpy as np 19 | import os 20 | import logging 21 | logging.basicConfig( 22 | format='%(asctime)s %(levelname)-8s %(message)s', 23 | level=logging.INFO, 24 | datefmt='%H:%M:%S') 25 | 26 | 27 | class State(Enum): 28 | INITIALIZED = 0 29 | ARMED = 1 30 | TAKEOFF = 2 31 | HOVER = 3 32 | LAND = 4 33 | POSE_ESTIMATOR = 5 34 | VISUALIZATION = 6 35 | TRAJ_FOLLOWER = 7 36 | VICON_CONNECTED = 8 37 | VICON_DISCONNECTED = 9 38 | 39 | 40 | class Action(Enum): 41 | IS_ARMABLE = 0 42 | IS_ARMED = 1 43 | HAS_ARRIVED = 2 44 | DISARMED = 3 45 | IS_SIM = 5 46 | 47 | 48 | 49 | 50 | class FSM: 51 | def __init__(self) -> None: 52 | self.transition = defaultdict(dict) 53 | self.transition[State.INITIALIZED][Action.IS_ARMABLE] = State.ARMED 54 | self.transition[State.ARMED][Action.IS_ARMED] = State.TAKEOFF 55 | self.transition[State.TAKEOFF][Action.HAS_ARRIVED] = State.HOVER 56 | self.transition[State.LAND][Action.DISARMED] = State.INITIALIZED 57 | 58 | self.transition[State.INITIALIZED][Action.IS_SIM] = State.VISUALIZATION 59 | 60 | # self.transition[State.LAND][Action.VICON_CONNECTED] = State.POSE_ESTIMATOR 61 | # self.transition[State.POSE_ESTIMATOR][Action.VICON_CONNECTED] = State.VISUALIZATION 62 | 63 | 64 | 65 | 66 | 67 | if __name__ == '__main__': 68 | app = QApplication(sys.argv) 69 | window = MainWindow() 70 | window.show() 71 | sys.exit(app.exec()) -------------------------------------------------------------------------------- /backup/main_rosetta.py: -------------------------------------------------------------------------------- 1 | from dronekit import connect, VehicleMode 2 | from pymavlink import mavutil 3 | import time 4 | 5 | from PyQt5.QtWidgets import QApplication, QTableWidgetItem, QMainWindow 6 | import sys 7 | from PyQt5 import uic 8 | from PyQt5.QtCore import QTimer, QObject, pyqtSignal, pyqtSlot 9 | from threading import Thread 10 | from pyvicon_datastream import tools 11 | from queue import Queue 12 | 13 | import configparser 14 | from enum import Enum 15 | from collections import defaultdict 16 | import time 17 | from DroneController.Quadrotor import Quadrotor 18 | import numpy as np 19 | import os 20 | import logging 21 | logging.basicConfig( 22 | format='%(asctime)s %(levelname)-8s %(message)s', 23 | level=logging.INFO, 24 | datefmt='%H:%M:%S') 25 | 26 | 27 | class State(Enum): 28 | INITIALIZED = 0 29 | ARMED = 1 30 | TAKEOFF = 2 31 | HOVER = 3 32 | LAND = 4 33 | POSE_ESTIMATOR = 5 34 | VISUALIZATION = 6 35 | TRAJ_FOLLOWER = 7 36 | VICON_CONNECTED = 8 37 | VICON_DISCONNECTED = 9 38 | 39 | 40 | class Action(Enum): 41 | IS_ARMABLE = 0 42 | IS_ARMED = 1 43 | HAS_ARRIVED = 2 44 | DISARMED = 3 45 | IS_SIM = 5 46 | 47 | 48 | 49 | 50 | class FSM: 51 | def __init__(self) -> None: 52 | self.transition = defaultdict(dict) 53 | self.transition[State.INITIALIZED][Action.IS_ARMABLE] = State.ARMED 54 | self.transition[State.ARMED][Action.IS_ARMED] = State.TAKEOFF 55 | self.transition[State.TAKEOFF][Action.HAS_ARRIVED] = State.HOVER 56 | self.transition[State.LAND][Action.DISARMED] = State.INITIALIZED 57 | 58 | self.transition[State.INITIALIZED][Action.IS_SIM] = State.VISUALIZATION 59 | 60 | # self.transition[State.LAND][Action.VICON_CONNECTED] = State.POSE_ESTIMATOR 61 | # self.transition[State.POSE_ESTIMATOR][Action.VICON_CONNECTED] = State.VISUALIZATION 62 | 63 | 64 | class ViconClient(QObject): 65 | pose_signal = pyqtSignal(dict) 66 | 67 | def __init__(self, parent: QObject) -> None: 68 | super().__init__(parent) 69 | self.tracker = tools.ObjectTracker("192.168.10.2") 70 | self.obj_name = "djimini" 71 | self.vicon_timer = QTimer() 72 | self.state = State.VICON_CONNECTED if self.vicon_timer.timeout.connect(self.pose_callback) else State.VICON_DISCONNECTED 73 | logging.error('vicon cannot be connected') 74 | self.connection_ressolve() 75 | 76 | 77 | 78 | def pose_callback(self): 79 | 80 | self.state = State.VICON_CONNECTED if self.tracker.is_connected else State.VICON_DISCONNECTED 81 | if self.state == State.VICON_DISCONNECTED: 82 | self.connection_ressolve() 83 | return 84 | 85 | latency, frameno, position = self.tracker.get_position(self.obj_name) 86 | if position != []: 87 | xyz_position = position[0][2:5] # get x,y,z only 88 | orientation = position[0][7] # get rotation around z axis 89 | data = {"position": np.array(xyz_position) / 1000.0, "orientation" : orientation} 90 | self.pose_signal.emit(data) 91 | 92 | def connection_ressolve(self): 93 | while self.state == State.VICON_DISCONNECTED: 94 | logging.warn('waiting for vicon to connect ...[!]') 95 | self.state = State.VICON_CONNECTED if self.vicon_timer.timeout.connect(self.pose_callback) else State.VICON_DISCONNECTED 96 | time.sleep(1) 97 | logging.info('vicon connected ...') 98 | self.vicon_timer.start(10) 99 | 100 | 101 | class djiDrone(QObject): 102 | updateFlightModeGUI = pyqtSignal(VehicleMode) 103 | 104 | def __init__(self, parent: QObject) -> None: 105 | super().__init__(parent) 106 | self.state = State.LAND 107 | self.default_cmd_vel = 0.4 108 | self.default_dt = 0.05 109 | self.traj_dt = 300 110 | 111 | def mavlink_connect(self, connection_string): 112 | self.vehicle = connect(connection_string, wait_ready=True) 113 | self.state = State.HOVER 114 | self.vehicle.mode = VehicleMode("GUIDED") 115 | # Display Flight Mode 116 | self.updateFlightModeGUI.emit(self.vehicle.mode) 117 | self.addObserverAndInit( 118 | 'mode' 119 | , lambda vehicle, name, mode: self.updateFlightModeGUI.emit(mode)) 120 | 121 | def addObserverAndInit(self, name, cb): 122 | """We go ahead and call our observer once at startup to get an initial value""" 123 | self.vehicle.add_attribute_listener(name, cb) 124 | 125 | 126 | 127 | ############### MAV LINK communication ########################################################################## 128 | def publish_cmd_vel(self, velocity_x, velocity_y, velocity_z): 129 | msg = self.vehicle.message_factory.set_position_target_local_ned_encode( 130 | 0, # time_boot_ms (not used) 131 | 0, 0, # target system, target component 132 | mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame 133 | 0b0000111111000111, # type_mask (only speeds enabled) 134 | 0, 0, 0, # x, y, z positions (not used) 135 | velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s 136 | 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) 137 | 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 138 | self.vehicle.send_mavlink(msg) 139 | 140 | def send_ned_velocity(self, velocity_x, velocity_y, velocity_z, duration): 141 | # send command to vehicle on x Hz cycle 142 | elapsed_time = 0.0 143 | while elapsed_time < duration: 144 | self.publish_cmd_vel(velocity_x, velocity_y, velocity_z) 145 | time.sleep(self.default_dt) 146 | elapsed_time += self.default_dt 147 | ############### Joystick communication ########################################################################## 148 | def vehicle_validation(self, function): 149 | if self.vehicle.mode == "GUIDED": 150 | logging.info('button clicked ', function.__name__) 151 | function() 152 | @pyqtSlot() 153 | def west_click(self): 154 | if self.state != State.HOVER: 155 | logging.warning("[Invalid request]: moving west is not possible") 156 | return 157 | @self.vehicle_validation 158 | def west_wrapped(): 159 | self.send_ned_velocity(0, self.default_cmd_vel, 0, 1) 160 | # self.send_ned_velocity(0, 0, 0, 1) 161 | @pyqtSlot() 162 | def east_click(self): 163 | if self.state != State.HOVER: 164 | logging.warning("[Invalid request]: moving east is not possible") 165 | return 166 | @self.vehicle_validation 167 | def east_wrapped(): 168 | self.send_ned_velocity(0, -self.default_cmd_vel, 0, 1) 169 | # self.send_ned_velocity(0, 0, 0, 1) 170 | 171 | @pyqtSlot() 172 | def north_click(self): 173 | if self.state != State.HOVER: 174 | logging.warning("[Invalid request]: moving north is not possible") 175 | return 176 | @self.vehicle_validation 177 | def north_wrapped(): 178 | self.send_ned_velocity(-self.default_cmd_vel, 0, 0, 1) 179 | # self.send_ned_velocity(0, 0, 0, 1) 180 | @pyqtSlot() 181 | def south_click(self): 182 | if self.state != State.HOVER: 183 | logging.warning("[Invalid request]: moving south is not possible") 184 | return 185 | @self.vehicle_validation 186 | def south_wrapped(): 187 | self.send_ned_velocity(self.default_cmd_vel, 0, 0, 1) 188 | # self.send_ned_velocity(0, 0, 0, 1) 189 | 190 | @pyqtSlot() 191 | def rtl_click(self): 192 | if self.state == State.LAND or self.state == State.INITIALIZED: 193 | logging.warning("[Invalid request]: landing is not possible") 194 | return 195 | @self.vehicle_validation 196 | def rtl_wrapped(): 197 | self.vehicle.mode = VehicleMode("LAND") 198 | self.state = State.LAND 199 | 200 | @pyqtSlot() 201 | def up_click(self): 202 | if self.state != State.HOVER: 203 | logging.warning("[Invalid request]: moving up is not possible") 204 | return 205 | @self.vehicle_validation 206 | def up_wrapped(): 207 | alt = self.vehicle.location.global_relative_frame.alt 208 | if alt < 3: 209 | self.send_ned_velocity(0, 0, -0.5 * self.default_cmd_vel, 1) 210 | # self.send_ned_velocity(0, 0, 0, 1) 211 | 212 | @pyqtSlot() 213 | def down_click(self): 214 | if self.state != State.HOVER: 215 | logging.warning("[Invalid request]: moving down is not possible") 216 | return 217 | @self.vehicle_validation 218 | def down_wrapped(): 219 | alt = self.vehicle.location.global_relative_frame.alt 220 | if alt > 0.5: 221 | self.send_ned_velocity(0, 0, 0.5 * self.default_cmd_vel, 1) 222 | # self.send_ned_velocity(0, 0, 0, 1) 223 | 224 | def traj_callback(self): 225 | if self.state != State.HOVER: 226 | logging.info("trajectory timer canceled") 227 | self.traj_timer.stop() 228 | 229 | self.traj_index += 1 230 | if self.traj_index < len(self.traj): 231 | target = self.traj[self.traj_index][1:] 232 | vel = target - self.coord 233 | vx, vy, vz = vel 234 | logging.debug(f"[Traj] vx = {vx:.3f}, vy = {vy:.3f} ") 235 | self.publish_cmd_vel(-vy, -vx, 0.0) 236 | 237 | 238 | elif self.traj_index - 10 < len(self.traj): 239 | self.publish_cmd_vel(0, 0, 0) 240 | else: 241 | logging.debug("trajectory timer stopped") 242 | self.traj_timer.stop() 243 | 244 | @pyqtSlot() 245 | def sendTrajectory(self): 246 | 247 | if not os.path.exists(self.config.traj_path): 248 | logging.error(f'{self.config.traj_path} does not exist!') 249 | return 250 | logging.info('sending trajectory') 251 | 252 | if self.state == State.HOVER: 253 | self.traj = np.loadtxt(self.config.traj_path, delimiter=",") 254 | self.traj_timer = QTimer() 255 | self.traj_index = 0 256 | self.traj_timer.timeout.connect(self.traj_callback) 257 | self.traj_timer.start(self.traj_dt) 258 | 259 | 260 | 261 | 262 | 263 | class MainWindow(QMainWindow): 264 | def __init__(self): 265 | super().__init__() 266 | uic.loadUi('window.ui', self) 267 | self.coord = np.zeros(4) 268 | 269 | # add visualizer 270 | self.quad = Quadrotor(size=0.5) 271 | self.viewer.addWidget(self.quad.canvas) 272 | 273 | 274 | self.vicon = ViconClient(self) 275 | self.vicon.pose_signal.connect(self.pose_callback) 276 | 277 | # viz timer 278 | self.viz_timer = QTimer() 279 | self.viz_timer.timeout.connect(self.updatePoseGUI) 280 | self.viz_timer.start(30) 281 | 282 | 283 | # establish mavlink connection 284 | self.dji = djiDrone(self) 285 | self.dji.updateFlightModeGUI.connect(self.updateFlightModeGUI) 286 | 287 | self.dji.mavlink_connect("0.0.0.0:18990") 288 | self.btnWest.clicked.connect(self.dji.west_click) 289 | self.btnEast.clicked.connect(self.dji.east_click) 290 | self.btnNorth.clicked.connect(self.dji.north_click) 291 | self.btnSouth.clicked.connect(self.dji.south_click) 292 | self.btnRTL.clicked.connect(self.dji.rtl_click) 293 | 294 | self.btnUp.clicked.connect(self.dji.up_click) 295 | self.btnDown.clicked.connect(self.dji.down_click) 296 | self.btnSendTraj.clicked.connect(self.dji.sendTrajectory) 297 | 298 | @pyqtSlot(dict) 299 | def pose_callback(self, data): 300 | posistion = data['position'] 301 | yaw = data['orientation'] 302 | 303 | x, y, z = posistion 304 | 305 | 306 | self.coord[0] = x 307 | self.coord[1] = y 308 | self.coord[2] = z 309 | self.coord[3] = yaw 310 | 311 | 312 | self.lblLongValue.setText(f"{x:.4f}") 313 | self.lblLatValue.setText(f"{y:.4f}") 314 | self.lblAltValue.setText(f"{z:.4f}") 315 | logging.debug(f'location: {x}, {y}, {z}') 316 | 317 | @pyqtSlot(VehicleMode) 318 | def updateFlightModeGUI(self, value): 319 | logging.info(f'flight mode change to {value}') 320 | index, mode = str(value).split(':') 321 | self.lblFlightModeValue.setText(mode) 322 | self.progressBar.setValue(100) 323 | 324 | def updatePoseGUI(self): 325 | heading = np.deg2rad(45) + self.coord[3] 326 | self.quad.update_pose( self.coord[0], self.coord[1], self.coord[2],0,0, heading) 327 | 328 | 329 | if __name__ == '__main__': 330 | app = QApplication(sys.argv) 331 | window = MainWindow() 332 | window.show() 333 | sys.exit(app.exec()) -------------------------------------------------------------------------------- /backup/main_vicon.py: -------------------------------------------------------------------------------- 1 | from dronekit import connect, VehicleMode 2 | from pymavlink import mavutil 3 | 4 | from PyQt5.QtWidgets import QApplication, QTableWidgetItem, QMainWindow 5 | import sys 6 | from PyQt5 import uic 7 | from PyQt5.QtCore import QTimer 8 | from threading import Thread 9 | from pyvicon_datastream import tools 10 | from queue import Queue 11 | 12 | import configparser 13 | from enum import Enum 14 | from collections import defaultdict 15 | import time 16 | from DroneController.Quadrotor import Quadrotor 17 | import numpy as np 18 | import os 19 | import logging 20 | logging.basicConfig( 21 | format='%(asctime)s %(levelname)-8s %(message)s', 22 | level=logging.INFO, 23 | datefmt='%H:%M:%S') 24 | 25 | 26 | class Config: 27 | def __init__(self): 28 | config = configparser.ConfigParser() 29 | config.read('config.ini') 30 | self.traj_path = f"{str(config['Trajectory']['traj_path'])}" 31 | self.traj_dt = int(config['Trajectory']['traj_dt']) 32 | self.default_takeoff_alt = float(config['DEFAULT']['TAKEOFF_ALTITUDE']) 33 | self.default_cmd_vel = float(config['DEFAULT']['CMD_VEL']) 34 | self.default_dt = float(config['DEFAULT']['DT']) 35 | self.is_sim = int(config['DRONEKIT']['SIM']) 36 | assert self.is_sim in [0, 1] 37 | self.ip_addr = "0.0.0.0:18990" if self.is_sim == 0 else "" 38 | 39 | if not os.path.exists(self.traj_path): 40 | logging.error(f'{self.traj_path} does not exist!') 41 | return 42 | 43 | 44 | 45 | class State(Enum): 46 | INITIALIZED = 0 47 | ARMED = 1 48 | TAKEOFF = 2 49 | HOVER = 3 50 | LAND = 4 51 | 52 | class Action(Enum): 53 | IS_ARMABLE = 0 54 | IS_ARMED = 1 55 | HAS_ARRIVED = 2 56 | DISARMED = 3 57 | 58 | 59 | class MainWindow(QMainWindow): 60 | def __init__(self): 61 | 62 | super().__init__() 63 | uic.loadUi('window.ui', self) 64 | self.config = Config() 65 | self.state = State.LAND 66 | self.coord = np.zeros(4) 67 | self.progressBar.setValue(0) 68 | self.thread = Thread(target=self.connect, args=(self.config.ip_addr,)) 69 | self.thread.start() 70 | 71 | self.transition = defaultdict(dict) 72 | self.transition[State.INITIALIZED][Action.IS_ARMABLE] = State.ARMED 73 | self.transition[State.ARMED][Action.IS_ARMED] = State.TAKEOFF 74 | self.transition[State.TAKEOFF][Action.HAS_ARRIVED] = State.HOVER 75 | self.transition[State.LAND][Action.DISARMED] = State.INITIALIZED 76 | 77 | self.launchAlt = self.config.default_takeoff_alt 78 | self.btnLaunch.clicked.connect(self.launch_click) 79 | 80 | self.btnWest.clicked.connect(self.west_click) 81 | self.btnEast.clicked.connect(self.east_click) 82 | self.btnNorth.clicked.connect(self.north_click) 83 | self.btnSouth.clicked.connect(self.south_click) 84 | self.btnRTL.clicked.connect(self.rtl_click) 85 | 86 | self.btnUp.clicked.connect(self.up_click) 87 | self.btnDown.clicked.connect(self.down_click) 88 | self.btnSendTraj.clicked.connect(self.sendTrajectory) 89 | 90 | # intialize buttons 91 | self.btnLaunch.setEnabled(False) 92 | # self.btnSendTraj.setEnabled(False) 93 | 94 | # add visualizer 95 | self.quad = Quadrotor(size=0.5) 96 | 97 | self.viewer.addWidget(self.quad.canvas) 98 | 99 | # add vicon clied 100 | self.tracker = tools.ObjectTracker("192.168.10.2") 101 | self.obj_name = "djimini" 102 | 103 | self.vicon_timer = QTimer() 104 | self.vicon_timer.timeout.connect(self.pose_callback) 105 | self.vicon_timer.start(1) 106 | 107 | self.viz_timer = QTimer() 108 | self.viz_timer.timeout.connect(self.updatePoseGUI) 109 | self.viz_timer.start(100) 110 | 111 | 112 | def pose_callback(self): 113 | 114 | latency, frameno, position = self.tracker.get_position(self.obj_name) 115 | if position != []: 116 | xyz_position = position[0][2:5] # get x,y,z only 117 | orientation = position[0][7] # get rotation around z axis 118 | data = {"position": np.array(xyz_position) / 1000.0, "orientation" : orientation} 119 | 120 | posistion = data['position'] 121 | yaw = data['orientation'] 122 | 123 | x, y, z = posistion 124 | 125 | 126 | self.coord[0] = -x 127 | self.coord[1] = y 128 | self.coord[2] = z 129 | self.coord[3] = yaw 130 | 131 | 132 | self.lblLongValue.setText(f"{x:.4f}") 133 | self.lblLatValue.setText(f"{y:.4f}") 134 | self.lblAltValue.setText(f"{z:.4f}") 135 | logging.debug(f'location: {x}, {y}, {z}') 136 | 137 | def traj_callback(self): 138 | self.traj_index += 1 139 | if self.traj_index < len(self.traj): 140 | target = self.traj[self.traj_index][1:] 141 | vel = target - self.coord 142 | vx, vy, vz = vel 143 | logging.debug(f"[Traj] vx = {vx:.3f}, vy = {vy:.3f} ") 144 | self.publish_cmd_vel(vy, vx, -vz) 145 | elif self.traj_index - 10 < len(self.traj): 146 | self.publish_cmd_vel(0, 0, 0) 147 | else: 148 | logging.debug("trajectory timer stopped") 149 | self.traj_timer.stop() 150 | 151 | def sendTrajectory(self): 152 | 153 | if not os.path.exists(self.config.traj_path): 154 | logging.error(f'{self.config.traj_path} does not exist!') 155 | return 156 | logging.info('sending trajectory') 157 | 158 | if self.state == State.HOVER: 159 | self.traj = np.loadtxt(self.config.traj_path, delimiter=",") 160 | self.traj_timer = QTimer() 161 | self.traj_index = 0 162 | self.traj_timer.timeout.connect(self.traj_callback) 163 | self.traj_timer.start(self.config.traj_dt) 164 | 165 | def connect(self, ip_addr): 166 | self.connection_string = ip_addr 167 | self.sitl = None 168 | 169 | # Start SITL if no connection string specified 170 | if len(self.connection_string) < 3: 171 | import dronekit_sitl 172 | self.sitl = dronekit_sitl.start_default() 173 | self.connection_string = self.sitl.connection_string() 174 | 175 | # Connect to the Vehicle 176 | logging.info('Connecting to vehicle on: %s' % self.connection_string) 177 | 178 | self.vehicle = connect(self.connection_string, wait_ready=True) 179 | self.progressBar.setValue(25) 180 | 181 | # Display Flight Mode 182 | self.updateFlightModeGUI(self.vehicle.mode) 183 | self.addObserverAndInit( 184 | 'mode' 185 | , lambda vehicle, name, mode: self.updateFlightModeGUI(mode)) 186 | 187 | # Display Location Info 188 | self.updateLocationGUI(self.vehicle.location) 189 | self.addObserverAndInit( 190 | 'location' 191 | , lambda vehicle, name, location: self.updateLocationGUI(location)) 192 | 193 | # change state 194 | if self.config.is_sim == 0: 195 | self.state = State.HOVER 196 | self.vehicle.mode = VehicleMode("GUIDED") 197 | else: 198 | self.state = State.INITIALIZED 199 | self.btnLaunch.setEnabled(True) 200 | 201 | def updateFlightModeGUI(self, value): 202 | logging.info(f'flight mode change to {value}') 203 | index, mode = str(value).split(':') 204 | self.lblFlightModeValue.setText(mode) 205 | 206 | 207 | def updatePoseGUI(self): 208 | heading = np.deg2rad(45) + self.coord[3] 209 | self.quad.update_pose( self.coord[0], self.coord[1], self.coord[2],0,0, heading) 210 | 211 | 212 | 213 | 214 | 215 | 216 | def updateLocationGUI(self, location): 217 | # self.lblLongValue.setText(str(location.global_frame.lon)) 218 | # self.lblLatValue.setText(str(location.global_frame.lat)) 219 | # location.local_frame. 220 | x = location.local_frame.east 221 | y = location.local_frame.north 222 | z = location.local_frame.down 223 | 224 | if x is None or y is None or z is None: 225 | x, y, z = 0, 0, 0 226 | z = max(-z, 0.0) 227 | logging.debug(f'location: {x}, {y}, {z}') 228 | self.coord[0] = x 229 | self.coord[1] = y 230 | self.coord[2] = z 231 | 232 | heading = np.deg2rad(45) 233 | self.lblLongValue.setText(f"{x:.4f}") 234 | self.lblLatValue.setText(f"{y:.4f}") 235 | self.lblAltValue.setText(f"{z:.4f}") 236 | 237 | # self.quad.update_pose(x,y,z,0,0,heading) 238 | 239 | def addObserverAndInit(self, name, cb): 240 | """We go ahead and call our observer once at startup to get an initial value""" 241 | self.vehicle.add_attribute_listener(name, cb) 242 | 243 | def getAction(self, state:State): 244 | """get an action based on the state of the vehicle""" 245 | if state == State.INITIALIZED and self.vehicle.is_armable: 246 | return Action.IS_ARMABLE 247 | elif state == State.ARMED and self.vehicle.armed and self.vehicle.mode.name == 'GUIDED': 248 | return Action.IS_ARMED 249 | elif state == State.TAKEOFF and self.vehicle.location.global_relative_frame.alt >= self.launchAlt * 0.95: 250 | return Action.HAS_ARRIVED 251 | elif state == State.LAND and not self.vehicle.armed: 252 | return Action.DISARMED 253 | def timerCallback(self): 254 | """ 255 | complete the launch process, update the state machine 256 | """ 257 | action = self.getAction(self.state) 258 | if action is None: 259 | return 260 | 261 | self.state = self.transition[self.state][action] 262 | logging.info("[State]: {} | Action = {}".format(self.state.name, self.vehicle.system_status.state)) 263 | 264 | if self.state == State.ARMED: 265 | self.vehicle.mode = VehicleMode("GUIDED") 266 | self.vehicle.armed = True 267 | self.progressBar.setValue(50) 268 | 269 | elif self.state == State.TAKEOFF: 270 | self.vehicle.simple_takeoff(self.launchAlt) 271 | self.progressBar.setValue(75) 272 | 273 | elif self.state == State.HOVER: 274 | logging.info("vehicle reached to hovering position") 275 | self.progressBar.setValue(100) 276 | self.btnSendTraj.setEnabled(True) 277 | 278 | elif self.state == State.INITIALIZED: 279 | logging.info("vehicle landed") 280 | self.timer.stop() 281 | 282 | ############### MAV LINK communication ########################################################################## 283 | 284 | def publish_cmd_vel(self, velocity_x, velocity_y, velocity_z): 285 | msg = self.vehicle.message_factory.set_position_target_local_ned_encode( 286 | 0, # time_boot_ms (not used) 287 | 0, 0, # target system, target component 288 | mavutil.mavlink.MAV_FRAME_LOCAL_NED, # frame 289 | 0b0000111111000111, # type_mask (only speeds enabled) 290 | 0, 0, 0, # x, y, z positions (not used) 291 | velocity_x, velocity_y, velocity_z, # x, y, z velocity in m/s 292 | 0, 0, 0, # x, y, z acceleration (not supported yet, ignored in GCS_Mavlink) 293 | 0, 0) # yaw, yaw_rate (not supported yet, ignored in GCS_Mavlink) 294 | self.vehicle.send_mavlink(msg) 295 | 296 | def send_ned_velocity(self, velocity_x, velocity_y, velocity_z, duration): 297 | # send command to vehicle on x Hz cycle 298 | elapsed_time = 0.0 299 | while elapsed_time < duration: 300 | self.publish_cmd_vel(velocity_x, velocity_y, velocity_z) 301 | time.sleep(self.config.default_dt) 302 | elapsed_time += self.config.default_dt 303 | 304 | 305 | ############### Joystick communication ########################################################################## 306 | def vehicle_validation(self, function): 307 | if self.vehicle.mode == "GUIDED": 308 | logging.debug('button clicked ', function.__name__) 309 | function() 310 | 311 | def west_click(self): 312 | if self.state != State.HOVER: 313 | logging.warning("[Invalid request]: moving west is not possible") 314 | return 315 | @self.vehicle_validation 316 | def west_wrapped(): 317 | self.send_ned_velocity(0, -self.config.default_cmd_vel, 0, 1) 318 | # self.send_ned_velocity(0, 0, 0, 1) 319 | 320 | def east_click(self): 321 | if self.state != State.HOVER: 322 | logging.warning("[Invalid request]: moving east is not possible") 323 | return 324 | @self.vehicle_validation 325 | def east_wrapped(): 326 | self.send_ned_velocity(0, self.config.default_cmd_vel, 0, 1) 327 | # self.send_ned_velocity(0, 0, 0, 1) 328 | 329 | def north_click(self): 330 | if self.state != State.HOVER: 331 | logging.warning("[Invalid request]: moving north is not possible") 332 | return 333 | @self.vehicle_validation 334 | def north_wrapped(): 335 | self.send_ned_velocity(self.config.default_cmd_vel, 0, 0, 1) 336 | # self.send_ned_velocity(0, 0, 0, 1) 337 | 338 | def south_click(self): 339 | if self.state != State.HOVER: 340 | logging.warning("[Invalid request]: moving south is not possible") 341 | return 342 | @self.vehicle_validation 343 | def south_wrapped(): 344 | self.send_ned_velocity(-self.config.default_cmd_vel, 0, 0, 1) 345 | # self.send_ned_velocity(0, 0, 0, 1) 346 | 347 | def rtl_click(self): 348 | if self.state == State.LAND or self.state == State.INITIALIZED: 349 | logging.warning("[Invalid request]: landing is not possible") 350 | return 351 | @self.vehicle_validation 352 | def rtl_wrapped(): 353 | self.vehicle.mode = VehicleMode("LAND") 354 | self.state = State.LAND 355 | 356 | 357 | def up_click(self): 358 | if self.state != State.HOVER: 359 | logging.warning("[Invalid request]: moving up is not possible") 360 | return 361 | @self.vehicle_validation 362 | def up_wrapped(): 363 | alt = self.vehicle.location.global_relative_frame.alt 364 | if alt < 3: 365 | self.send_ned_velocity(0, 0, -0.5 * self.config.default_cmd_vel, 1) 366 | # self.send_ned_velocity(0, 0, 0, 1) 367 | 368 | def down_click(self): 369 | if self.state != State.HOVER: 370 | logging.warning("[Invalid request]: moving down is not possible") 371 | return 372 | @self.vehicle_validation 373 | def down_wrapped(): 374 | alt = self.vehicle.location.global_relative_frame.alt 375 | if alt > 0.5: 376 | self.send_ned_velocity(0, 0, 0.5 * self.config.default_cmd_vel, 1) 377 | # self.send_ned_velocity(0, 0, 0, 1) 378 | 379 | 380 | def launch_click(self): 381 | """ 382 | Arms vehicle and fly to self.alt 383 | """ 384 | logging.debug('launch button pressed') 385 | if self.state == State.INITIALIZED: 386 | logging.info('initializeing taking off ...') 387 | self.timer = QTimer() 388 | self.timer.timeout.connect(self.timerCallback) 389 | self.timer.start(1000) 390 | else: 391 | logging.warning("launch has not been initialized !") 392 | 393 | 394 | 395 | 396 | if __name__ == '__main__': 397 | app = QApplication(sys.argv) 398 | window = MainWindow() 399 | window.show() 400 | sys.exit(app.exec()) -------------------------------------------------------------------------------- /backup/vicon.py: -------------------------------------------------------------------------------- 1 | import time 2 | from queue import Queue 3 | from threading import Thread 4 | from pyvicon_datastream import tools 5 | import numpy as np 6 | 7 | 8 | class ViconClient(Thread): 9 | 10 | def __init__(self, buffer:dict, OBJECT_NAME:str = "djimini", VICON_TRACKER_IP:str = "192.168.10.2") -> None: 11 | self.tracker = tools.ObjectTracker(VICON_TRACKER_IP) 12 | self.obj_name = OBJECT_NAME 13 | self.buffer = buffer 14 | Thread.__init__(self) 15 | 16 | def run(self): 17 | while(True): 18 | latency, frameno, position = self.tracker.get_position(self.obj_name) 19 | if position != []: 20 | xyz_position = position[0][2:5] # get x,y,z only 21 | orientation = position[0][7] # get rotation around z axis 22 | self.buffer = {"position": np.array(xyz_position) / 1000.0, "orientation" : orientation} 23 | 24 | time.sleep(0.01) 25 | 26 | 27 | 28 | if __name__ == "__main__": 29 | pose = Queue() 30 | 31 | vicon = ViconClient(pose) 32 | vicon.start() 33 | 34 | while True: 35 | try: 36 | if not pose.empty(): 37 | print(pose.get()) 38 | except KeyboardInterrupt: 39 | break 40 | -------------------------------------------------------------------------------- /backup/window_v0.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MainWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 400 10 | 279 11 | 12 | 13 | 14 | Drone Controller 15 | 16 | 17 | 18 | 19 | 20 | 100 21 | 180 22 | 113 23 | 32 24 | 25 | 26 | 27 | Go West 28 | 29 | 30 | 31 | 32 | 33 | 260 34 | 180 35 | 113 36 | 32 37 | 38 | 39 | 40 | Go East 41 | 42 | 43 | 44 | 45 | 46 | 180 47 | 150 48 | 113 49 | 32 50 | 51 | 52 | 53 | Go North 54 | 55 | 56 | 57 | 58 | 59 | 180 60 | 210 61 | 113 62 | 32 63 | 64 | 65 | 66 | Go South 67 | 68 | 69 | 70 | 71 | 72 | 210 73 | 180 74 | 51 75 | 32 76 | 77 | 78 | 79 | RTL 80 | 81 | 82 | 83 | 84 | 85 | 54 86 | 100 87 | 71 88 | 16 89 | 90 | 91 | 92 | Altitude : 93 | 94 | 95 | 96 | 97 | 98 | 20 99 | 150 100 | 71 101 | 31 102 | 103 | 104 | 105 | UP 106 | 107 | 108 | 109 | 110 | 111 | 20 112 | 170 113 | 71 114 | 32 115 | 116 | 117 | 118 | DOWN 119 | 120 | 121 | 122 | 123 | 124 | 130 125 | 100 126 | 161 127 | 16 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 54 138 | 50 139 | 71 140 | 16 141 | 142 | 143 | 144 | Latitude : 145 | 146 | 147 | 148 | 149 | 150 | 54 151 | 70 152 | 71 153 | 16 154 | 155 | 156 | 157 | Longitude : 158 | 159 | 160 | 161 | 162 | 163 | 130 164 | 50 165 | 211 166 | 16 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 130 177 | 70 178 | 211 179 | 16 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 84 190 | 10 191 | 91 192 | 16 193 | 194 | 195 | 196 | Flight Mode : 197 | 198 | 199 | 200 | 201 | 202 | 170 203 | 10 204 | 151 205 | 16 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 20 216 | 206 217 | 71 218 | 32 219 | 220 | 221 | 222 | Launch 223 | 224 | 225 | 226 | 227 | 228 | 29 229 | 40 230 | 341 231 | 91 232 | 233 | 234 | 235 | QFrame::StyledPanel 236 | 237 | 238 | QFrame::Raised 239 | 240 | 241 | 242 | 243 | 244 | 245 | 0 246 | 0 247 | 400 248 | 22 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | slot1() 258 | 259 | 260 | -------------------------------------------------------------------------------- /conda_init.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/bash 2 | 3 | source ~/anaconda3/etc/profile.d/conda.sh && conda activate droneController 4 | 5 | /home/airlab/anaconda3/condabin/conda 6 | -------------------------------------------------------------------------------- /drone.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/supertigim/droneController/84670adf09e1a913fbe61050ab86670ba6fbf5a3/drone.png -------------------------------------------------------------------------------- /param copy.yaml: -------------------------------------------------------------------------------- 1 | GUI: 2 | ui_path: "/home/airlab/PyDev/droneController/window.ui" 3 | viz_dt: 30 4 | 5 | 6 | Trajectory: 7 | traj_path: "/home/airlab/PyDev/droneController/trajs/spiral8.csv" 8 | traj_dt: 2 9 | fence: [-2, 2, -2, 2] 10 | 11 | Vicon: 12 | IP_ADDR: "192.168.10.2" 13 | obj_name: "mavicair1" # "djimini" 14 | vicon_dt: 1 15 | 16 | Drone: 17 | mavlink: "0.0.0.0:18990" 18 | default_cmd_vel: 0.4 19 | default_dt: 0.05 20 | 21 | -------------------------------------------------------------------------------- /param.yaml: -------------------------------------------------------------------------------- 1 | GUI: 2 | ui_path: "/home/airlab/PyDev/droneController/window.ui" 3 | viz_dt: 30 4 | 5 | 6 | Trajectory: 7 | traj_path: "/home/airlab/PyDev/droneController/trajs/spiral85.csv" 8 | traj_dt: 2 9 | fence: [-3, 3, -3, 3] 10 | 11 | Vicon: 12 | IP_ADDR: "192.168.10.2" 13 | obj_name: "djimini" # "djimini" mavicair1 14 | vicon_dt: 1 15 | 16 | Drone: 17 | mavlink: "0.0.0.0:18990" 18 | default_cmd_vel: 0.4 19 | default_dt: 0.05 20 | 21 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pymavlink==2.4.8 2 | dronekit 3 | dronekit-sitl 4 | pyqt5 5 | matplotlib 6 | pyvicon-datastream 7 | pyaml -------------------------------------------------------------------------------- /run.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/bash 2 | 3 | PYTHON="/home/airlab/anaconda3/envs/droneController/bin/python" 4 | cd "/home/airlab/PyDev/droneController" 5 | $PYTHON workspace.py -------------------------------------------------------------------------------- /trajs/.~lock.traj_2obs_vicon.csv#: -------------------------------------------------------------------------------- 1 | ,airlab,airlab-HP-EliteDesk-800-G3-TWR,12.08.2024 13:38,file:///home/airlab/.config/libreoffice/4; -------------------------------------------------------------------------------- /trajs/.~lock.traj_L_vicon.csv#: -------------------------------------------------------------------------------- 1 | ,airlab,airlab-HP-EliteDesk-800-G3-TWR,12.08.2024 14:02,file:///home/airlab/.config/libreoffice/4; -------------------------------------------------------------------------------- /trajs/interpolate.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from matplotlib import pyplot as plt 3 | 4 | def interpolate_2d(p1, p2, distance): 5 | """ 6 | Linearly interpolate between two 2D points based on distance. 7 | 8 | Parameters: 9 | p1 (tuple): First point (x1, y1) 10 | p2 (tuple): Second point (x2, y2) 11 | distance (float): Desired distance from p1 towards p2 12 | 13 | Returns: 14 | tuple: Interpolated point (x, y) 15 | """ 16 | x1, y1 = p1 17 | x2, y2 = p2 18 | 19 | # Calculate total distance between points 20 | total_distance = np.sqrt((x2 - x1)**2 + (y2 - y1)**2) 21 | 22 | # Calculate interpolation factor 23 | t = distance / total_distance 24 | 25 | # Ensure t is between 0 and 1 26 | t = max(0, min(1, t)) 27 | 28 | # Interpolate x and y coordinates 29 | x = x1 + t * (x2 - x1) 30 | y = y1 + t * (y2 - y1) 31 | 32 | return (x, y) 33 | 34 | 35 | if __name__ == "__main__": 36 | L1 = np.array([[1540.399, 1320.082], [1601.358, -177.795], [-970.348, -154.368]]) / 1000.0 37 | L2 = np.array([[-970.348, -154.368], [3187.236, -205.191], [2198.592, -1947.821]]) / 1000.0 38 | 39 | 40 | # path1 = np.array([interpolate_2d(p1, p2, 0.15) for p1, p2 in L1]) 41 | # path2 = np.array([interpolate_2d(p1, p2, 0.15) for p1, p2 in L1]) 42 | 43 | for i, p in enumerate(L1): 44 | if i == 0: 45 | continue 46 | points = interpolate_2d(L1[i-1], L1[i], 0.15) 47 | print(points) 48 | 49 | # plt.plot(path1[:, 0], path1[:, 1]) 50 | # plt.show() 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /trajs/spiral8.csv: -------------------------------------------------------------------------------- 1 | 0,0,0,1 2 | 0.1,0.000607266,0.000260376,1 3 | 0.2,0.00740714,0.00317609,1 4 | 0.3,0.0280163,0.0120138,1 5 | 0.4,0.0647502,0.0277683,1 6 | 0.5,0.1131,0.0485089,1 7 | 0.6,0.146436,0.0628135,1 8 | 0.7,0.194818,0.0835844,1 9 | 0.8,0.233435,0.100182,1 10 | 0.9,0.259784,0.111536,1 11 | 1,0.275673,0.118422,1 12 | 1.1,0.285838,0.122873,1 13 | 1.2,0.291845,0.125517,1 14 | 1.3,0.304367,0.131011,1 15 | 1.4,0.323197,0.139232,1 16 | 1.5,0.349097,0.150508,1 17 | 1.6,0.3806,0.164213,1 18 | 1.7,0.414875,0.179134,1 19 | 1.8,0.435207,0.188,1 20 | 1.9,0.467738,0.202224,1 21 | 2,0.496696,0.21495,1 22 | 2.1,0.52177,0.226049,1 23 | 2.2,0.543825,0.235891,1 24 | 2.3,0.564414,0.245145,1 25 | 2.4,0.575517,0.250153,1 26 | 2.5,0.596945,0.259833,1 27 | 2.6,0.62017,0.27033,1 28 | 2.7,0.645321,0.2817,1 29 | 2.8,0.671905,0.293735,1 30 | 2.9,0.699076,0.306074,1 31 | 3,0.711789,0.311868,1 32 | 3.1,0.738293,0.324003,1 33 | 3.2,0.763708,0.33573,1 34 | 3.3,0.787974,0.347026,1 35 | 3.4,0.811359,0.358008,1 36 | 3.5,0.834315,0.368873,1 37 | 3.6,0.843069,0.373035,1 38 | 3.7,0.866148,0.384049,1 39 | 3.8,0.889622,0.395305,1 40 | 3.9,0.913509,0.406814,1 41 | 4,0.937648,0.41851,1 42 | 4.1,0.961793,0.43029,1 43 | 4.2,0.968435,0.433548,1 44 | 4.3,0.992244,0.445293,1 45 | 4.4,1.01563,0.456948,1 46 | 4.5,1.03859,0.468511,1 47 | 4.6,1.0612,0.48002,1 48 | 4.7,1.08359,0.491532,1 49 | 4.8,1.08701,0.4933,1 50 | 4.9,1.10928,0.504881,1 51 | 5,1.13151,0.516554,1 52 | 5.1,1.15369,0.528319,1 53 | 5.2,1.17578,0.540158,1 54 | 5.3,1.19769,0.552042,1 55 | 5.4,1.19796,0.552187,1 56 | 5.5,1.21962,0.564092,1 57 | 5.6,1.24101,0.576005,1 58 | 5.7,1.2621,0.587929,1 59 | 5.8,1.28293,0.599877,1 60 | 5.9,1.3005,0.610105,1 61 | 6,1.3209,0.622143,1 62 | 6.1,1.34108,0.634246,1 63 | 6.2,1.36104,0.646414,1 64 | 6.3,1.38075,0.658644,1 65 | 6.4,1.39393,0.666953,1 66 | 6.5,1.41315,0.679274,1 67 | 6.6,1.43206,0.691648,1 68 | 6.7,1.45064,0.704079,1 69 | 6.8,1.46888,0.716572,1 70 | 6.9,1.47757,0.722631,1 71 | 7,1.49532,0.735231,1 72 | 7.1,1.51272,0.747912,1 73 | 7.2,1.52975,0.760675,1 74 | 7.3,1.54639,0.773524,1 75 | 7.4,1.55086,0.777041,1 76 | 7.5,1.56697,0.790001,1 77 | 7.6,1.58264,0.803053,1 78 | 7.7,1.59785,0.816203,1 79 | 7.79999,1.61258,0.829457,1 80 | 7.89999,1.61326,0.830087,1 81 | 7.99999,1.62745,0.843457,1 82 | 8.09999,1.64111,0.856944,1 83 | 8.2,1.65419,0.870553,1 84 | 8.3,1.66435,0.881678,1 85 | 8.4,1.6763,0.895521,1 86 | 8.5,1.68757,0.9095,1 87 | 8.6,1.69811,0.923619,1 88 | 8.7,1.70376,0.931722,1 89 | 8.8,1.71305,0.946067,1 90 | 8.9,1.72146,0.960555,1 91 | 9,1.72894,0.975181,1 92 | 9.1,1.73123,0.980131,1 93 | 9.2,1.73733,0.994925,1 94 | 9.3,1.74232,1.00982,1 95 | 9.4,1.74613,1.0248,1 96 | 9.5,1.74655,1.02682,1 97 | 9.6,1.74892,1.04183,1 98 | 9.7,1.74995,1.05681,1 99 | 9.8,1.74962,1.07171,1 100 | 9.9,1.74788,1.08645,1 101 | 10,1.74474,1.10097,1 102 | 10.1,1.74041,1.11472,1 103 | 10.2,1.73463,1.12861,1 104 | 10.3,1.7276,1.14212,1 105 | 10.4,1.71938,1.15522,1 106 | 10.5,1.719,1.15577,1 107 | 10.6,1.70965,1.16842,1 108 | 10.7,1.69928,1.18063,1 109 | 10.8,1.68798,1.19241,1 110 | 10.9,1.68553,1.19479,1 111 | 11,1.6732,1.20607,1 112 | 11.1,1.66008,1.21695,1 113 | 11.2,1.64624,1.22744,1 114 | 11.3,1.64024,1.23172,1 115 | 11.4,1.62546,1.2417,1 116 | 11.5,1.61007,1.25132,1 117 | 11.6,1.5941,1.26062,1 118 | 11.7,1.58345,1.26649,1 119 | 11.8,1.56662,1.27528,1 120 | 11.9,1.54931,1.28378,1 121 | 12,1.53155,1.29201,1 122 | 12.1,1.51554,1.29904,1 123 | 12.2,1.49699,1.30678,1 124 | 12.3,1.47804,1.31428,1 125 | 12.4,1.45872,1.32155,1 126 | 12.5,1.43905,1.32859,1 127 | 12.6,1.43701,1.32931,1 128 | 12.7,1.41697,1.33612,1 129 | 12.8,1.39661,1.34272,1 130 | 12.9,1.37593,1.34913,1 131 | 13,1.35497,1.35535,1 132 | 13.1,1.3484,1.35724,1 133 | 13.2,1.32707,1.36322,1 134 | 13.3,1.30547,1.36902,1 135 | 13.4,1.28361,1.37465,1 136 | 13.5,1.26151,1.38011,1 137 | 13.6,1.25033,1.38279,1 138 | 13.7,1.22787,1.38802,1 139 | 13.8,1.2052,1.39309,1 140 | 13.9,1.18231,1.39801,1 141 | 14,1.15921,1.40278,1 142 | 14.1,1.14349,1.40592,1 143 | 14.2,1.12006,1.41046,1 144 | 14.3,1.09644,1.41485,1 145 | 14.4,1.07265,1.41912,1 146 | 14.5,1.04868,1.42325,1 147 | 14.6,1.02862,1.42658,1 148 | 14.7,1.00435,1.43048,1 149 | 14.8,0.979924,1.43425,1 150 | 14.9,0.955345,1.43791,1 151 | 15,0.930621,1.44144,1 152 | 15.1,0.906547,1.44474,1 153 | 15.2,0.881553,1.44804,1 154 | 15.3,0.85643,1.45123,1 155 | 15.4,0.831181,1.45431,1 156 | 15.5,0.805814,1.45727,1 157 | 15.6,0.780331,1.46013,1 158 | 15.7,0.778112,1.46037,1 159 | 15.8,0.752509,1.46311,1 160 | 15.9,0.726802,1.46574,1 161 | 16,0.700994,1.46826,1 162 | 16.1,0.675089,1.47069,1 163 | 16.2,0.649092,1.47301,1 164 | 16.3,0.644218,1.47343,1 165 | 16.4,0.618117,1.47563,1 166 | 16.5,0.591932,1.47773,1 167 | 16.6,0.565669,1.47973,1 168 | 16.7,0.53933,1.48163,1 169 | 16.8,0.51292,1.48344,1 170 | 16.9,0.505806,1.48391,1 171 | 17,0.479311,1.48559,1 172 | 17.1,0.452753,1.48718,1 173 | 17.2,0.426137,1.48867,1 174 | 17.3,0.399465,1.49007,1 175 | 17.4,0.372742,1.49137,1 176 | 17.5,0.363845,1.49178,1 177 | 17.6,0.337059,1.49296,1 178 | 17.7,0.31023,1.49405,1 179 | 17.8,0.283362,1.49504,1 180 | 17.9,0.256457,1.49595,1 181 | 18,0.22952,1.49676,1 182 | 18.1,0.219333,1.49704,1 183 | 18.2,0.192357,1.49773,1 184 | 18.3,0.165357,1.49832,1 185 | 18.4,0.138335,1.49883,1 186 | 18.5,0.111297,1.49924,1 187 | 18.6,0.0842441,1.49957,1 188 | 18.7,0.0732824,1.49967,1 189 | 18.8,0.0462159,1.49987,1 190 | 18.9,0.0191437,1.49998,1 191 | 19,-0.00793097,1.5,1 192 | 19.1,-0.0350046,1.49992,1 193 | 19.2,-0.0620739,1.49976,1 194 | 19.3,-0.0732824,1.49967,1 195 | 19.4,-0.10034,1.49938,1 196 | 19.5,-0.127384,1.499,1 197 | 19.6,-0.154413,1.49854,1 198 | 19.7,-0.181422,1.49798,1 199 | 19.8,-0.208409,1.49733,1 200 | 19.9,-0.219333,1.49704,1 201 | 20,-0.246282,1.49626,1 202 | 20.1,-0.273199,1.4954,1 203 | 20.2,-0.300082,1.49444,1 204 | 20.3,-0.326926,1.49338,1 205 | 20.4,-0.353729,1.49224,1 206 | 20.5,-0.363845,1.49178,1 207 | 20.6,-0.390585,1.49051,1 208 | 20.7,-0.417274,1.48914,1 209 | 20.8,-0.443909,1.48768,1 210 | 20.9,-0.470487,1.48613,1 211 | 21,-0.497003,1.48448,1 212 | 21.1,-0.505806,1.48391,1 213 | 21.2,-0.532234,1.48213,1 214 | 21.3,-0.558593,1.48025,1 215 | 21.4,-0.584877,1.47828,1 216 | 21.5,-0.611083,1.47621,1 217 | 21.6,-0.637207,1.47403,1 218 | 21.7,-0.644218,1.47343,1 219 | 21.8,-0.670232,1.47113,1 220 | 21.9,-0.696154,1.46873,1 221 | 22,-0.721981,1.46622,1 222 | 22.1,-0.747708,1.46361,1 223 | 22.2,-0.77333,1.46089,1 224 | 22.3,-0.778112,1.46037,1 225 | 22.4,-0.803604,1.45752,1 226 | 22.5,-0.828982,1.45457,1 227 | 22.6,-0.85424,1.4515,1 228 | 22.7001,-0.879375,1.44833,1 229 | 22.8001,-0.904381,1.44504,1 230 | 22.9001,-0.906547,1.44474,1 231 | 23.0001,-0.931407,1.44133,1 232 | 23.1001,-0.956126,1.43779,1 233 | 23.2001,-0.980701,1.43414,1 234 | 23.3001,-1.00512,1.43036,1 235 | 23.4001,-1.02862,1.42658,1 236 | 23.5001,-1.05273,1.42256,1 237 | 23.6001,-1.07667,1.41841,1 238 | 23.7001,-1.10044,1.41412,1 239 | 23.8001,-1.12402,1.4097,1 240 | 23.9001,-1.14349,1.40592,1 241 | 24.0001,-1.16672,1.40125,1 242 | 24.1001,-1.18975,1.39643,1 243 | 24.2001,-1.21257,1.39146,1 244 | 24.3001,-1.23518,1.38634,1 245 | 24.4001,-1.25033,1.38279,1 246 | 24.5001,-1.27255,1.37741,1 247 | 24.6001,-1.29453,1.37187,1 248 | 24.7001,-1.31625,1.36615,1 249 | 24.8001,-1.33772,1.36026,1 250 | 24.9001,-1.3484,1.35724,1 251 | 25.0001,-1.36945,1.35108,1 252 | 25.1001,-1.39021,1.34473,1 253 | 25.2001,-1.41067,1.33819,1 254 | 25.3001,-1.43082,1.33144,1 255 | 25.4001,-1.43701,1.32931,1 256 | 25.5001,-1.45672,1.32228,1 257 | 25.6001,-1.47607,1.31504,1 258 | 25.7001,-1.49506,1.30756,1 259 | 25.8001,-1.51366,1.29985,1 260 | 25.9001,-1.51554,1.29904,1 261 | 26.0001,-1.53369,1.29104,1 262 | 26.1001,-1.5514,1.28279,1 263 | 26.2001,-1.56865,1.27425,1 264 | 26.3001,-1.58345,1.26649,1 265 | 26.4001,-1.59976,1.2574,1 266 | 26.5001,-1.61553,1.24799,1 267 | 26.6001,-1.63071,1.23824,1 268 | 26.7001,-1.64024,1.23172,1 269 | 26.8001,-1.65437,1.22139,1 270 | 26.9001,-1.6678,1.21068,1 271 | 27.0001,-1.68047,1.19957,1 272 | 27.1001,-1.68553,1.19479,1 273 | 27.2001,-1.69702,1.1831,1 274 | 27.3001,-1.70759,1.17098,1 275 | 27.4001,-1.71716,1.15842,1 276 | 27.5001,-1.719,1.15577,1 277 | 27.6001,-1.72727,1.14269,1 278 | 27.7001,-1.73435,1.1292,1 279 | 27.8001,-1.74019,1.11532,1 280 | 27.9001,-1.74041,1.11472,1 281 | 28.0001,-1.74487,1.10048,1 282 | 28.1001,-1.74796,1.08595,1 283 | 28.2001,-1.74962,1.07171,1 284 | 28.3001,-1.74995,1.05681,1 285 | 28.4001,-1.74892,1.04183,1 286 | 28.5001,-1.74655,1.02682,1 287 | 28.6001,-1.74291,1.01184,1 288 | 28.7001,-1.73807,0.99693,1 289 | 28.8001,-1.73211,0.98212,1 290 | 28.9001,-1.73123,0.980131,1 291 | 29.0001,-1.72409,0.96546,1 292 | 29.1001,-1.71598,0.950925,1 293 | 29.2001,-1.70698,0.936531,1 294 | 29.3001,-1.70376,0.931722,1 295 | 29.4001,-1.69366,0.917521,1 296 | 29.5001,-1.6828,0.903462,1 297 | 29.6001,-1.67123,0.889543,1 298 | 29.7001,-1.66435,0.881678,1 299 | 29.8001,-1.65176,0.867967,1 300 | 29.9001,-1.63857,0.854382,1 301 | 30.0001,-1.62481,0.840918,1 302 | 30.1001,-1.61326,0.830087,1 303 | 30.2001,-1.59856,0.816827,1 304 | 30.3001,-1.58337,0.80367,1 305 | 30.4001,-1.56772,0.790614,1 306 | 30.5001,-1.55163,0.777652,1 307 | 30.6001,-1.55086,0.777041,1 308 | 30.7001,-1.53433,0.764172,1 309 | 30.8001,-1.51741,0.751388,1 310 | 30.9001,-1.50011,0.738685,1 311 | 31.0001,-1.48245,0.726059,1 312 | 31.1001,-1.47757,0.722631,1 313 | 31.2001,-1.45947,0.710096,1 314 | 31.3001,-1.44104,0.69763,1 315 | 31.4001,-1.42229,0.685229,1 316 | 31.5001,-1.40323,0.67289,1 317 | 31.6001,-1.39393,0.666953,1 318 | 31.7001,-1.37444,0.6547,1 319 | 31.8001,-1.35468,0.642503,1 320 | 31.9001,-1.33466,0.630358,1 321 | 32.0001,-1.31437,0.618263,1 322 | 32.1001,-1.3005,0.610105,1 323 | 32.2001,-1.27981,0.598089,1 324 | 32.3001,-1.25888,0.586118,1 325 | 32.4001,-1.23773,0.574189,1 326 | 32.5001,-1.21636,0.5623,1 327 | 32.6001,-1.19796,0.552187,1 328 | 32.7001,-1.1762,0.540369,1 329 | 32.8001,-1.15425,0.528587,1 330 | 32.9001,-1.13211,0.51684,1 331 | 33.0001,-1.10979,0.505125,1 332 | 33.1001,-1.08728,0.493442,1 333 | 33.2001,-1.08701,0.4933,1 334 | 33.3001,-1.06433,0.481647,1 335 | 33.4001,-1.04149,0.470024,1 336 | 33.5001,-1.01849,0.458428,1 337 | 33.6001,-0.995327,0.446858,1 338 | 33.7001,-0.972019,0.435314,1 339 | 33.8001,-0.968435,0.433548,1 340 | 33.9001,-0.944961,0.422032,1 341 | 34.0001,-0.921348,0.410539,1 342 | 34.1001,-0.897602,0.399069,1 343 | 34.2001,-0.873726,0.38762,1 344 | 34.3,-0.849726,0.376191,1 345 | 34.4,-0.843069,0.373035,1 346 | 34.5,-0.818916,0.361631,1 347 | 34.6,-0.79465,0.350246,1 348 | 34.7,-0.770273,0.338878,1 349 | 34.8,-0.745791,0.327528,1 350 | 34.9,-0.721207,0.316193,1 351 | 35,-0.711789,0.311868,1 352 | 35.1,-0.687071,0.300554,1 353 | 35.2,-0.662262,0.289255,1 354 | 35.3,-0.637364,0.27797,1 355 | 35.4,-0.612381,0.266699,1 356 | 35.5,-0.587319,0.255439,1 357 | 35.6,-0.575517,0.250153,1 358 | 35.7,-0.550342,0.238911,1 359 | 35.8,-0.525097,0.22768,1 360 | 35.9,-0.499784,0.216459,1 361 | 36,-0.474406,0.205247,1 362 | 36.1,-0.448968,0.194046,1 363 | 36.2,-0.435207,0.188,1 364 | 36.3,-0.409683,0.176811,1 365 | 36.4,-0.384106,0.16563,1 366 | 36.5,-0.358482,0.154456,1 367 | 36.6,-0.332812,0.143289,1 368 | 36.7,-0.307101,0.132128,1 369 | 36.8,-0.291845,0.125517,1 370 | 36.9,-0.266074,0.114365,1 371 | 37,-0.24027,0.103218,1 372 | 37.1,-0.214437,0.092075,1 373 | 37.2,-0.188576,0.0809363,1 374 | 37.3,-0.162693,0.069801,1 375 | 37.4,-0.146436,0.0628135,1 376 | 37.5,-0.120522,0.0516828,1 377 | 37.6,-0.0945921,0.0405543,1 378 | 37.7,-0.0686509,0.0294275,1 379 | 37.8,-0.0427013,0.0183019,1 380 | 37.9,-0.0167465,0.00717715,1 381 | 38,-4.28626e-16,1.83697e-16,1 382 | 38.1,0.0259565,-0.0111245,1 383 | 38.2,0.0519099,-0.0222495,1 384 | 38.3,0.0778568,-0.0333755,1 385 | 38.4,0.103794,-0.0445028,1 386 | 38.5,0.129719,-0.055632,1 387 | 38.6,0.146436,-0.0628135,1 388 | 38.7,0.172333,-0.0739468,1 389 | 38.8,0.198208,-0.0850833,1 390 | 38.9,0.224059,-0.0962235,1 391 | 39,0.249882,-0.107368,1 392 | 39.1,0.275674,-0.118517,1 393 | 39.2,0.291845,-0.125517,1 394 | 39.3,0.31758,-0.136674,1 395 | 39.4,0.343275,-0.147837,1 396 | 39.5,0.368926,-0.159007,1 397 | 39.6,0.394532,-0.170184,1 398 | 39.7,0.420088,-0.181368,1 399 | 39.8,0.435207,-0.188,1 400 | 39.9,0.460677,-0.199197,1 401 | 40,0.486087,-0.210403,1 402 | 40.1,0.511435,-0.221619,1 403 | 40.2,0.536718,-0.232844,1 404 | 40.3,0.561931,-0.244081,1 405 | 40.4,0.575517,-0.250153,1 406 | 40.5,0.600616,-0.261407,1 407 | 40.6,0.625636,-0.272673,1 408 | 40.7,0.650574,-0.283952,1 409 | 40.7999,0.675426,-0.295244,1 410 | 40.8999,0.700187,-0.30655,1 411 | 40.9999,0.711789,-0.311868,1 412 | 41.0999,0.736411,-0.323196,1 413 | 41.1999,0.760933,-0.33454,1 414 | 41.2999,0.78535,-0.345901,1 415 | 41.3999,0.80966,-0.35728,1 416 | 41.4999,0.833856,-0.368676,1 417 | 41.5999,0.843069,-0.373035,1 418 | 41.6999,0.867103,-0.384458,1 419 | 41.7999,0.891013,-0.395901,1 420 | 41.8999,0.914796,-0.407366,1 421 | 41.9999,0.938446,-0.418852,1 422 | 42.0999,0.961959,-0.430361,1 423 | 42.1999,0.968435,-0.433548,1 424 | 42.2999,0.991766,-0.445088,1 425 | 42.3999,1.01495,-0.456653,1 426 | 42.4999,1.03797,-0.468245,1 427 | 42.5999,1.06084,-0.479865,1 428 | 42.6999,1.08355,-0.491513,1 429 | 42.7999,1.08701,-0.4933,1 430 | 42.8999,1.10951,-0.504983,1 431 | 42.9999,1.13184,-0.516697,1 432 | 43.0999,1.15398,-0.528444,1 433 | 43.1999,1.17594,-0.540226,1 434 | 43.2999,1.19769,-0.552043,1 435 | 43.3999,1.19796,-0.552187,1 436 | 43.4999,1.21951,-0.564042,1 437 | 43.5999,1.24085,-0.575936,1 438 | 43.6999,1.26197,-0.587871,1 439 | 43.7999,1.28286,-0.599849,1 440 | 43.8999,1.3005,-0.610105,1 441 | 43.9999,1.32096,-0.622167,1 442 | 44.0999,1.34116,-0.634278,1 443 | 44.1999,1.3611,-0.64644,1 444 | 44.2999,1.38077,-0.658655,1 445 | 44.3999,1.39393,-0.666953,1 446 | 44.4999,1.41312,-0.679263,1 447 | 44.5999,1.43202,-0.691634,1 448 | 44.6999,1.45061,-0.704068,1 449 | 44.7999,1.46887,-0.716569,1 450 | 44.8999,1.47757,-0.722631,1 451 | 44.9999,1.49533,-0.735237,1 452 | 45.0999,1.51273,-0.747918,1 453 | 45.1999,1.52976,-0.76068,1 454 | 45.2999,1.54639,-0.773525,1 455 | 45.3999,1.55086,-0.777041,1 456 | 45.4999,1.56696,-0.789998,1 457 | 45.5999,1.58264,-0.80305,1 458 | 45.6999,1.59785,-0.816202,1 459 | 45.7999,1.61258,-0.829457,1 460 | 45.8999,1.61326,-0.830087,1 461 | 45.9999,1.62745,-0.843458,1 462 | 46.0999,1.64111,-0.856945,1 463 | 46.1999,1.65419,-0.870553,1 464 | 46.2999,1.66435,-0.881678,1 465 | 46.3999,1.6763,-0.89552,1 466 | 46.4999,1.68757,-0.909499,1 467 | 46.5999,1.69811,-0.923619,1 468 | 46.6999,1.70376,-0.931722,1 469 | 46.7999,1.71305,-0.946067,1 470 | 46.8999,1.72146,-0.960555,1 471 | 46.9999,1.72894,-0.975181,1 472 | 47.0999,1.73123,-0.980131,1 473 | 47.1999,1.73733,-0.994925,1 474 | 47.2999,1.74232,-1.00982,1 475 | 47.3998,1.74613,-1.0248,1 476 | 47.4998,1.74655,-1.02682,1 477 | 47.5998,1.74892,-1.04183,1 478 | 47.6998,1.74995,-1.05681,1 479 | 47.7998,1.74962,-1.07171,1 480 | 47.8998,1.74788,-1.08645,1 481 | 47.9998,1.74474,-1.10097,1 482 | 48.0998,1.74041,-1.11472,1 483 | 48.1998,1.73463,-1.12861,1 484 | 48.2998,1.7276,-1.14212,1 485 | 48.3998,1.71938,-1.15522,1 486 | 48.4998,1.719,-1.15577,1 487 | 48.5998,1.70965,-1.16842,1 488 | 48.6998,1.69928,-1.18063,1 489 | 48.7998,1.68798,-1.19241,1 490 | 48.8998,1.68553,-1.19479,1 491 | 48.9998,1.6732,-1.20607,1 492 | 49.0998,1.66009,-1.21695,1 493 | 49.1998,1.64624,-1.22744,1 494 | 49.2998,1.64024,-1.23172,1 495 | 49.3998,1.62546,-1.2417,1 496 | 49.4998,1.61007,-1.25132,1 497 | 49.5998,1.5941,-1.26062,1 498 | 49.6998,1.58345,-1.26649,1 499 | 49.7998,1.56662,-1.27528,1 500 | 49.8998,1.54931,-1.28378,1 501 | 49.9998,1.53155,-1.29201,1 502 | 50.0998,1.51554,-1.29904,1 503 | 50.1998,1.49699,-1.30678,1 504 | 50.2998,1.47804,-1.31428,1 505 | 50.3998,1.45872,-1.32155,1 506 | 50.4998,1.43905,-1.32859,1 507 | 50.5998,1.43701,-1.32931,1 508 | 50.6998,1.41697,-1.33612,1 509 | 50.7998,1.39661,-1.34272,1 510 | 50.8998,1.37593,-1.34913,1 511 | 50.9998,1.35497,-1.35535,1 512 | 51.0998,1.3484,-1.35724,1 513 | 51.1998,1.32707,-1.36322,1 514 | 51.2998,1.30547,-1.36902,1 515 | 51.3998,1.28361,-1.37465,1 516 | 51.4998,1.26151,-1.38011,1 517 | 51.5998,1.25033,-1.38279,1 518 | 51.6998,1.22787,-1.38802,1 519 | 51.7998,1.2052,-1.39309,1 520 | 51.8998,1.18231,-1.39801,1 521 | 51.9998,1.15921,-1.40278,1 522 | 52.0998,1.14349,-1.40592,1 523 | 52.1998,1.12006,-1.41046,1 524 | 52.2998,1.09644,-1.41485,1 525 | 52.3998,1.07265,-1.41912,1 526 | 52.4998,1.04868,-1.42325,1 527 | 52.5998,1.02862,-1.42658,1 528 | 52.6998,1.00435,-1.43048,1 529 | 52.7998,0.979924,-1.43425,1 530 | 52.8998,0.955345,-1.43791,1 531 | 52.9998,0.930621,-1.44144,1 532 | 53.0998,0.906547,-1.44474,1 533 | 53.1998,0.881553,-1.44804,1 534 | 53.2998,0.85643,-1.45123,1 535 | 53.3998,0.831181,-1.45431,1 536 | 53.4998,0.805814,-1.45727,1 537 | 53.5998,0.780331,-1.46013,1 538 | 53.6998,0.778112,-1.46037,1 539 | 53.7998,0.752509,-1.46311,1 540 | 53.8997,0.726802,-1.46574,1 541 | 53.9997,0.700994,-1.46826,1 542 | 54.0997,0.675089,-1.47069,1 543 | 54.1997,0.649092,-1.47301,1 544 | 54.2997,0.644218,-1.47343,1 545 | 54.3997,0.618117,-1.47563,1 546 | 54.4997,0.591932,-1.47773,1 547 | 54.5997,0.565669,-1.47973,1 548 | 54.6997,0.53933,-1.48163,1 549 | 54.7997,0.51292,-1.48344,1 550 | 54.8997,0.505806,-1.48391,1 551 | 54.9997,0.479311,-1.48559,1 552 | 55.0997,0.452753,-1.48718,1 553 | 55.1997,0.426137,-1.48867,1 554 | 55.2997,0.399465,-1.49007,1 555 | 55.3997,0.372742,-1.49137,1 556 | 55.4997,0.363845,-1.49178,1 557 | 55.5997,0.337059,-1.49296,1 558 | 55.6997,0.31023,-1.49405,1 559 | 55.7997,0.283362,-1.49504,1 560 | 55.8997,0.256457,-1.49595,1 561 | 55.9997,0.22952,-1.49676,1 562 | 56.0997,0.219333,-1.49704,1 563 | 56.1997,0.192357,-1.49773,1 564 | 56.2997,0.165357,-1.49832,1 565 | 56.3997,0.138335,-1.49883,1 566 | 56.4997,0.111297,-1.49924,1 567 | 56.5997,0.0842441,-1.49957,1 568 | 56.6997,0.0732824,-1.49967,1 569 | 56.7997,0.0462159,-1.49987,1 570 | 56.8997,0.0191437,-1.49998,1 571 | 56.9997,-0.00793097,-1.5,1 572 | 57.0997,-0.0350046,-1.49992,1 573 | 57.1997,-0.0620739,-1.49976,1 574 | 57.2997,-0.0732824,-1.49967,1 575 | 57.3997,-0.10034,-1.49938,1 576 | 57.4997,-0.127384,-1.499,1 577 | 57.5997,-0.154413,-1.49854,1 578 | 57.6997,-0.181422,-1.49798,1 579 | 57.7997,-0.208409,-1.49733,1 580 | 57.8997,-0.219333,-1.49704,1 581 | 57.9997,-0.246282,-1.49626,1 582 | 58.0997,-0.273199,-1.4954,1 583 | 58.1997,-0.300082,-1.49444,1 584 | 58.2997,-0.326926,-1.49338,1 585 | 58.3997,-0.353729,-1.49224,1 586 | 58.4997,-0.363845,-1.49178,1 587 | 58.5997,-0.390585,-1.49051,1 588 | 58.6997,-0.417274,-1.48914,1 589 | 58.7997,-0.443909,-1.48768,1 590 | 58.8997,-0.470487,-1.48613,1 591 | 58.9997,-0.497003,-1.48448,1 592 | 59.0997,-0.505806,-1.48391,1 593 | 59.1997,-0.532234,-1.48213,1 594 | 59.2997,-0.558593,-1.48025,1 595 | 59.3997,-0.584877,-1.47828,1 596 | 59.4997,-0.611083,-1.47621,1 597 | 59.5997,-0.637207,-1.47403,1 598 | 59.6997,-0.644218,-1.47343,1 599 | 59.7997,-0.670232,-1.47113,1 600 | 59.8997,-0.696154,-1.46873,1 601 | 59.9997,-0.721981,-1.46622,1 602 | 60.0997,-0.747708,-1.46361,1 603 | 60.1997,-0.77333,-1.46089,1 604 | 60.2997,-0.778112,-1.46037,1 605 | 60.3997,-0.803604,-1.45752,1 606 | 60.4996,-0.828982,-1.45457,1 607 | 60.5996,-0.85424,-1.4515,1 608 | 60.6996,-0.879375,-1.44833,1 609 | 60.7996,-0.904381,-1.44504,1 610 | 60.8996,-0.906547,-1.44474,1 611 | 60.9996,-0.931407,-1.44133,1 612 | 61.0996,-0.956126,-1.43779,1 613 | 61.1996,-0.980701,-1.43414,1 614 | 61.2996,-1.00512,-1.43036,1 615 | 61.3996,-1.02862,-1.42658,1 616 | 61.4996,-1.05273,-1.42256,1 617 | 61.5996,-1.07667,-1.41841,1 618 | 61.6996,-1.10044,-1.41412,1 619 | 61.7996,-1.12402,-1.4097,1 620 | 61.8996,-1.14349,-1.40592,1 621 | 61.9996,-1.16672,-1.40125,1 622 | 62.0996,-1.18975,-1.39643,1 623 | 62.1996,-1.21257,-1.39146,1 624 | 62.2996,-1.23518,-1.38634,1 625 | 62.3996,-1.25033,-1.38279,1 626 | 62.4996,-1.27255,-1.37741,1 627 | 62.5996,-1.29453,-1.37187,1 628 | 62.6996,-1.31625,-1.36615,1 629 | 62.7996,-1.33772,-1.36026,1 630 | 62.8996,-1.3484,-1.35724,1 631 | 62.9996,-1.36945,-1.35108,1 632 | 63.0996,-1.39021,-1.34473,1 633 | 63.1996,-1.41067,-1.33819,1 634 | 63.2996,-1.43082,-1.33144,1 635 | 63.3996,-1.43701,-1.32931,1 636 | 63.4996,-1.45672,-1.32228,1 637 | 63.5996,-1.47607,-1.31504,1 638 | 63.6996,-1.49506,-1.30756,1 639 | 63.7996,-1.51366,-1.29985,1 640 | 63.8996,-1.51554,-1.29904,1 641 | 63.9996,-1.53369,-1.29104,1 642 | 64.0996,-1.5514,-1.28279,1 643 | 64.1996,-1.56865,-1.27425,1 644 | 64.2996,-1.58345,-1.26649,1 645 | 64.3996,-1.59976,-1.2574,1 646 | 64.4996,-1.61553,-1.24799,1 647 | 64.5996,-1.63071,-1.23824,1 648 | 64.6996,-1.64024,-1.23172,1 649 | 64.7996,-1.65437,-1.22139,1 650 | 64.8996,-1.6678,-1.21068,1 651 | 64.9996,-1.68047,-1.19957,1 652 | 65.0996,-1.68553,-1.19479,1 653 | 65.1996,-1.69702,-1.1831,1 654 | 65.2996,-1.70759,-1.17098,1 655 | 65.3996,-1.71716,-1.15842,1 656 | 65.4996,-1.719,-1.15577,1 657 | 65.5996,-1.72727,-1.14269,1 658 | 65.6996,-1.73435,-1.1292,1 659 | 65.7996,-1.74019,-1.11532,1 660 | 65.8996,-1.74041,-1.11472,1 661 | 65.9996,-1.74487,-1.10048,1 662 | 66.0996,-1.74796,-1.08595,1 663 | 66.1996,-1.74962,-1.07171,1 664 | 66.2996,-1.74995,-1.05681,1 665 | 66.3996,-1.74892,-1.04183,1 666 | 66.4996,-1.74655,-1.02682,1 667 | 66.5996,-1.74291,-1.01184,1 668 | 66.6996,-1.73807,-0.99693,1 669 | 66.7996,-1.73211,-0.98212,1 670 | 66.8996,-1.73123,-0.980131,1 671 | 66.9995,-1.72409,-0.96546,1 672 | 67.0995,-1.71598,-0.950925,1 673 | 67.1995,-1.70698,-0.936531,1 674 | 67.2995,-1.70376,-0.931722,1 675 | 67.3995,-1.69366,-0.91752,1 676 | 67.4995,-1.6828,-0.903462,1 677 | 67.5995,-1.67123,-0.889542,1 678 | 67.6995,-1.66435,-0.881678,1 679 | 67.7995,-1.65176,-0.867967,1 680 | 67.8995,-1.63857,-0.854383,1 681 | 67.9995,-1.62481,-0.840918,1 682 | 68.0995,-1.61326,-0.830087,1 683 | 68.1995,-1.59855,-0.816826,1 684 | 68.2995,-1.58336,-0.803669,1 685 | 68.3995,-1.56771,-0.790613,1 686 | 68.4995,-1.55163,-0.777652,1 687 | 68.5995,-1.55086,-0.777041,1 688 | 68.6995,-1.53434,-0.764174,1 689 | 68.7995,-1.51742,-0.751392,1 690 | 68.8995,-1.50012,-0.738689,1 691 | 68.9995,-1.48245,-0.72606,1 692 | 69.0995,-1.47757,-0.722631,1 693 | 69.1995,-1.45946,-0.710092,1 694 | 69.2995,-1.44102,-0.697623,1 695 | 69.3995,-1.42227,-0.685222,1 696 | 69.4995,-1.40323,-0.672887,1 697 | 69.5995,-1.39393,-0.666953,1 698 | 69.6995,-1.37446,-0.654709,1 699 | 69.7995,-1.35472,-0.642518,1 700 | 69.8995,-1.33469,-0.630375,1 701 | 69.9995,-1.31439,-0.618272,1 702 | 70.0995,-1.3005,-0.610105,1 703 | 70.1995,-1.27977,-0.598072,1 704 | 70.2995,-1.25881,-0.586085,1 705 | 70.3995,-1.23764,-0.574152,1 706 | 70.4995,-1.2163,-0.562277,1 707 | 70.5995,-1.19796,-0.552187,1 708 | 70.6995,-1.17629,-0.540405,1 709 | 70.7995,-1.15441,-0.528654,1 710 | 70.8995,-1.13229,-0.516916,1 711 | 70.9995,-1.10991,-0.50518,1 712 | 71.0995,-1.08728,-0.493442,1 713 | 71.1995,-1.08701,-0.4933,1 714 | 71.2995,-1.06416,-0.481575,1 715 | 71.3995,-1.04117,-0.469889,1 716 | 71.4995,-1.01812,-0.458269,1 717 | 71.5995,-0.995045,-0.446737,1 718 | 71.6995,-0.971973,-0.435294,1 719 | 71.7995,-0.968435,-0.433548,1 720 | 71.8995,-0.945295,-0.422175,1 721 | 71.9995,-0.921978,-0.410809,1 722 | 72.0995,-0.898355,-0.399392,1 723 | 72.1995,-0.87433,-0.387879,1 724 | 72.2995,-0.849889,-0.376261,1 725 | 72.3995,-0.843069,-0.373035,1 726 | 72.4995,-0.818257,-0.361348,1 727 | 72.5995,-0.793402,-0.349711,1 728 | 72.6995,-0.768763,-0.33823,1 729 | 72.7995,-0.744536,-0.326989,1 730 | 72.8995,-0.720771,-0.316007,1 731 | 72.9995,-0.711789,-0.311868,1 732 | 73.0995,-0.68836,-0.301108,1 733 | 73.1995,-0.664706,-0.290305,1 734 | 73.2995,-0.640351,-0.279253,1 735 | 73.3995,-0.614929,-0.267792,1 736 | 73.4995,-0.588344,-0.255879,1 737 | 73.5994,-0.575517,-0.250153,1 738 | 73.6994,-0.547845,-0.237839,1 739 | 73.7994,-0.520355,-0.225645,1 740 | 73.8994,-0.49395,-0.213955,1 741 | 73.9994,-0.469336,-0.203072,1 742 | 74.0994,-0.446726,-0.193083,1 743 | 74.1994,-0.435207,-0.188,1 744 | 74.2994,-0.414471,-0.178866,1 745 | 74.3994,-0.3932,-0.169533,1 746 | 74.4994,-0.369712,-0.159276,1 747 | 74.5994,-0.34269,-0.147528,1 748 | 74.6994,-0.311736,-0.134118,1 749 | 74.7994,-0.291845,-0.125517,1 750 | 74.8994,-0.25703,-0.110483,1 751 | 74.9994,-0.223141,-0.0958662,1 752 | 75.0994,-0.193306,-0.0830061,1 753 | 75.1994,-0.16993,-0.0729336,1 754 | 75.2994,-0.153676,-0.0659311,1 755 | 75.3994,-0.146436,-0.0628135,1 756 | 75.4994,-0.136944,-0.058731,1 757 | 75.5994,-0.125281,-0.0537258,1 758 | 75.6994,-0.105907,-0.0454177,1 759 | 75.7994,-0.0749691,-0.0321517,1 760 | 75.8994,-0.032154,-0.0137904,1 761 | 75.9994,-8.57253e-16,-3.67394e-16,1 762 | 76.0994,0.0513898,0.0220421,1 763 | 76.1994,0.0964803,0.0413836,1 764 | 76.2994,0.127649,0.054754,1 765 | 76.3994,0.142703,0.0612118,1 766 | 76.4994,0.146321,0.0627641,1 767 | -------------------------------------------------------------------------------- /trajs/spiral82.csv: -------------------------------------------------------------------------------- 1 | 0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00 2 | 2.000000000000000111e-01,7.407140000000000425e-03,3.176089999999999903e-03,1.000000000000000000e+00 3 | 4.000000000000000222e-01,6.475019999999999387e-02,2.776829999999999923e-02,1.000000000000000000e+00 4 | 5.999999999999999778e-01,1.464360000000000106e-01,6.281349999999999434e-02,1.000000000000000000e+00 5 | 8.000000000000000444e-01,2.334350000000000036e-01,1.001819999999999933e-01,1.000000000000000000e+00 6 | 1.000000000000000000e+00,2.756730000000000014e-01,1.184219999999999995e-01,1.000000000000000000e+00 7 | 1.199999999999999956e+00,2.918450000000000211e-01,1.255169999999999897e-01,1.000000000000000000e+00 8 | 1.399999999999999911e+00,3.231970000000000121e-01,1.392319999999999947e-01,1.000000000000000000e+00 9 | 1.600000000000000089e+00,3.805999999999999939e-01,1.642129999999999979e-01,1.000000000000000000e+00 10 | 1.800000000000000044e+00,4.352070000000000105e-01,1.880000000000000004e-01,1.000000000000000000e+00 11 | 2.000000000000000000e+00,4.966960000000000264e-01,2.149500000000000022e-01,1.000000000000000000e+00 12 | 2.200000000000000178e+00,5.438250000000000028e-01,2.358909999999999896e-01,1.000000000000000000e+00 13 | 2.399999999999999911e+00,5.755169999999999453e-01,2.501530000000000142e-01,1.000000000000000000e+00 14 | 2.600000000000000089e+00,6.201699999999999990e-01,2.703300000000000147e-01,1.000000000000000000e+00 15 | 2.799999999999999822e+00,6.719049999999999745e-01,2.937350000000000239e-01,1.000000000000000000e+00 16 | 3.000000000000000000e+00,7.117890000000000050e-01,3.118679999999999786e-01,1.000000000000000000e+00 17 | 3.200000000000000178e+00,7.637080000000000535e-01,3.357299999999999729e-01,1.000000000000000000e+00 18 | 3.399999999999999911e+00,8.113590000000000524e-01,3.580079999999999929e-01,1.000000000000000000e+00 19 | 3.600000000000000089e+00,8.430689999999999573e-01,3.730350000000000055e-01,1.000000000000000000e+00 20 | 3.799999999999999822e+00,8.896220000000000239e-01,3.953050000000000175e-01,1.000000000000000000e+00 21 | 4.000000000000000000e+00,9.376480000000000370e-01,4.185099999999999931e-01,1.000000000000000000e+00 22 | 4.200000000000000178e+00,9.684350000000000458e-01,4.335479999999999889e-01,1.000000000000000000e+00 23 | 4.400000000000000355e+00,1.015630000000000033e+00,4.569480000000000208e-01,1.000000000000000000e+00 24 | 4.599999999999999645e+00,1.061199999999999921e+00,4.800200000000000022e-01,1.000000000000000000e+00 25 | 4.799999999999999822e+00,1.087010000000000032e+00,4.933000000000000163e-01,1.000000000000000000e+00 26 | 5.000000000000000000e+00,1.131510000000000016e+00,5.165539999999999576e-01,1.000000000000000000e+00 27 | 5.200000000000000178e+00,1.175780000000000047e+00,5.401580000000000270e-01,1.000000000000000000e+00 28 | 5.400000000000000355e+00,1.197959999999999914e+00,5.521869999999999834e-01,1.000000000000000000e+00 29 | 5.599999999999999645e+00,1.241009999999999946e+00,5.760049999999999892e-01,1.000000000000000000e+00 30 | 5.799999999999999822e+00,1.282929999999999904e+00,5.998769999999999936e-01,1.000000000000000000e+00 31 | 6.000000000000000000e+00,1.320899999999999963e+00,6.221430000000000016e-01,1.000000000000000000e+00 32 | 6.200000000000000178e+00,1.361040000000000028e+00,6.464140000000000441e-01,1.000000000000000000e+00 33 | 6.400000000000000355e+00,1.393929999999999891e+00,6.669530000000000181e-01,1.000000000000000000e+00 34 | 6.599999999999999645e+00,1.432060000000000111e+00,6.916480000000000405e-01,1.000000000000000000e+00 35 | 6.799999999999999822e+00,1.468879999999999963e+00,7.165719999999999867e-01,1.000000000000000000e+00 36 | 7.000000000000000000e+00,1.495319999999999983e+00,7.352309999999999679e-01,1.000000000000000000e+00 37 | 7.200000000000000178e+00,1.529749999999999943e+00,7.606749999999999901e-01,1.000000000000000000e+00 38 | 7.400000000000000355e+00,1.550859999999999905e+00,7.770409999999999817e-01,1.000000000000000000e+00 39 | 7.599999999999999645e+00,1.582640000000000047e+00,8.030530000000000168e-01,1.000000000000000000e+00 40 | 7.799990000000000201e+00,1.612579999999999902e+00,8.294569999999999999e-01,1.000000000000000000e+00 41 | 7.999990000000000379e+00,1.627450000000000063e+00,8.434570000000000123e-01,1.000000000000000000e+00 42 | 8.199999999999999289e+00,1.654190000000000049e+00,8.705530000000000213e-01,1.000000000000000000e+00 43 | 8.400000000000000355e+00,1.676299999999999901e+00,8.955210000000000115e-01,1.000000000000000000e+00 44 | 8.599999999999999645e+00,1.698110000000000008e+00,9.236189999999999678e-01,1.000000000000000000e+00 45 | 8.800000000000000711e+00,1.713049999999999962e+00,9.460669999999999913e-01,1.000000000000000000e+00 46 | 9.000000000000000000e+00,1.728939999999999921e+00,9.751809999999999645e-01,1.000000000000000000e+00 47 | 9.199999999999999289e+00,1.737330000000000041e+00,9.949249999999999483e-01,1.000000000000000000e+00 48 | 9.400000000000000355e+00,1.746129999999999960e+00,1.024799999999999933e+00,1.000000000000000000e+00 49 | 9.599999999999999645e+00,1.748920000000000030e+00,1.041830000000000034e+00,1.000000000000000000e+00 50 | 9.800000000000000711e+00,1.749619999999999953e+00,1.071709999999999940e+00,1.000000000000000000e+00 51 | 1.000000000000000000e+01,1.744739999999999958e+00,1.100970000000000004e+00,1.000000000000000000e+00 52 | 1.019999999999999929e+01,1.734629999999999894e+00,1.128609999999999891e+00,1.000000000000000000e+00 53 | 1.040000000000000036e+01,1.719379999999999908e+00,1.155219999999999914e+00,1.000000000000000000e+00 54 | 1.059999999999999964e+01,1.709649999999999892e+00,1.168420000000000014e+00,1.000000000000000000e+00 55 | 1.080000000000000071e+01,1.687980000000000036e+00,1.192409999999999970e+00,1.000000000000000000e+00 56 | 1.100000000000000000e+01,1.673200000000000021e+00,1.206069999999999975e+00,1.000000000000000000e+00 57 | 1.119999999999999929e+01,1.646239999999999926e+00,1.227440000000000087e+00,1.000000000000000000e+00 58 | 1.140000000000000036e+01,1.625459999999999905e+00,1.241700000000000026e+00,1.000000000000000000e+00 59 | 1.159999999999999964e+01,1.594100000000000072e+00,1.260620000000000074e+00,1.000000000000000000e+00 60 | 1.180000000000000071e+01,1.566619999999999902e+00,1.275279999999999969e+00,1.000000000000000000e+00 61 | 1.200000000000000000e+01,1.531549999999999967e+00,1.292010000000000103e+00,1.000000000000000000e+00 62 | 1.219999999999999929e+01,1.496990000000000043e+00,1.306780000000000053e+00,1.000000000000000000e+00 63 | 1.240000000000000036e+01,1.458720000000000017e+00,1.321550000000000002e+00,1.000000000000000000e+00 64 | 1.259999999999999964e+01,1.437009999999999899e+00,1.329309999999999992e+00,1.000000000000000000e+00 65 | 1.280000000000000071e+01,1.396609999999999907e+00,1.342719999999999914e+00,1.000000000000000000e+00 66 | 1.300000000000000000e+01,1.354970000000000008e+00,1.355350000000000055e+00,1.000000000000000000e+00 67 | 1.319999999999999929e+01,1.327069999999999972e+00,1.363220000000000098e+00,1.000000000000000000e+00 68 | 1.340000000000000036e+01,1.283609999999999918e+00,1.374649999999999928e+00,1.000000000000000000e+00 69 | 1.359999999999999964e+01,1.250329999999999941e+00,1.382789999999999964e+00,1.000000000000000000e+00 70 | 1.380000000000000071e+01,1.205200000000000049e+00,1.393089999999999939e+00,1.000000000000000000e+00 71 | 1.400000000000000000e+01,1.159210000000000074e+00,1.402779999999999916e+00,1.000000000000000000e+00 72 | 1.419999999999999929e+01,1.120060000000000056e+00,1.410460000000000047e+00,1.000000000000000000e+00 73 | 1.440000000000000036e+01,1.072650000000000103e+00,1.419119999999999937e+00,1.000000000000000000e+00 74 | 1.459999999999999964e+01,1.028620000000000090e+00,1.426579999999999959e+00,1.000000000000000000e+00 75 | 1.480000000000000071e+01,9.799240000000000173e-01,1.434250000000000025e+00,1.000000000000000000e+00 76 | 1.500000000000000000e+01,9.306210000000000315e-01,1.441440000000000055e+00,1.000000000000000000e+00 77 | 1.519999999999999929e+01,8.815530000000000310e-01,1.448039999999999994e+00,1.000000000000000000e+00 78 | 1.540000000000000036e+01,8.311809999999999476e-01,1.454309999999999992e+00,1.000000000000000000e+00 79 | 1.559999999999999964e+01,7.803309999999999969e-01,1.460129999999999928e+00,1.000000000000000000e+00 80 | 1.580000000000000071e+01,7.525089999999999835e-01,1.463109999999999911e+00,1.000000000000000000e+00 81 | 1.600000000000000000e+01,7.009940000000000060e-01,1.468259999999999899e+00,1.000000000000000000e+00 82 | 1.619999999999999929e+01,6.490920000000000023e-01,1.473009999999999931e+00,1.000000000000000000e+00 83 | 1.639999999999999858e+01,6.181170000000000275e-01,1.475629999999999997e+00,1.000000000000000000e+00 84 | 1.660000000000000142e+01,5.656689999999999774e-01,1.479729999999999990e+00,1.000000000000000000e+00 85 | 1.680000000000000071e+01,5.129200000000000426e-01,1.483440000000000092e+00,1.000000000000000000e+00 86 | 1.700000000000000000e+01,4.793109999999999871e-01,1.485589999999999966e+00,1.000000000000000000e+00 87 | 1.719999999999999929e+01,4.261369999999999880e-01,1.488669999999999938e+00,1.000000000000000000e+00 88 | 1.739999999999999858e+01,3.727420000000000178e-01,1.491370000000000084e+00,1.000000000000000000e+00 89 | 1.760000000000000142e+01,3.370589999999999975e-01,1.492960000000000065e+00,1.000000000000000000e+00 90 | 1.780000000000000071e+01,2.833620000000000028e-01,1.495039999999999925e+00,1.000000000000000000e+00 91 | 1.800000000000000000e+01,2.295200000000000018e-01,1.496760000000000090e+00,1.000000000000000000e+00 92 | 1.819999999999999929e+01,1.923570000000000002e-01,1.497730000000000006e+00,1.000000000000000000e+00 93 | 1.839999999999999858e+01,1.383350000000000135e-01,1.498830000000000107e+00,1.000000000000000000e+00 94 | 1.860000000000000142e+01,8.424410000000000232e-02,1.499570000000000070e+00,1.000000000000000000e+00 95 | 1.880000000000000071e+01,4.621589999999999726e-02,1.499870000000000037e+00,1.000000000000000000e+00 96 | 1.900000000000000000e+01,-7.930970000000000728e-03,1.500000000000000000e+00,1.000000000000000000e+00 97 | 1.919999999999999929e+01,-6.207390000000000130e-02,1.499759999999999982e+00,1.000000000000000000e+00 98 | 1.939999999999999858e+01,-1.003399999999999986e-01,1.499379999999999935e+00,1.000000000000000000e+00 99 | 1.960000000000000142e+01,-1.544129999999999947e-01,1.498539999999999983e+00,1.000000000000000000e+00 100 | 1.980000000000000071e+01,-2.084090000000000109e-01,1.497330000000000050e+00,1.000000000000000000e+00 101 | 2.000000000000000000e+01,-2.462820000000000009e-01,1.496259999999999923e+00,1.000000000000000000e+00 102 | 2.019999999999999929e+01,-3.000820000000000154e-01,1.494439999999999991e+00,1.000000000000000000e+00 103 | 2.039999999999999858e+01,-3.537290000000000156e-01,1.492240000000000011e+00,1.000000000000000000e+00 104 | 2.060000000000000142e+01,-3.905850000000000155e-01,1.490510000000000002e+00,1.000000000000000000e+00 105 | 2.080000000000000071e+01,-4.439089999999999980e-01,1.487679999999999891e+00,1.000000000000000000e+00 106 | 2.100000000000000000e+01,-4.970029999999999726e-01,1.484480000000000022e+00,1.000000000000000000e+00 107 | 2.119999999999999929e+01,-5.322339999999999849e-01,1.482129999999999947e+00,1.000000000000000000e+00 108 | 2.139999999999999858e+01,-5.848769999999999802e-01,1.478280000000000038e+00,1.000000000000000000e+00 109 | 2.160000000000000142e+01,-6.372069999999999679e-01,1.474029999999999951e+00,1.000000000000000000e+00 110 | 2.180000000000000071e+01,-6.702320000000000499e-01,1.471130000000000049e+00,1.000000000000000000e+00 111 | 2.200000000000000000e+01,-7.219809999999999839e-01,1.466220000000000079e+00,1.000000000000000000e+00 112 | 2.219999999999999929e+01,-7.733299999999999619e-01,1.460890000000000022e+00,1.000000000000000000e+00 113 | 2.239999999999999858e+01,-8.036039999999999850e-01,1.457519999999999927e+00,1.000000000000000000e+00 114 | 2.260000000000000142e+01,-8.542399999999999993e-01,1.451500000000000012e+00,1.000000000000000000e+00 115 | 2.280010000000000048e+01,-9.043809999999999905e-01,1.445040000000000102e+00,1.000000000000000000e+00 116 | 2.300009999999999977e+01,-9.314069999999999849e-01,1.441330000000000000e+00,1.000000000000000000e+00 117 | 2.320009999999999906e+01,-9.807010000000000449e-01,1.434139999999999970e+00,1.000000000000000000e+00 118 | 2.340009999999999835e+01,-1.028620000000000090e+00,1.426579999999999959e+00,1.000000000000000000e+00 119 | 2.360010000000000119e+01,-1.076670000000000016e+00,1.418409999999999949e+00,1.000000000000000000e+00 120 | 2.380010000000000048e+01,-1.124020000000000019e+00,1.409699999999999953e+00,1.000000000000000000e+00 121 | 2.400009999999999977e+01,-1.166719999999999979e+00,1.401250000000000107e+00,1.000000000000000000e+00 122 | 2.420009999999999906e+01,-1.212569999999999926e+00,1.391459999999999919e+00,1.000000000000000000e+00 123 | 2.440009999999999835e+01,-1.250329999999999941e+00,1.382789999999999964e+00,1.000000000000000000e+00 124 | 2.460010000000000119e+01,-1.294529999999999959e+00,1.371869999999999923e+00,1.000000000000000000e+00 125 | 2.480010000000000048e+01,-1.337720000000000020e+00,1.360260000000000025e+00,1.000000000000000000e+00 126 | 2.500009999999999977e+01,-1.369450000000000056e+00,1.351080000000000059e+00,1.000000000000000000e+00 127 | 2.520009999999999906e+01,-1.410670000000000091e+00,1.338189999999999991e+00,1.000000000000000000e+00 128 | 2.540009999999999835e+01,-1.437009999999999899e+00,1.329309999999999992e+00,1.000000000000000000e+00 129 | 2.560010000000000119e+01,-1.476069999999999993e+00,1.315039999999999987e+00,1.000000000000000000e+00 130 | 2.580010000000000048e+01,-1.513660000000000005e+00,1.299849999999999950e+00,1.000000000000000000e+00 131 | 2.600009999999999977e+01,-1.533689999999999998e+00,1.291039999999999965e+00,1.000000000000000000e+00 132 | 2.620009999999999906e+01,-1.568650000000000100e+00,1.274250000000000105e+00,1.000000000000000000e+00 133 | 2.640009999999999835e+01,-1.599760000000000071e+00,1.257400000000000073e+00,1.000000000000000000e+00 134 | 2.660010000000000119e+01,-1.630710000000000104e+00,1.238240000000000007e+00,1.000000000000000000e+00 135 | 2.680010000000000048e+01,-1.654369999999999896e+00,1.221389999999999976e+00,1.000000000000000000e+00 136 | 2.700009999999999977e+01,-1.680469999999999908e+00,1.199570000000000025e+00,1.000000000000000000e+00 137 | 2.720009999999999906e+01,-1.697019999999999973e+00,1.183100000000000041e+00,1.000000000000000000e+00 138 | 2.740009999999999835e+01,-1.717160000000000020e+00,1.158420000000000005e+00,1.000000000000000000e+00 139 | 2.760010000000000119e+01,-1.727270000000000083e+00,1.142689999999999984e+00,1.000000000000000000e+00 140 | 2.780010000000000048e+01,-1.740189999999999904e+00,1.115320000000000089e+00,1.000000000000000000e+00 141 | 2.800009999999999977e+01,-1.744869999999999921e+00,1.100479999999999903e+00,1.000000000000000000e+00 142 | 2.820009999999999906e+01,-1.749619999999999953e+00,1.071709999999999940e+00,1.000000000000000000e+00 143 | 2.840009999999999835e+01,-1.748920000000000030e+00,1.041830000000000034e+00,1.000000000000000000e+00 144 | 2.860010000000000119e+01,-1.742909999999999959e+00,1.011840000000000073e+00,1.000000000000000000e+00 145 | 2.880010000000000048e+01,-1.732110000000000039e+00,9.821199999999999930e-01,1.000000000000000000e+00 146 | 2.900009999999999977e+01,-1.724089999999999900e+00,9.654599999999999849e-01,1.000000000000000000e+00 147 | 2.920009999999999906e+01,-1.706979999999999942e+00,9.365310000000000024e-01,1.000000000000000000e+00 148 | 2.940009999999999835e+01,-1.693659999999999943e+00,9.175210000000000310e-01,1.000000000000000000e+00 149 | 2.960010000000000119e+01,-1.671229999999999993e+00,8.895429999999999726e-01,1.000000000000000000e+00 150 | 2.980010000000000048e+01,-1.651759999999999895e+00,8.679670000000000440e-01,1.000000000000000000e+00 151 | 3.000009999999999977e+01,-1.624810000000000088e+00,8.409180000000000543e-01,1.000000000000000000e+00 152 | 3.020009999999999906e+01,-1.598559999999999981e+00,8.168269999999999698e-01,1.000000000000000000e+00 153 | 3.040009999999999835e+01,-1.567720000000000002e+00,7.906140000000000390e-01,1.000000000000000000e+00 154 | 3.060010000000000119e+01,-1.550859999999999905e+00,7.770409999999999817e-01,1.000000000000000000e+00 155 | 3.080010000000000048e+01,-1.517409999999999926e+00,7.513879999999999448e-01,1.000000000000000000e+00 156 | 3.100009999999999977e+01,-1.482450000000000045e+00,7.260590000000000099e-01,1.000000000000000000e+00 157 | 3.120009999999999906e+01,-1.459470000000000045e+00,7.100959999999999495e-01,1.000000000000000000e+00 158 | 3.140009999999999835e+01,-1.422290000000000054e+00,6.852289999999999770e-01,1.000000000000000000e+00 159 | 3.160010000000000119e+01,-1.393929999999999891e+00,6.669530000000000181e-01,1.000000000000000000e+00 160 | 3.180010000000000048e+01,-1.354680000000000106e+00,6.425030000000000463e-01,1.000000000000000000e+00 161 | 3.200010000000000332e+01,-1.314370000000000038e+00,6.182630000000000070e-01,1.000000000000000000e+00 162 | 3.220009999999999906e+01,-1.279809999999999892e+00,5.980889999999999818e-01,1.000000000000000000e+00 163 | 3.240010000000000190e+01,-1.237729999999999997e+00,5.741889999999999494e-01,1.000000000000000000e+00 164 | 3.260009999999999764e+01,-1.197959999999999914e+00,5.521869999999999834e-01,1.000000000000000000e+00 165 | 3.280010000000000048e+01,-1.154249999999999998e+00,5.285870000000000291e-01,1.000000000000000000e+00 166 | 3.300010000000000332e+01,-1.109790000000000054e+00,5.051250000000000462e-01,1.000000000000000000e+00 167 | 3.320009999999999906e+01,-1.087010000000000032e+00,4.933000000000000163e-01,1.000000000000000000e+00 168 | 3.340010000000000190e+01,-1.041490000000000027e+00,4.700239999999999974e-01,1.000000000000000000e+00 169 | 3.360009999999999764e+01,-9.953269999999999618e-01,4.468579999999999774e-01,1.000000000000000000e+00 170 | 3.380010000000000048e+01,-9.684350000000000458e-01,4.335479999999999889e-01,1.000000000000000000e+00 171 | 3.400010000000000332e+01,-9.213479999999999448e-01,4.105389999999999873e-01,1.000000000000000000e+00 172 | 3.420009999999999906e+01,-8.737260000000000026e-01,3.876200000000000201e-01,1.000000000000000000e+00 173 | 3.439999999999999858e+01,-8.430689999999999573e-01,3.730350000000000055e-01,1.000000000000000000e+00 174 | 3.460000000000000142e+01,-7.946499999999999675e-01,3.502460000000000018e-01,1.000000000000000000e+00 175 | 3.479999999999999716e+01,-7.457909999999999817e-01,3.275279999999999858e-01,1.000000000000000000e+00 176 | 3.500000000000000000e+01,-7.117890000000000050e-01,3.118679999999999786e-01,1.000000000000000000e+00 177 | 3.520000000000000284e+01,-6.622620000000000173e-01,2.892549999999999844e-01,1.000000000000000000e+00 178 | 3.539999999999999858e+01,-6.123809999999999532e-01,2.666990000000000194e-01,1.000000000000000000e+00 179 | 3.560000000000000142e+01,-5.755169999999999453e-01,2.501530000000000142e-01,1.000000000000000000e+00 180 | 3.579999999999999716e+01,-5.250970000000000359e-01,2.276799999999999935e-01,1.000000000000000000e+00 181 | 3.600000000000000000e+01,-4.744059999999999944e-01,2.052470000000000128e-01,1.000000000000000000e+00 182 | 3.620000000000000284e+01,-4.352070000000000105e-01,1.880000000000000004e-01,1.000000000000000000e+00 183 | 3.639999999999999858e+01,-3.841060000000000030e-01,1.656299999999999994e-01,1.000000000000000000e+00 184 | 3.660000000000000142e+01,-3.328119999999999967e-01,1.432889999999999997e-01,1.000000000000000000e+00 185 | 3.679999999999999716e+01,-2.918450000000000211e-01,1.255169999999999897e-01,1.000000000000000000e+00 186 | 3.700000000000000000e+01,-2.402700000000000113e-01,1.032180000000000042e-01,1.000000000000000000e+00 187 | 3.720000000000000284e+01,-1.885759999999999936e-01,8.093630000000000269e-02,1.000000000000000000e+00 188 | 3.739999999999999858e+01,-1.464360000000000106e-01,6.281349999999999434e-02,1.000000000000000000e+00 189 | 3.760000000000000142e+01,-9.459209999999999841e-02,4.055430000000000146e-02,1.000000000000000000e+00 190 | 3.779999999999999716e+01,-4.270129999999999759e-02,1.830189999999999942e-02,1.000000000000000000e+00 191 | 3.800000000000000000e+01,-4.286260000000000056e-16,1.836970000000000048e-16,1.000000000000000000e+00 192 | 3.820000000000000284e+01,5.190990000000000187e-02,-2.224949999999999845e-02,1.000000000000000000e+00 193 | 3.839999999999999858e+01,1.037939999999999974e-01,-4.450280000000000208e-02,1.000000000000000000e+00 194 | 3.860000000000000142e+01,1.464360000000000106e-01,-6.281349999999999434e-02,1.000000000000000000e+00 195 | 3.879999999999999716e+01,1.982079999999999953e-01,-8.508330000000000060e-02,1.000000000000000000e+00 196 | 3.900000000000000000e+01,2.498819999999999930e-01,-1.073680000000000051e-01,1.000000000000000000e+00 197 | 3.920000000000000284e+01,2.918450000000000211e-01,-1.255169999999999897e-01,1.000000000000000000e+00 198 | 3.939999999999999858e+01,3.432749999999999968e-01,-1.478369999999999962e-01,1.000000000000000000e+00 199 | 3.960000000000000142e+01,3.945319999999999938e-01,-1.701840000000000019e-01,1.000000000000000000e+00 200 | 3.979999999999999716e+01,4.352070000000000105e-01,-1.880000000000000004e-01,1.000000000000000000e+00 201 | 4.000000000000000000e+01,4.860869999999999913e-01,-2.104030000000000067e-01,1.000000000000000000e+00 202 | 4.020000000000000284e+01,5.367180000000000284e-01,-2.328439999999999954e-01,1.000000000000000000e+00 203 | 4.039999999999999858e+01,5.755169999999999453e-01,-2.501530000000000142e-01,1.000000000000000000e+00 204 | 4.060000000000000142e+01,6.256359999999999699e-01,-2.726729999999999987e-01,1.000000000000000000e+00 205 | 4.079990000000000094e+01,6.754259999999999708e-01,-2.952440000000000064e-01,1.000000000000000000e+00 206 | 4.099989999999999668e+01,7.117890000000000050e-01,-3.118679999999999786e-01,1.000000000000000000e+00 207 | 4.119989999999999952e+01,7.609329999999999705e-01,-3.345400000000000040e-01,1.000000000000000000e+00 208 | 4.139990000000000236e+01,8.096600000000000463e-01,-3.572799999999999865e-01,1.000000000000000000e+00 209 | 4.159989999999999810e+01,8.430689999999999573e-01,-3.730350000000000055e-01,1.000000000000000000e+00 210 | 4.179990000000000094e+01,8.910130000000000550e-01,-3.959010000000000029e-01,1.000000000000000000e+00 211 | 4.199989999999999668e+01,9.384460000000000024e-01,-4.188520000000000021e-01,1.000000000000000000e+00 212 | 4.219989999999999952e+01,9.684350000000000458e-01,-4.335479999999999889e-01,1.000000000000000000e+00 213 | 4.239990000000000236e+01,1.014950000000000019e+00,-4.566529999999999756e-01,1.000000000000000000e+00 214 | 4.259989999999999810e+01,1.060840000000000005e+00,-4.798649999999999860e-01,1.000000000000000000e+00 215 | 4.279990000000000094e+01,1.087010000000000032e+00,-4.933000000000000163e-01,1.000000000000000000e+00 216 | 4.299989999999999668e+01,1.131839999999999957e+00,-5.166969999999999619e-01,1.000000000000000000e+00 217 | 4.319989999999999952e+01,1.175939999999999985e+00,-5.402259999999999840e-01,1.000000000000000000e+00 218 | 4.339990000000000236e+01,1.197959999999999914e+00,-5.521869999999999834e-01,1.000000000000000000e+00 219 | 4.359989999999999810e+01,1.240850000000000009e+00,-5.759360000000000035e-01,1.000000000000000000e+00 220 | 4.379990000000000094e+01,1.282859999999999889e+00,-5.998489999999999656e-01,1.000000000000000000e+00 221 | 4.399989999999999668e+01,1.320959999999999912e+00,-6.221670000000000256e-01,1.000000000000000000e+00 222 | 4.419989999999999952e+01,1.361099999999999977e+00,-6.464400000000000146e-01,1.000000000000000000e+00 223 | 4.439990000000000236e+01,1.393929999999999891e+00,-6.669530000000000181e-01,1.000000000000000000e+00 224 | 4.459989999999999810e+01,1.432020000000000071e+00,-6.916339999999999710e-01,1.000000000000000000e+00 225 | 4.479990000000000094e+01,1.468869999999999898e+00,-7.165690000000000115e-01,1.000000000000000000e+00 226 | 4.499989999999999668e+01,1.495330000000000048e+00,-7.352370000000000294e-01,1.000000000000000000e+00 227 | 4.519989999999999952e+01,1.529760000000000009e+00,-7.606800000000000228e-01,1.000000000000000000e+00 228 | 4.539990000000000236e+01,1.550859999999999905e+00,-7.770409999999999817e-01,1.000000000000000000e+00 229 | 4.559989999999999810e+01,1.582640000000000047e+00,-8.030500000000000416e-01,1.000000000000000000e+00 230 | 4.579990000000000094e+01,1.612579999999999902e+00,-8.294569999999999999e-01,1.000000000000000000e+00 231 | 4.599989999999999668e+01,1.627450000000000063e+00,-8.434580000000000410e-01,1.000000000000000000e+00 232 | 4.619989999999999952e+01,1.654190000000000049e+00,-8.705530000000000213e-01,1.000000000000000000e+00 233 | 4.639990000000000236e+01,1.676299999999999901e+00,-8.955199999999999827e-01,1.000000000000000000e+00 234 | 4.659989999999999810e+01,1.698110000000000008e+00,-9.236189999999999678e-01,1.000000000000000000e+00 235 | 4.679990000000000094e+01,1.713049999999999962e+00,-9.460669999999999913e-01,1.000000000000000000e+00 236 | 4.699989999999999668e+01,1.728939999999999921e+00,-9.751809999999999645e-01,1.000000000000000000e+00 237 | 4.719989999999999952e+01,1.737330000000000041e+00,-9.949249999999999483e-01,1.000000000000000000e+00 238 | 4.739979999999999905e+01,1.746129999999999960e+00,-1.024799999999999933e+00,1.000000000000000000e+00 239 | 4.759980000000000189e+01,1.748920000000000030e+00,-1.041830000000000034e+00,1.000000000000000000e+00 240 | 4.779979999999999762e+01,1.749619999999999953e+00,-1.071709999999999940e+00,1.000000000000000000e+00 241 | 4.799980000000000047e+01,1.744739999999999958e+00,-1.100970000000000004e+00,1.000000000000000000e+00 242 | 4.819980000000000331e+01,1.734629999999999894e+00,-1.128609999999999891e+00,1.000000000000000000e+00 243 | 4.839979999999999905e+01,1.719379999999999908e+00,-1.155219999999999914e+00,1.000000000000000000e+00 244 | 4.859980000000000189e+01,1.709649999999999892e+00,-1.168420000000000014e+00,1.000000000000000000e+00 245 | 4.879979999999999762e+01,1.687980000000000036e+00,-1.192409999999999970e+00,1.000000000000000000e+00 246 | 4.899980000000000047e+01,1.673200000000000021e+00,-1.206069999999999975e+00,1.000000000000000000e+00 247 | 4.919980000000000331e+01,1.646239999999999926e+00,-1.227440000000000087e+00,1.000000000000000000e+00 248 | 4.939979999999999905e+01,1.625459999999999905e+00,-1.241700000000000026e+00,1.000000000000000000e+00 249 | 4.959980000000000189e+01,1.594100000000000072e+00,-1.260620000000000074e+00,1.000000000000000000e+00 250 | 4.979979999999999762e+01,1.566619999999999902e+00,-1.275279999999999969e+00,1.000000000000000000e+00 251 | 4.999980000000000047e+01,1.531549999999999967e+00,-1.292010000000000103e+00,1.000000000000000000e+00 252 | 5.019980000000000331e+01,1.496990000000000043e+00,-1.306780000000000053e+00,1.000000000000000000e+00 253 | 5.039979999999999905e+01,1.458720000000000017e+00,-1.321550000000000002e+00,1.000000000000000000e+00 254 | 5.059980000000000189e+01,1.437009999999999899e+00,-1.329309999999999992e+00,1.000000000000000000e+00 255 | 5.079979999999999762e+01,1.396609999999999907e+00,-1.342719999999999914e+00,1.000000000000000000e+00 256 | 5.099980000000000047e+01,1.354970000000000008e+00,-1.355350000000000055e+00,1.000000000000000000e+00 257 | 5.119980000000000331e+01,1.327069999999999972e+00,-1.363220000000000098e+00,1.000000000000000000e+00 258 | 5.139979999999999905e+01,1.283609999999999918e+00,-1.374649999999999928e+00,1.000000000000000000e+00 259 | 5.159980000000000189e+01,1.250329999999999941e+00,-1.382789999999999964e+00,1.000000000000000000e+00 260 | 5.179979999999999762e+01,1.205200000000000049e+00,-1.393089999999999939e+00,1.000000000000000000e+00 261 | 5.199980000000000047e+01,1.159210000000000074e+00,-1.402779999999999916e+00,1.000000000000000000e+00 262 | 5.219980000000000331e+01,1.120060000000000056e+00,-1.410460000000000047e+00,1.000000000000000000e+00 263 | 5.239979999999999905e+01,1.072650000000000103e+00,-1.419119999999999937e+00,1.000000000000000000e+00 264 | 5.259980000000000189e+01,1.028620000000000090e+00,-1.426579999999999959e+00,1.000000000000000000e+00 265 | 5.279979999999999762e+01,9.799240000000000173e-01,-1.434250000000000025e+00,1.000000000000000000e+00 266 | 5.299980000000000047e+01,9.306210000000000315e-01,-1.441440000000000055e+00,1.000000000000000000e+00 267 | 5.319980000000000331e+01,8.815530000000000310e-01,-1.448039999999999994e+00,1.000000000000000000e+00 268 | 5.339979999999999905e+01,8.311809999999999476e-01,-1.454309999999999992e+00,1.000000000000000000e+00 269 | 5.359980000000000189e+01,7.803309999999999969e-01,-1.460129999999999928e+00,1.000000000000000000e+00 270 | 5.379979999999999762e+01,7.525089999999999835e-01,-1.463109999999999911e+00,1.000000000000000000e+00 271 | 5.399969999999999715e+01,7.009940000000000060e-01,-1.468259999999999899e+00,1.000000000000000000e+00 272 | 5.419969999999999999e+01,6.490920000000000023e-01,-1.473009999999999931e+00,1.000000000000000000e+00 273 | 5.439970000000000283e+01,6.181170000000000275e-01,-1.475629999999999997e+00,1.000000000000000000e+00 274 | 5.459969999999999857e+01,5.656689999999999774e-01,-1.479729999999999990e+00,1.000000000000000000e+00 275 | 5.479970000000000141e+01,5.129200000000000426e-01,-1.483440000000000092e+00,1.000000000000000000e+00 276 | 5.499969999999999715e+01,4.793109999999999871e-01,-1.485589999999999966e+00,1.000000000000000000e+00 277 | 5.519969999999999999e+01,4.261369999999999880e-01,-1.488669999999999938e+00,1.000000000000000000e+00 278 | 5.539970000000000283e+01,3.727420000000000178e-01,-1.491370000000000084e+00,1.000000000000000000e+00 279 | 5.559969999999999857e+01,3.370589999999999975e-01,-1.492960000000000065e+00,1.000000000000000000e+00 280 | 5.579970000000000141e+01,2.833620000000000028e-01,-1.495039999999999925e+00,1.000000000000000000e+00 281 | 5.599969999999999715e+01,2.295200000000000018e-01,-1.496760000000000090e+00,1.000000000000000000e+00 282 | 5.619969999999999999e+01,1.923570000000000002e-01,-1.497730000000000006e+00,1.000000000000000000e+00 283 | 5.639970000000000283e+01,1.383350000000000135e-01,-1.498830000000000107e+00,1.000000000000000000e+00 284 | 5.659969999999999857e+01,8.424410000000000232e-02,-1.499570000000000070e+00,1.000000000000000000e+00 285 | 5.679970000000000141e+01,4.621589999999999726e-02,-1.499870000000000037e+00,1.000000000000000000e+00 286 | 5.699969999999999715e+01,-7.930970000000000728e-03,-1.500000000000000000e+00,1.000000000000000000e+00 287 | 5.719969999999999999e+01,-6.207390000000000130e-02,-1.499759999999999982e+00,1.000000000000000000e+00 288 | 5.739970000000000283e+01,-1.003399999999999986e-01,-1.499379999999999935e+00,1.000000000000000000e+00 289 | 5.759969999999999857e+01,-1.544129999999999947e-01,-1.498539999999999983e+00,1.000000000000000000e+00 290 | 5.779970000000000141e+01,-2.084090000000000109e-01,-1.497330000000000050e+00,1.000000000000000000e+00 291 | 5.799969999999999715e+01,-2.462820000000000009e-01,-1.496259999999999923e+00,1.000000000000000000e+00 292 | 5.819969999999999999e+01,-3.000820000000000154e-01,-1.494439999999999991e+00,1.000000000000000000e+00 293 | 5.839970000000000283e+01,-3.537290000000000156e-01,-1.492240000000000011e+00,1.000000000000000000e+00 294 | 5.859969999999999857e+01,-3.905850000000000155e-01,-1.490510000000000002e+00,1.000000000000000000e+00 295 | 5.879970000000000141e+01,-4.439089999999999980e-01,-1.487679999999999891e+00,1.000000000000000000e+00 296 | 5.899969999999999715e+01,-4.970029999999999726e-01,-1.484480000000000022e+00,1.000000000000000000e+00 297 | 5.919969999999999999e+01,-5.322339999999999849e-01,-1.482129999999999947e+00,1.000000000000000000e+00 298 | 5.939970000000000283e+01,-5.848769999999999802e-01,-1.478280000000000038e+00,1.000000000000000000e+00 299 | 5.959969999999999857e+01,-6.372069999999999679e-01,-1.474029999999999951e+00,1.000000000000000000e+00 300 | 5.979970000000000141e+01,-6.702320000000000499e-01,-1.471130000000000049e+00,1.000000000000000000e+00 301 | 5.999969999999999715e+01,-7.219809999999999839e-01,-1.466220000000000079e+00,1.000000000000000000e+00 302 | 6.019969999999999999e+01,-7.733299999999999619e-01,-1.460890000000000022e+00,1.000000000000000000e+00 303 | 6.039970000000000283e+01,-8.036039999999999850e-01,-1.457519999999999927e+00,1.000000000000000000e+00 304 | 6.059960000000000235e+01,-8.542399999999999993e-01,-1.451500000000000012e+00,1.000000000000000000e+00 305 | 6.079959999999999809e+01,-9.043809999999999905e-01,-1.445040000000000102e+00,1.000000000000000000e+00 306 | 6.099960000000000093e+01,-9.314069999999999849e-01,-1.441330000000000000e+00,1.000000000000000000e+00 307 | 6.119959999999999667e+01,-9.807010000000000449e-01,-1.434139999999999970e+00,1.000000000000000000e+00 308 | 6.139959999999999951e+01,-1.028620000000000090e+00,-1.426579999999999959e+00,1.000000000000000000e+00 309 | 6.159960000000000235e+01,-1.076670000000000016e+00,-1.418409999999999949e+00,1.000000000000000000e+00 310 | 6.179959999999999809e+01,-1.124020000000000019e+00,-1.409699999999999953e+00,1.000000000000000000e+00 311 | 6.199960000000000093e+01,-1.166719999999999979e+00,-1.401250000000000107e+00,1.000000000000000000e+00 312 | 6.219959999999999667e+01,-1.212569999999999926e+00,-1.391459999999999919e+00,1.000000000000000000e+00 313 | 6.239959999999999951e+01,-1.250329999999999941e+00,-1.382789999999999964e+00,1.000000000000000000e+00 314 | 6.259960000000000235e+01,-1.294529999999999959e+00,-1.371869999999999923e+00,1.000000000000000000e+00 315 | 6.279959999999999809e+01,-1.337720000000000020e+00,-1.360260000000000025e+00,1.000000000000000000e+00 316 | 6.299960000000000093e+01,-1.369450000000000056e+00,-1.351080000000000059e+00,1.000000000000000000e+00 317 | 6.319959999999999667e+01,-1.410670000000000091e+00,-1.338189999999999991e+00,1.000000000000000000e+00 318 | 6.339959999999999951e+01,-1.437009999999999899e+00,-1.329309999999999992e+00,1.000000000000000000e+00 319 | 6.359960000000000235e+01,-1.476069999999999993e+00,-1.315039999999999987e+00,1.000000000000000000e+00 320 | 6.379959999999999809e+01,-1.513660000000000005e+00,-1.299849999999999950e+00,1.000000000000000000e+00 321 | 6.399960000000000093e+01,-1.533689999999999998e+00,-1.291039999999999965e+00,1.000000000000000000e+00 322 | 6.419960000000000377e+01,-1.568650000000000100e+00,-1.274250000000000105e+00,1.000000000000000000e+00 323 | 6.439960000000000662e+01,-1.599760000000000071e+00,-1.257400000000000073e+00,1.000000000000000000e+00 324 | 6.459959999999999525e+01,-1.630710000000000104e+00,-1.238240000000000007e+00,1.000000000000000000e+00 325 | 6.479959999999999809e+01,-1.654369999999999896e+00,-1.221389999999999976e+00,1.000000000000000000e+00 326 | 6.499960000000000093e+01,-1.680469999999999908e+00,-1.199570000000000025e+00,1.000000000000000000e+00 327 | 6.519960000000000377e+01,-1.697019999999999973e+00,-1.183100000000000041e+00,1.000000000000000000e+00 328 | 6.539960000000000662e+01,-1.717160000000000020e+00,-1.158420000000000005e+00,1.000000000000000000e+00 329 | 6.559959999999999525e+01,-1.727270000000000083e+00,-1.142689999999999984e+00,1.000000000000000000e+00 330 | 6.579959999999999809e+01,-1.740189999999999904e+00,-1.115320000000000089e+00,1.000000000000000000e+00 331 | 6.599960000000000093e+01,-1.744869999999999921e+00,-1.100479999999999903e+00,1.000000000000000000e+00 332 | 6.619960000000000377e+01,-1.749619999999999953e+00,-1.071709999999999940e+00,1.000000000000000000e+00 333 | 6.639960000000000662e+01,-1.748920000000000030e+00,-1.041830000000000034e+00,1.000000000000000000e+00 334 | 6.659959999999999525e+01,-1.742909999999999959e+00,-1.011840000000000073e+00,1.000000000000000000e+00 335 | 6.679959999999999809e+01,-1.732110000000000039e+00,-9.821199999999999930e-01,1.000000000000000000e+00 336 | 6.699949999999999761e+01,-1.724089999999999900e+00,-9.654599999999999849e-01,1.000000000000000000e+00 337 | 6.719950000000000045e+01,-1.706979999999999942e+00,-9.365310000000000024e-01,1.000000000000000000e+00 338 | 6.739950000000000330e+01,-1.693659999999999943e+00,-9.175200000000000022e-01,1.000000000000000000e+00 339 | 6.759950000000000614e+01,-1.671229999999999993e+00,-8.895420000000000549e-01,1.000000000000000000e+00 340 | 6.779949999999999477e+01,-1.651759999999999895e+00,-8.679670000000000440e-01,1.000000000000000000e+00 341 | 6.799949999999999761e+01,-1.624810000000000088e+00,-8.409180000000000543e-01,1.000000000000000000e+00 342 | 6.819950000000000045e+01,-1.598549999999999915e+00,-8.168260000000000520e-01,1.000000000000000000e+00 343 | 6.839950000000000330e+01,-1.567709999999999937e+00,-7.906130000000000102e-01,1.000000000000000000e+00 344 | 6.859950000000000614e+01,-1.550859999999999905e+00,-7.770409999999999817e-01,1.000000000000000000e+00 345 | 6.879949999999999477e+01,-1.517419999999999991e+00,-7.513919999999999488e-01,1.000000000000000000e+00 346 | 6.899949999999999761e+01,-1.482450000000000045e+00,-7.260600000000000387e-01,1.000000000000000000e+00 347 | 6.919950000000000045e+01,-1.459459999999999980e+00,-7.100919999999999455e-01,1.000000000000000000e+00 348 | 6.939950000000000330e+01,-1.422269999999999923e+00,-6.852219999999999978e-01,1.000000000000000000e+00 349 | 6.959950000000000614e+01,-1.393929999999999891e+00,-6.669530000000000181e-01,1.000000000000000000e+00 350 | 6.979949999999999477e+01,-1.354719999999999924e+00,-6.425180000000000335e-01,1.000000000000000000e+00 351 | 6.999949999999999761e+01,-1.314389999999999947e+00,-6.182720000000000438e-01,1.000000000000000000e+00 352 | 7.019950000000000045e+01,-1.279770000000000074e+00,-5.980720000000000480e-01,1.000000000000000000e+00 353 | 7.039950000000000330e+01,-1.237640000000000073e+00,-5.741519999999999957e-01,1.000000000000000000e+00 354 | 7.059950000000000614e+01,-1.197959999999999914e+00,-5.521869999999999834e-01,1.000000000000000000e+00 355 | 7.079949999999999477e+01,-1.154409999999999936e+00,-5.286539999999999573e-01,1.000000000000000000e+00 356 | 7.099949999999999761e+01,-1.109909999999999952e+00,-5.051799999999999624e-01,1.000000000000000000e+00 357 | 7.119950000000000045e+01,-1.087010000000000032e+00,-4.933000000000000163e-01,1.000000000000000000e+00 358 | 7.139950000000000330e+01,-1.041169999999999929e+00,-4.698890000000000011e-01,1.000000000000000000e+00 359 | 7.159950000000000614e+01,-9.950449999999999573e-01,-4.467369999999999952e-01,1.000000000000000000e+00 360 | 7.179949999999999477e+01,-9.684350000000000458e-01,-4.335479999999999889e-01,1.000000000000000000e+00 361 | 7.199949999999999761e+01,-9.219779999999999642e-01,-4.108089999999999797e-01,1.000000000000000000e+00 362 | 7.219950000000000045e+01,-8.743300000000000516e-01,-3.878789999999999738e-01,1.000000000000000000e+00 363 | 7.239950000000000330e+01,-8.430689999999999573e-01,-3.730350000000000055e-01,1.000000000000000000e+00 364 | 7.259950000000000614e+01,-7.934020000000000516e-01,-3.497109999999999941e-01,1.000000000000000000e+00 365 | 7.279949999999999477e+01,-7.445359999999999756e-01,-3.269889999999999741e-01,1.000000000000000000e+00 366 | 7.299949999999999761e+01,-7.117890000000000050e-01,-3.118679999999999786e-01,1.000000000000000000e+00 367 | 7.319950000000000045e+01,-6.647060000000000191e-01,-2.903049999999999797e-01,1.000000000000000000e+00 368 | 7.339950000000000330e+01,-6.149289999999999479e-01,-2.677919999999999745e-01,1.000000000000000000e+00 369 | 7.359940000000000282e+01,-5.755169999999999453e-01,-2.501530000000000142e-01,1.000000000000000000e+00 370 | 7.379940000000000566e+01,-5.203550000000000120e-01,-2.256450000000000122e-01,1.000000000000000000e+00 371 | 7.399939999999999429e+01,-4.693359999999999754e-01,-2.030720000000000025e-01,1.000000000000000000e+00 372 | 7.419939999999999714e+01,-4.352070000000000105e-01,-1.880000000000000004e-01,1.000000000000000000e+00 373 | 7.439939999999999998e+01,-3.931999999999999940e-01,-1.695329999999999893e-01,1.000000000000000000e+00 374 | 7.459940000000000282e+01,-3.426899999999999946e-01,-1.475279999999999925e-01,1.000000000000000000e+00 375 | 7.479940000000000566e+01,-2.918450000000000211e-01,-1.255169999999999897e-01,1.000000000000000000e+00 376 | 7.499939999999999429e+01,-2.231410000000000060e-01,-9.586619999999999864e-02,1.000000000000000000e+00 377 | 7.519939999999999714e+01,-1.699299999999999977e-01,-7.293360000000000121e-02,1.000000000000000000e+00 378 | 7.539939999999999998e+01,-1.464360000000000106e-01,-6.281349999999999434e-02,1.000000000000000000e+00 379 | 7.559940000000000282e+01,-1.252810000000000035e-01,-5.372579999999999717e-02,1.000000000000000000e+00 380 | 7.579940000000000566e+01,-7.496909999999999685e-02,-3.215169999999999834e-02,1.000000000000000000e+00 381 | 7.599939999999999429e+01,-8.572530000000000283e-16,-3.673940000000000097e-16,1.000000000000000000e+00 382 | 7.619939999999999714e+01,9.648030000000000495e-02,4.138359999999999955e-02,1.000000000000000000e+00 383 | 7.639939999999999998e+01,1.427029999999999965e-01,6.121179999999999677e-02,1.000000000000000000e+00 384 | -------------------------------------------------------------------------------- /trajs/spiral84.csv: -------------------------------------------------------------------------------- 1 | 0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00 2 | 4.000000000000000222e-01,6.475019999999999387e-02,2.776829999999999923e-02,1.000000000000000000e+00 3 | 8.000000000000000444e-01,2.334350000000000036e-01,1.001819999999999933e-01,1.000000000000000000e+00 4 | 1.199999999999999956e+00,2.918450000000000211e-01,1.255169999999999897e-01,1.000000000000000000e+00 5 | 1.600000000000000089e+00,3.805999999999999939e-01,1.642129999999999979e-01,1.000000000000000000e+00 6 | 2.000000000000000000e+00,4.966960000000000264e-01,2.149500000000000022e-01,1.000000000000000000e+00 7 | 2.399999999999999911e+00,5.755169999999999453e-01,2.501530000000000142e-01,1.000000000000000000e+00 8 | 2.799999999999999822e+00,6.719049999999999745e-01,2.937350000000000239e-01,1.000000000000000000e+00 9 | 3.200000000000000178e+00,7.637080000000000535e-01,3.357299999999999729e-01,1.000000000000000000e+00 10 | 3.600000000000000089e+00,8.430689999999999573e-01,3.730350000000000055e-01,1.000000000000000000e+00 11 | 4.000000000000000000e+00,9.376480000000000370e-01,4.185099999999999931e-01,1.000000000000000000e+00 12 | 4.400000000000000355e+00,1.015630000000000033e+00,4.569480000000000208e-01,1.000000000000000000e+00 13 | 4.799999999999999822e+00,1.087010000000000032e+00,4.933000000000000163e-01,1.000000000000000000e+00 14 | 5.200000000000000178e+00,1.175780000000000047e+00,5.401580000000000270e-01,1.000000000000000000e+00 15 | 5.599999999999999645e+00,1.241009999999999946e+00,5.760049999999999892e-01,1.000000000000000000e+00 16 | 6.000000000000000000e+00,1.320899999999999963e+00,6.221430000000000016e-01,1.000000000000000000e+00 17 | 6.400000000000000355e+00,1.393929999999999891e+00,6.669530000000000181e-01,1.000000000000000000e+00 18 | 6.799999999999999822e+00,1.468879999999999963e+00,7.165719999999999867e-01,1.000000000000000000e+00 19 | 7.200000000000000178e+00,1.529749999999999943e+00,7.606749999999999901e-01,1.000000000000000000e+00 20 | 7.599999999999999645e+00,1.582640000000000047e+00,8.030530000000000168e-01,1.000000000000000000e+00 21 | 7.999990000000000379e+00,1.627450000000000063e+00,8.434570000000000123e-01,1.000000000000000000e+00 22 | 8.400000000000000355e+00,1.676299999999999901e+00,8.955210000000000115e-01,1.000000000000000000e+00 23 | 8.800000000000000711e+00,1.713049999999999962e+00,9.460669999999999913e-01,1.000000000000000000e+00 24 | 9.199999999999999289e+00,1.737330000000000041e+00,9.949249999999999483e-01,1.000000000000000000e+00 25 | 9.599999999999999645e+00,1.748920000000000030e+00,1.041830000000000034e+00,1.000000000000000000e+00 26 | 1.000000000000000000e+01,1.744739999999999958e+00,1.100970000000000004e+00,1.000000000000000000e+00 27 | 1.040000000000000036e+01,1.719379999999999908e+00,1.155219999999999914e+00,1.000000000000000000e+00 28 | 1.080000000000000071e+01,1.687980000000000036e+00,1.192409999999999970e+00,1.000000000000000000e+00 29 | 1.119999999999999929e+01,1.646239999999999926e+00,1.227440000000000087e+00,1.000000000000000000e+00 30 | 1.159999999999999964e+01,1.594100000000000072e+00,1.260620000000000074e+00,1.000000000000000000e+00 31 | 1.200000000000000000e+01,1.531549999999999967e+00,1.292010000000000103e+00,1.000000000000000000e+00 32 | 1.240000000000000036e+01,1.458720000000000017e+00,1.321550000000000002e+00,1.000000000000000000e+00 33 | 1.280000000000000071e+01,1.396609999999999907e+00,1.342719999999999914e+00,1.000000000000000000e+00 34 | 1.319999999999999929e+01,1.327069999999999972e+00,1.363220000000000098e+00,1.000000000000000000e+00 35 | 1.359999999999999964e+01,1.250329999999999941e+00,1.382789999999999964e+00,1.000000000000000000e+00 36 | 1.400000000000000000e+01,1.159210000000000074e+00,1.402779999999999916e+00,1.000000000000000000e+00 37 | 1.440000000000000036e+01,1.072650000000000103e+00,1.419119999999999937e+00,1.000000000000000000e+00 38 | 1.480000000000000071e+01,9.799240000000000173e-01,1.434250000000000025e+00,1.000000000000000000e+00 39 | 1.519999999999999929e+01,8.815530000000000310e-01,1.448039999999999994e+00,1.000000000000000000e+00 40 | 1.559999999999999964e+01,7.803309999999999969e-01,1.460129999999999928e+00,1.000000000000000000e+00 41 | 1.600000000000000000e+01,7.009940000000000060e-01,1.468259999999999899e+00,1.000000000000000000e+00 42 | 1.639999999999999858e+01,6.181170000000000275e-01,1.475629999999999997e+00,1.000000000000000000e+00 43 | 1.680000000000000071e+01,5.129200000000000426e-01,1.483440000000000092e+00,1.000000000000000000e+00 44 | 1.719999999999999929e+01,4.261369999999999880e-01,1.488669999999999938e+00,1.000000000000000000e+00 45 | 1.760000000000000142e+01,3.370589999999999975e-01,1.492960000000000065e+00,1.000000000000000000e+00 46 | 1.800000000000000000e+01,2.295200000000000018e-01,1.496760000000000090e+00,1.000000000000000000e+00 47 | 1.839999999999999858e+01,1.383350000000000135e-01,1.498830000000000107e+00,1.000000000000000000e+00 48 | 1.880000000000000071e+01,4.621589999999999726e-02,1.499870000000000037e+00,1.000000000000000000e+00 49 | 1.919999999999999929e+01,-6.207390000000000130e-02,1.499759999999999982e+00,1.000000000000000000e+00 50 | 1.960000000000000142e+01,-1.544129999999999947e-01,1.498539999999999983e+00,1.000000000000000000e+00 51 | 2.000000000000000000e+01,-2.462820000000000009e-01,1.496259999999999923e+00,1.000000000000000000e+00 52 | 2.039999999999999858e+01,-3.537290000000000156e-01,1.492240000000000011e+00,1.000000000000000000e+00 53 | 2.080000000000000071e+01,-4.439089999999999980e-01,1.487679999999999891e+00,1.000000000000000000e+00 54 | 2.119999999999999929e+01,-5.322339999999999849e-01,1.482129999999999947e+00,1.000000000000000000e+00 55 | 2.160000000000000142e+01,-6.372069999999999679e-01,1.474029999999999951e+00,1.000000000000000000e+00 56 | 2.200000000000000000e+01,-7.219809999999999839e-01,1.466220000000000079e+00,1.000000000000000000e+00 57 | 2.239999999999999858e+01,-8.036039999999999850e-01,1.457519999999999927e+00,1.000000000000000000e+00 58 | 2.280010000000000048e+01,-9.043809999999999905e-01,1.445040000000000102e+00,1.000000000000000000e+00 59 | 2.320009999999999906e+01,-9.807010000000000449e-01,1.434139999999999970e+00,1.000000000000000000e+00 60 | 2.360010000000000119e+01,-1.076670000000000016e+00,1.418409999999999949e+00,1.000000000000000000e+00 61 | 2.400009999999999977e+01,-1.166719999999999979e+00,1.401250000000000107e+00,1.000000000000000000e+00 62 | 2.440009999999999835e+01,-1.250329999999999941e+00,1.382789999999999964e+00,1.000000000000000000e+00 63 | 2.480010000000000048e+01,-1.337720000000000020e+00,1.360260000000000025e+00,1.000000000000000000e+00 64 | 2.520009999999999906e+01,-1.410670000000000091e+00,1.338189999999999991e+00,1.000000000000000000e+00 65 | 2.560010000000000119e+01,-1.476069999999999993e+00,1.315039999999999987e+00,1.000000000000000000e+00 66 | 2.600009999999999977e+01,-1.533689999999999998e+00,1.291039999999999965e+00,1.000000000000000000e+00 67 | 2.640009999999999835e+01,-1.599760000000000071e+00,1.257400000000000073e+00,1.000000000000000000e+00 68 | 2.680010000000000048e+01,-1.654369999999999896e+00,1.221389999999999976e+00,1.000000000000000000e+00 69 | 2.720009999999999906e+01,-1.697019999999999973e+00,1.183100000000000041e+00,1.000000000000000000e+00 70 | 2.760010000000000119e+01,-1.727270000000000083e+00,1.142689999999999984e+00,1.000000000000000000e+00 71 | 2.800009999999999977e+01,-1.744869999999999921e+00,1.100479999999999903e+00,1.000000000000000000e+00 72 | 2.840009999999999835e+01,-1.748920000000000030e+00,1.041830000000000034e+00,1.000000000000000000e+00 73 | 2.880010000000000048e+01,-1.732110000000000039e+00,9.821199999999999930e-01,1.000000000000000000e+00 74 | 2.920009999999999906e+01,-1.706979999999999942e+00,9.365310000000000024e-01,1.000000000000000000e+00 75 | 2.960010000000000119e+01,-1.671229999999999993e+00,8.895429999999999726e-01,1.000000000000000000e+00 76 | 3.000009999999999977e+01,-1.624810000000000088e+00,8.409180000000000543e-01,1.000000000000000000e+00 77 | 3.040009999999999835e+01,-1.567720000000000002e+00,7.906140000000000390e-01,1.000000000000000000e+00 78 | 3.080010000000000048e+01,-1.517409999999999926e+00,7.513879999999999448e-01,1.000000000000000000e+00 79 | 3.120009999999999906e+01,-1.459470000000000045e+00,7.100959999999999495e-01,1.000000000000000000e+00 80 | 3.160010000000000119e+01,-1.393929999999999891e+00,6.669530000000000181e-01,1.000000000000000000e+00 81 | 3.200010000000000332e+01,-1.314370000000000038e+00,6.182630000000000070e-01,1.000000000000000000e+00 82 | 3.240010000000000190e+01,-1.237729999999999997e+00,5.741889999999999494e-01,1.000000000000000000e+00 83 | 3.280010000000000048e+01,-1.154249999999999998e+00,5.285870000000000291e-01,1.000000000000000000e+00 84 | 3.320009999999999906e+01,-1.087010000000000032e+00,4.933000000000000163e-01,1.000000000000000000e+00 85 | 3.360009999999999764e+01,-9.953269999999999618e-01,4.468579999999999774e-01,1.000000000000000000e+00 86 | 3.400010000000000332e+01,-9.213479999999999448e-01,4.105389999999999873e-01,1.000000000000000000e+00 87 | 3.439999999999999858e+01,-8.430689999999999573e-01,3.730350000000000055e-01,1.000000000000000000e+00 88 | 3.479999999999999716e+01,-7.457909999999999817e-01,3.275279999999999858e-01,1.000000000000000000e+00 89 | 3.520000000000000284e+01,-6.622620000000000173e-01,2.892549999999999844e-01,1.000000000000000000e+00 90 | 3.560000000000000142e+01,-5.755169999999999453e-01,2.501530000000000142e-01,1.000000000000000000e+00 91 | 3.600000000000000000e+01,-4.744059999999999944e-01,2.052470000000000128e-01,1.000000000000000000e+00 92 | 3.639999999999999858e+01,-3.841060000000000030e-01,1.656299999999999994e-01,1.000000000000000000e+00 93 | 3.679999999999999716e+01,-2.918450000000000211e-01,1.255169999999999897e-01,1.000000000000000000e+00 94 | 3.720000000000000284e+01,-1.885759999999999936e-01,8.093630000000000269e-02,1.000000000000000000e+00 95 | 3.760000000000000142e+01,-9.459209999999999841e-02,4.055430000000000146e-02,1.000000000000000000e+00 96 | 3.800000000000000000e+01,-4.286260000000000056e-16,1.836970000000000048e-16,1.000000000000000000e+00 97 | 3.839999999999999858e+01,1.037939999999999974e-01,-4.450280000000000208e-02,1.000000000000000000e+00 98 | 3.879999999999999716e+01,1.982079999999999953e-01,-8.508330000000000060e-02,1.000000000000000000e+00 99 | 3.920000000000000284e+01,2.918450000000000211e-01,-1.255169999999999897e-01,1.000000000000000000e+00 100 | 3.960000000000000142e+01,3.945319999999999938e-01,-1.701840000000000019e-01,1.000000000000000000e+00 101 | 4.000000000000000000e+01,4.860869999999999913e-01,-2.104030000000000067e-01,1.000000000000000000e+00 102 | 4.039999999999999858e+01,5.755169999999999453e-01,-2.501530000000000142e-01,1.000000000000000000e+00 103 | 4.079990000000000094e+01,6.754259999999999708e-01,-2.952440000000000064e-01,1.000000000000000000e+00 104 | 4.119989999999999952e+01,7.609329999999999705e-01,-3.345400000000000040e-01,1.000000000000000000e+00 105 | 4.159989999999999810e+01,8.430689999999999573e-01,-3.730350000000000055e-01,1.000000000000000000e+00 106 | 4.199989999999999668e+01,9.384460000000000024e-01,-4.188520000000000021e-01,1.000000000000000000e+00 107 | 4.239990000000000236e+01,1.014950000000000019e+00,-4.566529999999999756e-01,1.000000000000000000e+00 108 | 4.279990000000000094e+01,1.087010000000000032e+00,-4.933000000000000163e-01,1.000000000000000000e+00 109 | 4.319989999999999952e+01,1.175939999999999985e+00,-5.402259999999999840e-01,1.000000000000000000e+00 110 | 4.359989999999999810e+01,1.240850000000000009e+00,-5.759360000000000035e-01,1.000000000000000000e+00 111 | 4.399989999999999668e+01,1.320959999999999912e+00,-6.221670000000000256e-01,1.000000000000000000e+00 112 | 4.439990000000000236e+01,1.393929999999999891e+00,-6.669530000000000181e-01,1.000000000000000000e+00 113 | 4.479990000000000094e+01,1.468869999999999898e+00,-7.165690000000000115e-01,1.000000000000000000e+00 114 | 4.519989999999999952e+01,1.529760000000000009e+00,-7.606800000000000228e-01,1.000000000000000000e+00 115 | 4.559989999999999810e+01,1.582640000000000047e+00,-8.030500000000000416e-01,1.000000000000000000e+00 116 | 4.599989999999999668e+01,1.627450000000000063e+00,-8.434580000000000410e-01,1.000000000000000000e+00 117 | 4.639990000000000236e+01,1.676299999999999901e+00,-8.955199999999999827e-01,1.000000000000000000e+00 118 | 4.679990000000000094e+01,1.713049999999999962e+00,-9.460669999999999913e-01,1.000000000000000000e+00 119 | 4.719989999999999952e+01,1.737330000000000041e+00,-9.949249999999999483e-01,1.000000000000000000e+00 120 | 4.759980000000000189e+01,1.748920000000000030e+00,-1.041830000000000034e+00,1.000000000000000000e+00 121 | 4.799980000000000047e+01,1.744739999999999958e+00,-1.100970000000000004e+00,1.000000000000000000e+00 122 | 4.839979999999999905e+01,1.719379999999999908e+00,-1.155219999999999914e+00,1.000000000000000000e+00 123 | 4.879979999999999762e+01,1.687980000000000036e+00,-1.192409999999999970e+00,1.000000000000000000e+00 124 | 4.919980000000000331e+01,1.646239999999999926e+00,-1.227440000000000087e+00,1.000000000000000000e+00 125 | 4.959980000000000189e+01,1.594100000000000072e+00,-1.260620000000000074e+00,1.000000000000000000e+00 126 | 4.999980000000000047e+01,1.531549999999999967e+00,-1.292010000000000103e+00,1.000000000000000000e+00 127 | 5.039979999999999905e+01,1.458720000000000017e+00,-1.321550000000000002e+00,1.000000000000000000e+00 128 | 5.079979999999999762e+01,1.396609999999999907e+00,-1.342719999999999914e+00,1.000000000000000000e+00 129 | 5.119980000000000331e+01,1.327069999999999972e+00,-1.363220000000000098e+00,1.000000000000000000e+00 130 | 5.159980000000000189e+01,1.250329999999999941e+00,-1.382789999999999964e+00,1.000000000000000000e+00 131 | 5.199980000000000047e+01,1.159210000000000074e+00,-1.402779999999999916e+00,1.000000000000000000e+00 132 | 5.239979999999999905e+01,1.072650000000000103e+00,-1.419119999999999937e+00,1.000000000000000000e+00 133 | 5.279979999999999762e+01,9.799240000000000173e-01,-1.434250000000000025e+00,1.000000000000000000e+00 134 | 5.319980000000000331e+01,8.815530000000000310e-01,-1.448039999999999994e+00,1.000000000000000000e+00 135 | 5.359980000000000189e+01,7.803309999999999969e-01,-1.460129999999999928e+00,1.000000000000000000e+00 136 | 5.399969999999999715e+01,7.009940000000000060e-01,-1.468259999999999899e+00,1.000000000000000000e+00 137 | 5.439970000000000283e+01,6.181170000000000275e-01,-1.475629999999999997e+00,1.000000000000000000e+00 138 | 5.479970000000000141e+01,5.129200000000000426e-01,-1.483440000000000092e+00,1.000000000000000000e+00 139 | 5.519969999999999999e+01,4.261369999999999880e-01,-1.488669999999999938e+00,1.000000000000000000e+00 140 | 5.559969999999999857e+01,3.370589999999999975e-01,-1.492960000000000065e+00,1.000000000000000000e+00 141 | 5.599969999999999715e+01,2.295200000000000018e-01,-1.496760000000000090e+00,1.000000000000000000e+00 142 | 5.639970000000000283e+01,1.383350000000000135e-01,-1.498830000000000107e+00,1.000000000000000000e+00 143 | 5.679970000000000141e+01,4.621589999999999726e-02,-1.499870000000000037e+00,1.000000000000000000e+00 144 | 5.719969999999999999e+01,-6.207390000000000130e-02,-1.499759999999999982e+00,1.000000000000000000e+00 145 | 5.759969999999999857e+01,-1.544129999999999947e-01,-1.498539999999999983e+00,1.000000000000000000e+00 146 | 5.799969999999999715e+01,-2.462820000000000009e-01,-1.496259999999999923e+00,1.000000000000000000e+00 147 | 5.839970000000000283e+01,-3.537290000000000156e-01,-1.492240000000000011e+00,1.000000000000000000e+00 148 | 5.879970000000000141e+01,-4.439089999999999980e-01,-1.487679999999999891e+00,1.000000000000000000e+00 149 | 5.919969999999999999e+01,-5.322339999999999849e-01,-1.482129999999999947e+00,1.000000000000000000e+00 150 | 5.959969999999999857e+01,-6.372069999999999679e-01,-1.474029999999999951e+00,1.000000000000000000e+00 151 | 5.999969999999999715e+01,-7.219809999999999839e-01,-1.466220000000000079e+00,1.000000000000000000e+00 152 | 6.039970000000000283e+01,-8.036039999999999850e-01,-1.457519999999999927e+00,1.000000000000000000e+00 153 | 6.079959999999999809e+01,-9.043809999999999905e-01,-1.445040000000000102e+00,1.000000000000000000e+00 154 | 6.119959999999999667e+01,-9.807010000000000449e-01,-1.434139999999999970e+00,1.000000000000000000e+00 155 | 6.159960000000000235e+01,-1.076670000000000016e+00,-1.418409999999999949e+00,1.000000000000000000e+00 156 | 6.199960000000000093e+01,-1.166719999999999979e+00,-1.401250000000000107e+00,1.000000000000000000e+00 157 | 6.239959999999999951e+01,-1.250329999999999941e+00,-1.382789999999999964e+00,1.000000000000000000e+00 158 | 6.279959999999999809e+01,-1.337720000000000020e+00,-1.360260000000000025e+00,1.000000000000000000e+00 159 | 6.319959999999999667e+01,-1.410670000000000091e+00,-1.338189999999999991e+00,1.000000000000000000e+00 160 | 6.359960000000000235e+01,-1.476069999999999993e+00,-1.315039999999999987e+00,1.000000000000000000e+00 161 | 6.399960000000000093e+01,-1.533689999999999998e+00,-1.291039999999999965e+00,1.000000000000000000e+00 162 | 6.439960000000000662e+01,-1.599760000000000071e+00,-1.257400000000000073e+00,1.000000000000000000e+00 163 | 6.479959999999999809e+01,-1.654369999999999896e+00,-1.221389999999999976e+00,1.000000000000000000e+00 164 | 6.519960000000000377e+01,-1.697019999999999973e+00,-1.183100000000000041e+00,1.000000000000000000e+00 165 | 6.559959999999999525e+01,-1.727270000000000083e+00,-1.142689999999999984e+00,1.000000000000000000e+00 166 | 6.599960000000000093e+01,-1.744869999999999921e+00,-1.100479999999999903e+00,1.000000000000000000e+00 167 | 6.639960000000000662e+01,-1.748920000000000030e+00,-1.041830000000000034e+00,1.000000000000000000e+00 168 | 6.679959999999999809e+01,-1.732110000000000039e+00,-9.821199999999999930e-01,1.000000000000000000e+00 169 | 6.719950000000000045e+01,-1.706979999999999942e+00,-9.365310000000000024e-01,1.000000000000000000e+00 170 | 6.759950000000000614e+01,-1.671229999999999993e+00,-8.895420000000000549e-01,1.000000000000000000e+00 171 | 6.799949999999999761e+01,-1.624810000000000088e+00,-8.409180000000000543e-01,1.000000000000000000e+00 172 | 6.839950000000000330e+01,-1.567709999999999937e+00,-7.906130000000000102e-01,1.000000000000000000e+00 173 | 6.879949999999999477e+01,-1.517419999999999991e+00,-7.513919999999999488e-01,1.000000000000000000e+00 174 | 6.919950000000000045e+01,-1.459459999999999980e+00,-7.100919999999999455e-01,1.000000000000000000e+00 175 | 6.959950000000000614e+01,-1.393929999999999891e+00,-6.669530000000000181e-01,1.000000000000000000e+00 176 | 6.999949999999999761e+01,-1.314389999999999947e+00,-6.182720000000000438e-01,1.000000000000000000e+00 177 | 7.039950000000000330e+01,-1.237640000000000073e+00,-5.741519999999999957e-01,1.000000000000000000e+00 178 | 7.079949999999999477e+01,-1.154409999999999936e+00,-5.286539999999999573e-01,1.000000000000000000e+00 179 | 7.119950000000000045e+01,-1.087010000000000032e+00,-4.933000000000000163e-01,1.000000000000000000e+00 180 | 7.159950000000000614e+01,-9.950449999999999573e-01,-4.467369999999999952e-01,1.000000000000000000e+00 181 | 7.199949999999999761e+01,-9.219779999999999642e-01,-4.108089999999999797e-01,1.000000000000000000e+00 182 | 7.239950000000000330e+01,-8.430689999999999573e-01,-3.730350000000000055e-01,1.000000000000000000e+00 183 | 7.279949999999999477e+01,-7.445359999999999756e-01,-3.269889999999999741e-01,1.000000000000000000e+00 184 | 7.319950000000000045e+01,-6.647060000000000191e-01,-2.903049999999999797e-01,1.000000000000000000e+00 185 | 7.359940000000000282e+01,-5.755169999999999453e-01,-2.501530000000000142e-01,1.000000000000000000e+00 186 | 7.399939999999999429e+01,-4.693359999999999754e-01,-2.030720000000000025e-01,1.000000000000000000e+00 187 | 7.439939999999999998e+01,-3.931999999999999940e-01,-1.695329999999999893e-01,1.000000000000000000e+00 188 | 7.479940000000000566e+01,-2.918450000000000211e-01,-1.255169999999999897e-01,1.000000000000000000e+00 189 | 7.519939999999999714e+01,-1.699299999999999977e-01,-7.293360000000000121e-02,1.000000000000000000e+00 190 | 7.559940000000000282e+01,-1.252810000000000035e-01,-5.372579999999999717e-02,1.000000000000000000e+00 191 | 7.599939999999999429e+01,-8.572530000000000283e-16,-3.673940000000000097e-16,1.000000000000000000e+00 192 | 7.639939999999999998e+01,1.427029999999999965e-01,6.121179999999999677e-02,1.000000000000000000e+00 193 | 7.649939999999999429e+01,1.463210000000000066e-01,6.276410000000000322e-02,1.000000000000000000e+00 194 | -------------------------------------------------------------------------------- /trajs/spiral85.csv: -------------------------------------------------------------------------------- 1 | 0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00 2 | 4.000000000000000222e-01,6.475019999999999387e-02,2.776829999999999923e-02,1.000000000000000000e+00 3 | 8.000000000000000444e-01,2.334350000000000036e-01,1.001819999999999933e-01,1.000000000000000000e+00 4 | 1.199999999999999956e+00,2.918450000000000211e-01,1.255169999999999897e-01,1.000000000000000000e+00 5 | 1.600000000000000089e+00,3.805999999999999939e-01,1.642129999999999979e-01,1.000000000000000000e+00 6 | 2.000000000000000000e+00,4.966960000000000264e-01,2.149500000000000022e-01,1.000000000000000000e+00 7 | 2.399999999999999911e+00,5.755169999999999453e-01,2.501530000000000142e-01,1.000000000000000000e+00 8 | 2.799999999999999822e+00,6.719049999999999745e-01,2.937350000000000239e-01,1.000000000000000000e+00 9 | 3.200000000000000178e+00,7.637080000000000535e-01,3.357299999999999729e-01,1.000000000000000000e+00 10 | 3.600000000000000089e+00,8.430689999999999573e-01,3.730350000000000055e-01,1.000000000000000000e+00 11 | 4.000000000000000000e+00,9.376480000000000370e-01,4.185099999999999931e-01,1.000000000000000000e+00 12 | 4.400000000000000355e+00,1.015630000000000033e+00,4.569480000000000208e-01,1.000000000000000000e+00 13 | 4.799999999999999822e+00,1.087010000000000032e+00,4.933000000000000163e-01,1.000000000000000000e+00 14 | 5.200000000000000178e+00,1.175780000000000047e+00,5.401580000000000270e-01,1.000000000000000000e+00 15 | 5.599999999999999645e+00,1.241009999999999946e+00,5.760049999999999892e-01,1.000000000000000000e+00 16 | 6.000000000000000000e+00,1.320899999999999963e+00,6.221430000000000016e-01,1.000000000000000000e+00 17 | 6.400000000000000355e+00,1.393929999999999891e+00,6.669530000000000181e-01,1.000000000000000000e+00 18 | 6.799999999999999822e+00,1.468879999999999963e+00,7.165719999999999867e-01,1.000000000000000000e+00 19 | 7.200000000000000178e+00,1.529749999999999943e+00,7.606749999999999901e-01,1.000000000000000000e+00 20 | 7.599999999999999645e+00,1.582640000000000047e+00,8.030530000000000168e-01,1.000000000000000000e+00 21 | 7.999990000000000379e+00,1.627450000000000063e+00,8.434570000000000123e-01,1.000000000000000000e+00 22 | 8.400000000000000355e+00,1.676299999999999901e+00,8.955210000000000115e-01,1.000000000000000000e+00 23 | 8.800000000000000711e+00,1.713049999999999962e+00,9.460669999999999913e-01,1.000000000000000000e+00 24 | 9.199999999999999289e+00,1.737330000000000041e+00,9.949249999999999483e-01,1.000000000000000000e+00 25 | 9.599999999999999645e+00,1.748920000000000030e+00,1.041830000000000034e+00,1.000000000000000000e+00 26 | 1.000000000000000000e+01,1.744739999999999958e+00,1.100970000000000004e+00,1.000000000000000000e+00 27 | 1.040000000000000036e+01,1.719379999999999908e+00,1.155219999999999914e+00,1.000000000000000000e+00 28 | 1.080000000000000071e+01,1.687980000000000036e+00,1.192409999999999970e+00,1.000000000000000000e+00 29 | 1.119999999999999929e+01,1.646239999999999926e+00,1.227440000000000087e+00,1.000000000000000000e+00 30 | 1.159999999999999964e+01,1.594100000000000072e+00,1.260620000000000074e+00,1.000000000000000000e+00 31 | 1.200000000000000000e+01,1.531549999999999967e+00,1.292010000000000103e+00,1.000000000000000000e+00 32 | 1.240000000000000036e+01,1.458720000000000017e+00,1.321550000000000002e+00,1.000000000000000000e+00 33 | 1.280000000000000071e+01,1.396609999999999907e+00,1.342719999999999914e+00,1.000000000000000000e+00 34 | 1.319999999999999929e+01,1.327069999999999972e+00,1.363220000000000098e+00,1.000000000000000000e+00 35 | 1.359999999999999964e+01,1.250329999999999941e+00,1.382789999999999964e+00,1.000000000000000000e+00 36 | 1.400000000000000000e+01,1.159210000000000074e+00,1.402779999999999916e+00,1.000000000000000000e+00 37 | 1.440000000000000036e+01,1.072650000000000103e+00,1.419119999999999937e+00,1.000000000000000000e+00 38 | 1.480000000000000071e+01,9.799240000000000173e-01,1.434250000000000025e+00,1.000000000000000000e+00 39 | 1.519999999999999929e+01,8.815530000000000310e-01,1.448039999999999994e+00,1.000000000000000000e+00 40 | 1.559999999999999964e+01,7.803309999999999969e-01,1.460129999999999928e+00,1.000000000000000000e+00 41 | 1.600000000000000000e+01,7.009940000000000060e-01,1.468259999999999899e+00,1.000000000000000000e+00 42 | 1.639999999999999858e+01,6.181170000000000275e-01,1.475629999999999997e+00,1.000000000000000000e+00 43 | 1.680000000000000071e+01,5.129200000000000426e-01,1.483440000000000092e+00,1.000000000000000000e+00 44 | 1.719999999999999929e+01,4.261369999999999880e-01,1.488669999999999938e+00,1.000000000000000000e+00 45 | 1.760000000000000142e+01,3.370589999999999975e-01,1.492960000000000065e+00,1.000000000000000000e+00 46 | 1.800000000000000000e+01,2.295200000000000018e-01,1.496760000000000090e+00,1.000000000000000000e+00 47 | 1.839999999999999858e+01,1.383350000000000135e-01,1.498830000000000107e+00,1.000000000000000000e+00 48 | 1.880000000000000071e+01,4.621589999999999726e-02,1.499870000000000037e+00,1.000000000000000000e+00 49 | 1.919999999999999929e+01,-6.207390000000000130e-02,1.499759999999999982e+00,1.000000000000000000e+00 50 | 1.960000000000000142e+01,-1.544129999999999947e-01,1.498539999999999983e+00,1.000000000000000000e+00 51 | 2.000000000000000000e+01,-2.462820000000000009e-01,1.496259999999999923e+00,1.000000000000000000e+00 52 | 2.039999999999999858e+01,-3.537290000000000156e-01,1.492240000000000011e+00,1.000000000000000000e+00 53 | 2.080000000000000071e+01,-4.439089999999999980e-01,1.487679999999999891e+00,1.000000000000000000e+00 54 | 2.119999999999999929e+01,-5.322339999999999849e-01,1.482129999999999947e+00,1.000000000000000000e+00 55 | 2.160000000000000142e+01,-6.372069999999999679e-01,1.474029999999999951e+00,1.000000000000000000e+00 56 | 2.200000000000000000e+01,-7.219809999999999839e-01,1.466220000000000079e+00,1.000000000000000000e+00 57 | 2.239999999999999858e+01,-8.036039999999999850e-01,1.457519999999999927e+00,1.000000000000000000e+00 58 | 2.280010000000000048e+01,-9.043809999999999905e-01,1.445040000000000102e+00,1.000000000000000000e+00 59 | 2.320009999999999906e+01,-9.807010000000000449e-01,1.434139999999999970e+00,1.000000000000000000e+00 60 | 2.360010000000000119e+01,-1.076670000000000016e+00,1.418409999999999949e+00,1.000000000000000000e+00 61 | 2.400009999999999977e+01,-1.166719999999999979e+00,1.401250000000000107e+00,1.000000000000000000e+00 62 | 2.440009999999999835e+01,-1.250329999999999941e+00,1.382789999999999964e+00,1.000000000000000000e+00 63 | 2.480010000000000048e+01,-1.337720000000000020e+00,1.360260000000000025e+00,1.000000000000000000e+00 64 | 2.520009999999999906e+01,-1.410670000000000091e+00,1.338189999999999991e+00,1.000000000000000000e+00 65 | 2.560010000000000119e+01,-1.476069999999999993e+00,1.315039999999999987e+00,1.000000000000000000e+00 66 | 2.600009999999999977e+01,-1.533689999999999998e+00,1.291039999999999965e+00,1.000000000000000000e+00 67 | 2.640009999999999835e+01,-1.599760000000000071e+00,1.257400000000000073e+00,1.000000000000000000e+00 68 | 2.680010000000000048e+01,-1.654369999999999896e+00,1.221389999999999976e+00,1.000000000000000000e+00 69 | 2.720009999999999906e+01,-1.697019999999999973e+00,1.183100000000000041e+00,1.000000000000000000e+00 70 | 2.760010000000000119e+01,-1.727270000000000083e+00,1.142689999999999984e+00,1.000000000000000000e+00 71 | 2.800009999999999977e+01,-1.744869999999999921e+00,1.100479999999999903e+00,1.000000000000000000e+00 72 | 2.840009999999999835e+01,-1.748920000000000030e+00,1.041830000000000034e+00,1.000000000000000000e+00 73 | 2.880010000000000048e+01,-1.732110000000000039e+00,9.821199999999999930e-01,1.000000000000000000e+00 74 | 2.920009999999999906e+01,-1.706979999999999942e+00,9.365310000000000024e-01,1.000000000000000000e+00 75 | 2.960010000000000119e+01,-1.671229999999999993e+00,8.895429999999999726e-01,1.000000000000000000e+00 76 | 3.000009999999999977e+01,-1.624810000000000088e+00,8.409180000000000543e-01,1.000000000000000000e+00 77 | 3.040009999999999835e+01,-1.567720000000000002e+00,7.906140000000000390e-01,1.000000000000000000e+00 78 | 3.080010000000000048e+01,-1.517409999999999926e+00,7.513879999999999448e-01,1.000000000000000000e+00 79 | 3.120009999999999906e+01,-1.459470000000000045e+00,7.100959999999999495e-01,1.000000000000000000e+00 80 | 3.160010000000000119e+01,-1.393929999999999891e+00,6.669530000000000181e-01,1.000000000000000000e+00 81 | 3.200010000000000332e+01,-1.314370000000000038e+00,6.182630000000000070e-01,1.000000000000000000e+00 82 | 3.240010000000000190e+01,-1.237729999999999997e+00,5.741889999999999494e-01,1.000000000000000000e+00 83 | 3.280010000000000048e+01,-1.154249999999999998e+00,5.285870000000000291e-01,1.000000000000000000e+00 84 | 3.320009999999999906e+01,-1.087010000000000032e+00,4.933000000000000163e-01,1.000000000000000000e+00 85 | 3.360009999999999764e+01,-9.953269999999999618e-01,4.468579999999999774e-01,1.000000000000000000e+00 86 | 3.400010000000000332e+01,-9.213479999999999448e-01,4.105389999999999873e-01,1.000000000000000000e+00 87 | 3.439999999999999858e+01,-8.430689999999999573e-01,3.730350000000000055e-01,1.000000000000000000e+00 88 | 3.479999999999999716e+01,-7.457909999999999817e-01,3.275279999999999858e-01,1.000000000000000000e+00 89 | 3.520000000000000284e+01,-6.622620000000000173e-01,2.892549999999999844e-01,1.000000000000000000e+00 90 | 3.560000000000000142e+01,-5.755169999999999453e-01,2.501530000000000142e-01,1.000000000000000000e+00 91 | 3.600000000000000000e+01,-4.744059999999999944e-01,2.052470000000000128e-01,1.000000000000000000e+00 92 | 3.639999999999999858e+01,-3.841060000000000030e-01,1.656299999999999994e-01,1.000000000000000000e+00 93 | 3.679999999999999716e+01,-2.918450000000000211e-01,1.255169999999999897e-01,1.000000000000000000e+00 94 | 3.720000000000000284e+01,-1.885759999999999936e-01,8.093630000000000269e-02,1.000000000000000000e+00 95 | 3.760000000000000142e+01,-9.459209999999999841e-02,4.055430000000000146e-02,1.000000000000000000e+00 96 | 3.800000000000000000e+01,-4.286260000000000056e-16,1.836970000000000048e-16,1.000000000000000000e+00 97 | 3.839999999999999858e+01,1.037939999999999974e-01,-4.450280000000000208e-02,1.000000000000000000e+00 98 | 3.879999999999999716e+01,1.982079999999999953e-01,-8.508330000000000060e-02,1.000000000000000000e+00 99 | 3.920000000000000284e+01,2.918450000000000211e-01,-1.255169999999999897e-01,1.000000000000000000e+00 100 | 3.960000000000000142e+01,3.945319999999999938e-01,-1.701840000000000019e-01,1.000000000000000000e+00 101 | 4.000000000000000000e+01,4.860869999999999913e-01,-2.104030000000000067e-01,1.000000000000000000e+00 102 | 4.039999999999999858e+01,5.755169999999999453e-01,-2.501530000000000142e-01,1.000000000000000000e+00 103 | 4.079990000000000094e+01,6.754259999999999708e-01,-2.952440000000000064e-01,1.000000000000000000e+00 104 | 4.119989999999999952e+01,7.609329999999999705e-01,-3.345400000000000040e-01,1.000000000000000000e+00 105 | 4.159989999999999810e+01,8.430689999999999573e-01,-3.730350000000000055e-01,1.000000000000000000e+00 106 | 4.199989999999999668e+01,9.384460000000000024e-01,-4.188520000000000021e-01,1.000000000000000000e+00 107 | 4.239990000000000236e+01,1.014950000000000019e+00,-4.566529999999999756e-01,1.000000000000000000e+00 108 | 4.279990000000000094e+01,1.087010000000000032e+00,-4.933000000000000163e-01,1.000000000000000000e+00 109 | 4.319989999999999952e+01,1.175939999999999985e+00,-5.402259999999999840e-01,1.000000000000000000e+00 110 | 4.359989999999999810e+01,1.240850000000000009e+00,-5.759360000000000035e-01,1.000000000000000000e+00 111 | 4.399989999999999668e+01,1.320959999999999912e+00,-6.221670000000000256e-01,1.000000000000000000e+00 112 | 4.439990000000000236e+01,1.393929999999999891e+00,-6.669530000000000181e-01,1.000000000000000000e+00 113 | 4.479990000000000094e+01,1.468869999999999898e+00,-7.165690000000000115e-01,1.000000000000000000e+00 114 | 4.519989999999999952e+01,1.529760000000000009e+00,-7.606800000000000228e-01,1.000000000000000000e+00 115 | 4.559989999999999810e+01,1.582640000000000047e+00,-8.030500000000000416e-01,1.000000000000000000e+00 116 | 4.599989999999999668e+01,1.627450000000000063e+00,-8.434580000000000410e-01,1.000000000000000000e+00 117 | 4.639990000000000236e+01,1.676299999999999901e+00,-8.955199999999999827e-01,1.000000000000000000e+00 118 | 4.679990000000000094e+01,1.713049999999999962e+00,-9.460669999999999913e-01,1.000000000000000000e+00 119 | 4.719989999999999952e+01,1.737330000000000041e+00,-9.949249999999999483e-01,1.000000000000000000e+00 120 | 4.759980000000000189e+01,1.748920000000000030e+00,-1.041830000000000034e+00,1.000000000000000000e+00 121 | 4.799980000000000047e+01,1.744739999999999958e+00,-1.100970000000000004e+00,1.000000000000000000e+00 122 | 4.839979999999999905e+01,1.719379999999999908e+00,-1.155219999999999914e+00,1.000000000000000000e+00 123 | 4.879979999999999762e+01,1.687980000000000036e+00,-1.192409999999999970e+00,1.000000000000000000e+00 124 | 4.919980000000000331e+01,1.646239999999999926e+00,-1.227440000000000087e+00,1.000000000000000000e+00 125 | 4.959980000000000189e+01,1.594100000000000072e+00,-1.260620000000000074e+00,1.000000000000000000e+00 126 | 4.999980000000000047e+01,1.531549999999999967e+00,-1.292010000000000103e+00,1.000000000000000000e+00 127 | 5.039979999999999905e+01,1.458720000000000017e+00,-1.321550000000000002e+00,1.000000000000000000e+00 128 | 5.079979999999999762e+01,1.396609999999999907e+00,-1.342719999999999914e+00,1.000000000000000000e+00 129 | 5.119980000000000331e+01,1.327069999999999972e+00,-1.363220000000000098e+00,1.000000000000000000e+00 130 | 5.159980000000000189e+01,1.250329999999999941e+00,-1.382789999999999964e+00,1.000000000000000000e+00 131 | 5.199980000000000047e+01,1.159210000000000074e+00,-1.402779999999999916e+00,1.000000000000000000e+00 132 | 5.239979999999999905e+01,1.072650000000000103e+00,-1.419119999999999937e+00,1.000000000000000000e+00 133 | 5.279979999999999762e+01,9.799240000000000173e-01,-1.434250000000000025e+00,1.000000000000000000e+00 134 | 5.319980000000000331e+01,8.815530000000000310e-01,-1.448039999999999994e+00,1.000000000000000000e+00 135 | 5.359980000000000189e+01,7.803309999999999969e-01,-1.460129999999999928e+00,1.000000000000000000e+00 136 | 5.399969999999999715e+01,7.009940000000000060e-01,-1.468259999999999899e+00,1.000000000000000000e+00 137 | 5.439970000000000283e+01,6.181170000000000275e-01,-1.475629999999999997e+00,1.000000000000000000e+00 138 | 5.479970000000000141e+01,5.129200000000000426e-01,-1.483440000000000092e+00,1.000000000000000000e+00 139 | 5.519969999999999999e+01,4.261369999999999880e-01,-1.488669999999999938e+00,1.000000000000000000e+00 140 | 5.559969999999999857e+01,3.370589999999999975e-01,-1.492960000000000065e+00,1.000000000000000000e+00 141 | 5.599969999999999715e+01,2.295200000000000018e-01,-1.496760000000000090e+00,1.000000000000000000e+00 142 | 5.639970000000000283e+01,1.383350000000000135e-01,-1.498830000000000107e+00,1.000000000000000000e+00 143 | 5.679970000000000141e+01,4.621589999999999726e-02,-1.499870000000000037e+00,1.000000000000000000e+00 144 | 5.719969999999999999e+01,-6.207390000000000130e-02,-1.499759999999999982e+00,1.000000000000000000e+00 145 | 5.759969999999999857e+01,-1.544129999999999947e-01,-1.498539999999999983e+00,1.000000000000000000e+00 146 | 5.799969999999999715e+01,-2.462820000000000009e-01,-1.496259999999999923e+00,1.000000000000000000e+00 147 | 5.839970000000000283e+01,-3.537290000000000156e-01,-1.492240000000000011e+00,1.000000000000000000e+00 148 | 5.879970000000000141e+01,-4.439089999999999980e-01,-1.487679999999999891e+00,1.000000000000000000e+00 149 | 5.919969999999999999e+01,-5.322339999999999849e-01,-1.482129999999999947e+00,1.000000000000000000e+00 150 | 5.959969999999999857e+01,-6.372069999999999679e-01,-1.474029999999999951e+00,1.000000000000000000e+00 151 | 5.999969999999999715e+01,-7.219809999999999839e-01,-1.466220000000000079e+00,1.000000000000000000e+00 152 | 6.039970000000000283e+01,-8.036039999999999850e-01,-1.457519999999999927e+00,1.000000000000000000e+00 153 | 6.079959999999999809e+01,-9.043809999999999905e-01,-1.445040000000000102e+00,1.000000000000000000e+00 154 | 6.119959999999999667e+01,-9.807010000000000449e-01,-1.434139999999999970e+00,1.000000000000000000e+00 155 | 6.159960000000000235e+01,-1.076670000000000016e+00,-1.418409999999999949e+00,1.000000000000000000e+00 156 | 6.199960000000000093e+01,-1.166719999999999979e+00,-1.401250000000000107e+00,1.000000000000000000e+00 157 | 6.239959999999999951e+01,-1.250329999999999941e+00,-1.382789999999999964e+00,1.000000000000000000e+00 158 | 6.279959999999999809e+01,-1.337720000000000020e+00,-1.360260000000000025e+00,1.000000000000000000e+00 159 | 6.319959999999999667e+01,-1.410670000000000091e+00,-1.338189999999999991e+00,1.000000000000000000e+00 160 | 6.359960000000000235e+01,-1.476069999999999993e+00,-1.315039999999999987e+00,1.000000000000000000e+00 161 | 6.399960000000000093e+01,-1.533689999999999998e+00,-1.291039999999999965e+00,1.000000000000000000e+00 162 | 6.439960000000000662e+01,-1.599760000000000071e+00,-1.257400000000000073e+00,1.000000000000000000e+00 163 | 6.479959999999999809e+01,-1.654369999999999896e+00,-1.221389999999999976e+00,1.000000000000000000e+00 164 | 6.519960000000000377e+01,-1.697019999999999973e+00,-1.183100000000000041e+00,1.000000000000000000e+00 165 | 6.559959999999999525e+01,-1.727270000000000083e+00,-1.142689999999999984e+00,1.000000000000000000e+00 166 | 6.599960000000000093e+01,-1.744869999999999921e+00,-1.100479999999999903e+00,1.000000000000000000e+00 167 | 6.639960000000000662e+01,-1.748920000000000030e+00,-1.041830000000000034e+00,1.000000000000000000e+00 168 | 6.679959999999999809e+01,-1.732110000000000039e+00,-9.821199999999999930e-01,1.000000000000000000e+00 169 | 6.719950000000000045e+01,-1.706979999999999942e+00,-9.365310000000000024e-01,1.000000000000000000e+00 170 | 6.759950000000000614e+01,-1.671229999999999993e+00,-8.895420000000000549e-01,1.000000000000000000e+00 171 | 6.799949999999999761e+01,-1.624810000000000088e+00,-8.409180000000000543e-01,1.000000000000000000e+00 172 | 6.839950000000000330e+01,-1.567709999999999937e+00,-7.906130000000000102e-01,1.000000000000000000e+00 173 | 6.879949999999999477e+01,-1.517419999999999991e+00,-7.513919999999999488e-01,1.000000000000000000e+00 174 | 6.919950000000000045e+01,-1.459459999999999980e+00,-7.100919999999999455e-01,1.000000000000000000e+00 175 | 6.959950000000000614e+01,-1.393929999999999891e+00,-6.669530000000000181e-01,1.000000000000000000e+00 176 | 6.999949999999999761e+01,-1.314389999999999947e+00,-6.182720000000000438e-01,1.000000000000000000e+00 177 | 7.039950000000000330e+01,-1.237640000000000073e+00,-5.741519999999999957e-01,1.000000000000000000e+00 178 | 7.079949999999999477e+01,-1.154409999999999936e+00,-5.286539999999999573e-01,1.000000000000000000e+00 179 | 7.119950000000000045e+01,-1.087010000000000032e+00,-4.933000000000000163e-01,1.000000000000000000e+00 180 | 7.159950000000000614e+01,-9.950449999999999573e-01,-4.467369999999999952e-01,1.000000000000000000e+00 181 | 7.199949999999999761e+01,-9.219779999999999642e-01,-4.108089999999999797e-01,1.000000000000000000e+00 182 | 7.239950000000000330e+01,-8.430689999999999573e-01,-3.730350000000000055e-01,1.000000000000000000e+00 183 | 7.279949999999999477e+01,-7.445359999999999756e-01,-3.269889999999999741e-01,1.000000000000000000e+00 184 | 7.319950000000000045e+01,-6.647060000000000191e-01,-2.903049999999999797e-01,1.000000000000000000e+00 185 | 7.359940000000000282e+01,-5.755169999999999453e-01,-2.501530000000000142e-01,1.000000000000000000e+00 186 | 7.399939999999999429e+01,-4.693359999999999754e-01,-2.030720000000000025e-01,1.000000000000000000e+00 187 | 7.439939999999999998e+01,-3.931999999999999940e-01,-1.695329999999999893e-01,1.000000000000000000e+00 188 | 7.479940000000000566e+01,-2.918450000000000211e-01,-1.255169999999999897e-01,1.000000000000000000e+00 189 | 7.519939999999999714e+01,-1.699299999999999977e-01,-7.293360000000000121e-02,1.000000000000000000e+00 190 | 7.559940000000000282e+01,-1.252810000000000035e-01,-5.372579999999999717e-02,1.000000000000000000e+00 191 | 7.599939999999999429e+01,-8.572530000000000283e-16,-3.673940000000000097e-16,1.000000000000000000e+00 192 | 7.639939999999999998e+01,1.427029999999999965e-01,6.121179999999999677e-02,1.000000000000000000e+00 193 | 0.000000000000000000e+00,0.000000000000000000e+00,0.000000000000000000e+00,1.000000000000000000e+00 194 | 4.000000000000000222e-01,6.475019999999999387e-02,2.776829999999999923e-02,1.000000000000000000e+00 195 | 8.000000000000000444e-01,2.334350000000000036e-01,1.001819999999999933e-01,1.000000000000000000e+00 196 | 1.199999999999999956e+00,2.918450000000000211e-01,1.255169999999999897e-01,1.000000000000000000e+00 197 | 1.600000000000000089e+00,3.805999999999999939e-01,1.642129999999999979e-01,1.000000000000000000e+00 198 | 2.000000000000000000e+00,4.966960000000000264e-01,2.149500000000000022e-01,1.000000000000000000e+00 199 | 2.399999999999999911e+00,5.755169999999999453e-01,2.501530000000000142e-01,1.000000000000000000e+00 200 | 2.799999999999999822e+00,6.719049999999999745e-01,2.937350000000000239e-01,1.000000000000000000e+00 201 | 3.200000000000000178e+00,7.637080000000000535e-01,3.357299999999999729e-01,1.000000000000000000e+00 202 | 3.600000000000000089e+00,8.430689999999999573e-01,3.730350000000000055e-01,1.000000000000000000e+00 203 | -------------------------------------------------------------------------------- /trajs/subsample.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | data = np.loadtxt('spiral8.csv', delimiter=",") 4 | 5 | indexes = np.arange(0, len(data), 4) 6 | 7 | print(indexes[0], len(data), indexes[-1]) 8 | subsample_data = data[indexes] 9 | new_data = np.vstack((subsample_data, subsample_data[:10, :])) 10 | print(new_data.shape) 11 | np.savetxt('spiral86.csv', new_data, delimiter=",") 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /trajs/traj_2obs_vicon.csv: -------------------------------------------------------------------------------- 1 | -8.299999999999999600e-01,-1.836999999999999966e+00,1.000000000000000000e+00 2 | -7.450009766038754311e-01,-1.730148176934997784e+00,9.666666666666666741e-01 3 | -6.732094693608344782e-01,-1.617727069642602267e+00,9.333333333333333481e-01 4 | -6.127077209589250062e-01,-1.500718453063208724e+00,9.000000000000000222e-01 5 | -5.614818009297389834e-01,-1.379867714941392709e+00,8.666666666666666963e-01 6 | -5.174414402223255216e-01,-1.255751744018448246e+00,8.333333333333333703e-01 7 | -4.784349769579034906e-01,-1.128849487975007504e+00,8.000000000000000444e-01 8 | -4.422629474196489996e-01,-9.996145186081809575e-01,7.666666666666667185e-01 9 | -4.066939460413602214e-01,-8.685488864882431326e-01,7.333333333333333925e-01 10 | -3.694864109755313653e-01,-7.362772529944685562e-01,7.000000000000000666e-01 11 | -3.284199353272029276e-01,-6.036197553983935205e-01,6.666666666666667407e-01 12 | -2.813395068693251844e-01,-4.716613014086229461e-01,6.333333333333334147e-01 13 | -2.262156722140321874e-01,-3.418140293991329681e-01,6.000000000000000888e-01 14 | -1.612229224834268171e-01,-2.158685578188107301e-01,5.666666666666667629e-01 15 | -8.483751657306651262e-02,-9.602846083151206358e-02,5.333333333333334370e-01 16 | 4.045592306325729304e-03,1.507873557164422784e-02,5.000000000000001110e-01 17 | 1.059791516409798323e-01,1.144216791395017280e-01,4.666666666666667851e-01 18 | 1.950075113927089432e-01,1.912763973608800350e-01,5.083333333333334147e-01 19 | 2.214872799386580215e-01,2.598686164083160444e-01,5.750000000000000666e-01 20 | 2.125925962902268096e-01,3.195713400922356451e-01,6.166666666666666963e-01 21 | 2.079693593430856635e-01,3.832199343962959803e-01,5.583333333333333481e-01 22 | 2.040434112067165684e-01,4.518545762024127610e-01,5.666666666666666519e-01 23 | 2.014534969122979968e-01,5.237532988982975812e-01,5.083333333333333037e-01 24 | 2.060424277862622500e-01,5.868621150830488631e-01,4.833333333333332815e-01 25 | 2.144310017189269835e-01,6.584775773020118406e-01,4.666666666666666186e-01 26 | 2.216722900609196523e-01,7.321555176058034409e-01,4.416666666666665964e-01 27 | 2.320972683112977886e-01,8.059800531062989437e-01,4.249999999999999334e-01 28 | 2.374767479595125685e-01,8.825850834668040878e-01,3.666666666666665853e-01 29 | 2.487750559944096884e-01,9.499331493483137434e-01,3.416666666666665630e-01 30 | 2.762099574344731301e-01,9.762973842541801117e-01,3.333333333333332038e-01 31 | 2.899711155728670331e-01,1.042220659602041133e+00,3.083333333333331816e-01 32 | 2.540660354501778073e-01,1.061180022831516467e+00,3.499999999999998668e-01 33 | 2.722501877571185669e-01,1.091569003603025001e+00,3.416666666666665075e-01 34 | 2.863589916674630920e-01,1.122730324675994096e+00,3.333333333333331483e-01 35 | 2.961191975043004354e-01,1.154953384026603302e+00,3.249999999999997891e-01 36 | 3.012508798827253087e-01,1.188472780088129399e+00,3.166666666666664298e-01 37 | 3.014610660199024683e-01,1.223463101766075978e+00,3.083333333333330706e-01 38 | 3.250509598799229960e-01,1.240152520775734013e+00,2.916666666666664076e-01 39 | 2.599444749552340950e-01,1.268564788422566370e+00,3.333333333333330373e-01 40 | 2.328350376904378960e-01,1.242273510632923683e+00,3.999999999999996891e-01 41 | 1.934009278531922393e-01,1.218401465658118576e+00,4.833333333333330040e-01 42 | 1.890537612914772458e-01,1.151095373014281664e+00,5.666666666666663188e-01 43 | 1.989701917003015574e-01,1.103471393157673175e+00,6.166666666666663632e-01 44 | 2.051630553039033023e-01,1.158022021148545466e+00,6.999999999999997335e-01 45 | 2.038196686182897621e-01,1.224286329930003303e+00,7.249999999999997558e-01 46 | 1.946902384297918565e-01,1.262814808570267289e+00,8.166666666666664298e-01 47 | 2.053725904692363102e-01,1.302579167104369162e+00,8.916666666666663854e-01 48 | 1.942874641113844780e-01,1.306652685224906740e+00,9.833333333333330595e-01 49 | 2.016833735190618759e-01,1.402957447555637005e+00,1.041666666666666297e+00 50 | 1.924967843317423877e-01,1.504247403710541331e+00,1.108333333333332948e+00 51 | 2.121702828190488166e-01,1.601598826130484987e+00,1.174999999999999600e+00 52 | 3.081163865777842292e-01,1.690351056745890368e+00,1.216666666666666341e+00 53 | 4.439997678723078822e-01,1.783415148566596997e+00,1.183333333333332904e+00 54 | 6.050058529824089426e-01,1.800442405932133116e+00,1.249999999999999556e+00 55 | -------------------------------------------------------------------------------- /trajs/traj_L_vicon.csv: -------------------------------------------------------------------------------- 1 | 1.80044240593213,0.605005852982409,1 2 | 1.7834151485666,0.443999767872308,1 3 | 1.69035105674589,0.308116386577784,1 4 | 1.60159882613049,0.212170282819049,1 5 | 1.50424740371054,0.192496784331742,1 6 | 1.40295744755564,0.201683373519062,1 7 | 1.30665268522491,0.201683373519,1 8 | 1.30257916710437,0.201683373519062,1 9 | 1.26281480857027,0.201683373519062,1 10 | 1.22428632993,0.201683373519062,1 11 | 1.15802202114855,0.201683373519062,1 12 | 1.10347139315767,0.201683373519062,1 13 | 1.15109537301428,0.201683373519062,1 14 | 1.21840146565812,0.201683373519062,1 15 | 1.24227351063292,0.201683373519062,1 16 | 1.26856478842257,0.201683373519062,1 17 | 1.24015252077573,0.201683373519062,1 18 | 1.22346310176608,0.201683373519062,1 19 | 1.18847278008813,0.201683373519062,1 20 | 1.1549533840266,0.201683373519062,1 21 | 1.12273032467599,0.201683373519062,1 22 | 1.09156900360303,0.201683373519062,1 23 | 1.06118002283152,0.201683373519062,1 24 | 1.04222065960204,0.201683373519062,1 25 | 0.97629738425418,0.201683373519062,1 26 | 0.949933149348314,0.201683373519062,1 27 | 0.882585083466804,0.201683373519062,1 28 | 0.805980053106299,0.201683373519062,1 29 | 0.732155517605803,0.201683373519062,1 30 | 0.658477577302012,0.201683373519062,1 31 | 0.586862115083049,0.201683373519062,1 32 | 0.523753298898298,0.201683373519062,1 33 | 0.451854576202413,0.201683373519062,1 34 | 0.383219934396296,0.201683373519062,1 35 | 0.319571340092236,0.201683373519062,1 36 | 0.259868616408316,0.201683373519062,1 37 | 0.19127639736088,0.201683373519062,1 38 | 0.114421679139502,0.201683373519062,1 39 | 0.0150787355716442,0.201683373519062,1 40 | -0.0960284608315121,0.201683373519062,1 41 | -0.215868557818811,0.201683373519062,1 42 | -0.341814029399133,0.201683373519062,1 43 | -0.471661301408623,0.201683373519062,1 44 | -0.603619755398394,0.201683373519062,1 45 | -0.736277252994469,0.201683373519062,1 46 | -0.868548886488243,0.201683373519062,1 47 | -0.999614518608181,0.201683373519062,1 48 | -1.12884948797501,0.201683373519062,1 49 | -1.25575174401845,0.201683373519062,1 50 | -1.37986771494139,0.201683373519062,1 51 | -1.50071845306321,0.201683373519062,1 52 | -1.6177270696426,0.201683373519062,1 53 | -1.730148176935,0.201683373519062,1 54 | -1.837,0.201683373519062,1 55 | -------------------------------------------------------------------------------- /window.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | MainWindow 4 | 5 | 6 | 7 | 0 8 | 0 9 | 915 10 | 724 11 | 12 | 13 | 14 | Drone Controller 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | Qt::Vertical 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | QFrame::StyledPanel 36 | 37 | 38 | QFrame::Raised 39 | 40 | 41 | 42 | 43 | 44 | #Points 45 | 46 | 47 | 48 | 49 | 50 | 51 | x_coord 52 | 53 | 54 | 55 | 56 | 57 | 58 | Qt::Vertical 59 | 60 | 61 | 62 | 20 63 | 40 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | z_coord 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 500 93 | 94 | 95 | 96 | 97 | 98 | 99 | 24 100 | 101 | 102 | 103 | 104 | 105 | 106 | Qt::Vertical 107 | 108 | 109 | 110 | 20 111 | 40 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | y_coord 120 | 121 | 122 | 123 | 124 | 125 | 126 | MaxVel 127 | 128 | 129 | 130 | 131 | 132 | 133 | altVal 134 | 135 | 136 | 137 | 138 | 139 | 140 | 0.0 m/s 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | Launch 151 | 152 | 153 | 154 | 155 | 156 | 157 | DOWN 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | arrows/down.pngarrows/down.png 169 | 170 | 171 | 172 | 28 173 | 28 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | Flight Mode : 182 | 183 | 184 | 185 | 186 | 187 | 188 | UP 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | arrows/up.pngarrows/up.png 200 | 201 | 202 | 203 | 28 204 | 28 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | -1 214 | 75 215 | false 216 | true 217 | 218 | 219 | 220 | QPushButton{ 221 | background-color: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, 222 | stop: 0 #D3D3D3, stop: 0.4 #D8D8D8, 223 | stop: 0.5 #DDDDDD, stop: 1.0 #E1E1E1); 224 | border-style: outset; 225 | border-width: 1px; 226 | border-radius: 19px; 227 | border-color: beige; 228 | font: bold 14px; 229 | min-width:1em; 230 | padding: 10px; 231 | } 232 | 233 | QPushButton:pressed { 234 | background-color: rgb(224, 0, 0); 235 | border-style: inset; 236 | } 237 | 238 | 239 | LAND 240 | 241 | 242 | 243 | 28 244 | 28 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | arrows/left.pngarrows/left.png 264 | 265 | 266 | 267 | 28 268 | 28 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | arrows/right.pngarrows/right.png 281 | 282 | 283 | 284 | 28 285 | 28 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | Send Traj 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | Qt::Vertical 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 0 314 | 0 315 | 915 316 | 25 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | slot1() 326 | 327 | 328 | -------------------------------------------------------------------------------- /workspace.py: -------------------------------------------------------------------------------- 1 | 2 | #!/home/airlab/anaconda3/envs/droneController/bin/python 3 | import sys 4 | from PyQt5.QtWidgets import QApplication 5 | from DroneController import MainWindow, ConfigParser 6 | 7 | 8 | if __name__ == '__main__': 9 | config = ConfigParser('param.yaml') 10 | 11 | app = QApplication(sys.argv) 12 | window = MainWindow(config) 13 | window.show() 14 | sys.exit(app.exec()) --------------------------------------------------------------------------------