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