├── docs ├── .gitignore ├── generated │ └── .gitignore ├── tutorials │ ├── tutorials.rst │ └── hover.rstinclude ├── images │ ├── chooser.png │ ├── large.jpg │ ├── medium.jpg │ ├── piecewise_poly.png │ ├── software_architecture.png │ ├── CrazyflieWithSingleMarker.jpg │ └── markerConfigurationExample.jpg ├── requirements.txt ├── howto │ ├── howto.rst │ └── git_integration.rstinclude ├── generate_install_deps_code.py ├── gettingstarted.rst ├── overview.rst ├── glossary.rst ├── api.rst └── changelog.rst ├── ros_ws ├── src │ └── crazyswarm │ │ ├── scripts │ │ ├── pycrazyswarm │ │ │ ├── cfsim │ │ │ │ ├── __init__.py │ │ │ │ ├── .gitignore │ │ │ │ ├── Makefile │ │ │ │ └── setup.py │ │ │ ├── visualizer │ │ │ │ ├── __init__.py │ │ │ │ ├── crazyflie2.obj.gz │ │ │ │ ├── visNull.py │ │ │ │ └── visMatplotlib.py │ │ │ ├── __init__.py │ │ │ ├── output.py │ │ │ ├── joystick.py │ │ │ ├── testCrazyflieSim.py │ │ │ ├── keyboard.py │ │ │ ├── videowriter.py │ │ │ ├── testVispy.py │ │ │ └── crazyswarm_py.py │ │ ├── pytest.ini │ │ ├── test_ros.py │ │ ├── waypoints.csv │ │ ├── conftest.py │ │ ├── takeoff.csv │ │ ├── individual_hover.py │ │ ├── hello_world.py │ │ ├── niceHover.py │ │ ├── example_cmd_pos.py │ │ ├── test_yamlString.py │ │ ├── led_colors.py │ │ ├── waypoints_simple.py │ │ ├── figure8_csv.py │ │ ├── cmdFullState.py │ │ ├── udp_multicast.py │ │ ├── cmdVelocityCircle.py │ │ ├── swap6v.py │ │ ├── test_simOnly.py │ │ ├── graphVisualization.py │ │ ├── sequence_trajectories │ │ │ ├── 1 │ │ │ │ ├── 18.csv │ │ │ │ ├── 17.csv │ │ │ │ ├── 5.csv │ │ │ │ ├── 8.csv │ │ │ │ ├── 3.csv │ │ │ │ ├── 9.csv │ │ │ │ └── 10.csv │ │ │ ├── 2 │ │ │ │ ├── 18.csv │ │ │ │ ├── 17.csv │ │ │ │ ├── 5.csv │ │ │ │ ├── 10.csv │ │ │ │ ├── 9.csv │ │ │ │ ├── 3.csv │ │ │ │ └── 8.csv │ │ │ ├── 3 │ │ │ │ ├── 18.csv │ │ │ │ ├── 17.csv │ │ │ │ ├── 5.csv │ │ │ │ ├── 9.csv │ │ │ │ ├── 10.csv │ │ │ │ ├── 3.csv │ │ │ │ └── 8.csv │ │ │ ├── 4 │ │ │ │ ├── 18.csv │ │ │ │ ├── 5.csv │ │ │ │ ├── 17.csv │ │ │ │ ├── 3.csv │ │ │ │ ├── 8.csv │ │ │ │ ├── 9.csv │ │ │ │ └── 10.csv │ │ │ ├── 5 │ │ │ │ ├── 18.csv │ │ │ │ ├── 5.csv │ │ │ │ ├── 17.csv │ │ │ │ ├── 3.csv │ │ │ │ ├── 9.csv │ │ │ │ ├── 8.csv │ │ │ │ └── 10.csv │ │ │ ├── 6 │ │ │ │ ├── 18.csv │ │ │ │ ├── 17.csv │ │ │ │ ├── 5.csv │ │ │ │ ├── 3.csv │ │ │ │ ├── 8.csv │ │ │ │ ├── 9.csv │ │ │ │ └── 10.csv │ │ │ └── 7 │ │ │ │ ├── 18.csv │ │ │ │ ├── 17.csv │ │ │ │ ├── 5.csv │ │ │ │ ├── 3.csv │ │ │ │ ├── 10.csv │ │ │ │ ├── 8.csv │ │ │ │ └── 9.csv │ │ ├── waypoints.py │ │ ├── backgroundComputation.py │ │ └── test_videoOutput.py │ │ ├── srv │ │ ├── Stop.srv │ │ ├── SetGroupMask.srv │ │ ├── UpdateParams.srv │ │ ├── Land.srv │ │ ├── Takeoff.srv │ │ ├── NotifySetpointsStop.srv │ │ ├── UploadTrajectory.srv │ │ ├── GoTo.srv │ │ └── StartTrajectory.srv │ │ ├── msg │ │ ├── GenericLogData.msg │ │ ├── LogBlock.msg │ │ ├── VelocityWorld.msg │ │ ├── Position.msg │ │ ├── Hover.msg │ │ ├── FullState.msg │ │ └── TrajectoryPolynomialPiece.msg │ │ ├── launch │ │ ├── crazyflies.yaml │ │ ├── allCrazyflies.yaml │ │ ├── mocap_helper.launch │ │ └── hover_swarm.launch │ │ ├── package.xml │ │ └── src │ │ └── mocap_helper.cpp └── .catkin_workspace ├── prebuilt └── cradio.bin ├── .gitignore ├── conda_env.yaml ├── notes ├── weights.txt ├── todo.md ├── stm32timerclocks.txt └── firmware-code-review.txt ├── .readthedocs.yml ├── .gitmodules ├── .github └── workflows │ ├── ci-docs.yml │ ├── ci-simonly.yml │ └── ci-ros.yml ├── pc_permissions.sh ├── LICENSE └── README.md /docs/.gitignore: -------------------------------------------------------------------------------- 1 | _build 2 | html 3 | -------------------------------------------------------------------------------- /docs/generated/.gitignore: -------------------------------------------------------------------------------- 1 | *.rst 2 | *.rstinclude 3 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/cfsim/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/Stop.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | --- 3 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/visualizer/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/SetGroupMask.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | --- 3 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/UpdateParams.srv: -------------------------------------------------------------------------------- 1 | string[] params 2 | --- 3 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/msg/GenericLogData.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64[] values 3 | -------------------------------------------------------------------------------- /docs/tutorials/tutorials.rst: -------------------------------------------------------------------------------- 1 | Tutorials 2 | ========= 3 | 4 | .. include:: hover.rstinclude 5 | -------------------------------------------------------------------------------- /prebuilt/cradio.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/prebuilt/cradio.bin -------------------------------------------------------------------------------- /docs/images/chooser.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/docs/images/chooser.png -------------------------------------------------------------------------------- /docs/images/large.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/docs/images/large.jpg -------------------------------------------------------------------------------- /docs/images/medium.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/docs/images/medium.jpg -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/msg/LogBlock.msg: -------------------------------------------------------------------------------- 1 | string topic_name 2 | int16 frequency 3 | string[] variables 4 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/Land.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | float32 height 3 | duration duration 4 | --- 5 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/msg/VelocityWorld.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Vector3 vel 3 | float32 yawRate -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/Takeoff.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | float32 height 3 | duration duration 4 | --- 5 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/NotifySetpointsStop.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | uint32 remainValidMillisecs 3 | --- 4 | -------------------------------------------------------------------------------- /docs/images/piecewise_poly.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/docs/images/piecewise_poly.png -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/msg/Position.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 x 3 | float32 y 4 | float32 z 5 | float32 yaw 6 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/cfsim/.gitignore: -------------------------------------------------------------------------------- 1 | cffirmware.py 2 | cffirmware_wrap.c 3 | *.so 4 | build 5 | -------------------------------------------------------------------------------- /ros_ws/.catkin_workspace: -------------------------------------------------------------------------------- 1 | # This file currently only serves to mark the location of a catkin workspace for tool integration 2 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/msg/Hover.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float32 vx 3 | float32 vy 4 | float32 yawrate 5 | float32 zDistance 6 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pytest.ini: -------------------------------------------------------------------------------- 1 | [pytest] 2 | markers = 3 | ros: tests that can only run when ROS is installed. 4 | -------------------------------------------------------------------------------- /docs/images/software_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/docs/images/software_architecture.png -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/__init__.py: -------------------------------------------------------------------------------- 1 | from .crazyswarm_py import * 2 | 3 | __all__ = ["Crazyswarm", "cfsim"] 4 | -------------------------------------------------------------------------------- /docs/images/CrazyflieWithSingleMarker.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/docs/images/CrazyflieWithSingleMarker.jpg -------------------------------------------------------------------------------- /docs/images/markerConfigurationExample.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/docs/images/markerConfigurationExample.jpg -------------------------------------------------------------------------------- /docs/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | pyyaml 3 | sphinx 4 | sphinx-argparse 5 | sphinx-rtd-theme 6 | sphinx-tabs 7 | sphinxcontrib-programoutput 8 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/UploadTrajectory.srv: -------------------------------------------------------------------------------- 1 | uint8 trajectoryId 2 | uint32 pieceOffset 3 | TrajectoryPolynomialPiece[] pieces 4 | --- 5 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/msg/FullState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | geometry_msgs/Pose pose 3 | geometry_msgs/Twist twist 4 | geometry_msgs/Vector3 acc 5 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/launch/crazyflies.yaml: -------------------------------------------------------------------------------- 1 | crazyflies: 2 | - channel: 100 3 | id: 1 4 | initialPosition: [1.5, 1.5, 0.0] 5 | type: medium 6 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/GoTo.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | bool relative 3 | geometry_msgs/Point goal 4 | float32 yaw # deg 5 | duration duration 6 | --- 7 | -------------------------------------------------------------------------------- /docs/howto/howto.rst: -------------------------------------------------------------------------------- 1 | How-To Guides 2 | ============= 3 | 4 | .. include:: git_integration.rstinclude 5 | 6 | .. include:: streaming_setpoint.rstinclude 7 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/srv/StartTrajectory.srv: -------------------------------------------------------------------------------- 1 | uint8 groupMask 2 | uint8 trajectoryId 3 | float32 timescale 4 | bool reversed 5 | bool relative 6 | --- 7 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/msg/TrajectoryPolynomialPiece.msg: -------------------------------------------------------------------------------- 1 | # 2 | 3 | float32[] poly_x 4 | float32[] poly_y 5 | float32[] poly_z 6 | float32[] poly_yaw 7 | duration duration -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/test_ros.py: -------------------------------------------------------------------------------- 1 | import pytest 2 | 3 | 4 | @pytest.mark.ros 5 | def test_ros_import(): 6 | from pycrazyswarm.crazyflie import TimeHelper, CrazyflieServer 7 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/waypoints.csv: -------------------------------------------------------------------------------- 1 | id,x[m],y[m],z[m],t[s] 2 | 1,0,0,1,0 3 | 1,0,1,1,2 4 | 1,0,0,2,4 5 | 2,1,0,1,0 6 | 2,1,1,2,4 7 | 3,1,1,1,0 8 | 3,1,1,3,2 9 | 3,1,2,2,4 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/visualizer/crazyflie2.obj.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/USC-ACTLab/crazyswarm/HEAD/ros_ws/src/crazyswarm/scripts/pycrazyswarm/visualizer/crazyflie2.obj.gz -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/launch/allCrazyflies.yaml: -------------------------------------------------------------------------------- 1 | crazyflies: 2 | - id: 1 3 | channel: 100 4 | initialPosition: [1.5, 1.5, 0.0] 5 | type: medium 6 | - id: 40 7 | channel: 100 8 | initialPosition: [-1.0, -0.5, 0.0] 9 | type: default 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | ros_ws/build/ 2 | ros_ws/devel/ 3 | ros_ws/src/CMakeLists.txt 4 | .DS_Store 5 | *.pyc 6 | *~ 7 | *.mexa64 8 | # james's symlinks :) 9 | cfs.yaml 10 | firm 11 | client 12 | launch # ignore launch symlink 13 | !*ros_ws/*launch* # don't ignore folders with launch in the name 14 | -------------------------------------------------------------------------------- /conda_env.yaml: -------------------------------------------------------------------------------- 1 | name: crazyswarm 2 | dependencies: 3 | - numpy 4 | - matplotlib 5 | - vispy 6 | - pyyaml 7 | - swig 8 | - sphinx 9 | - sphinx_rtd_theme 10 | - pyqt 11 | - conda-forge::sphinx-argparse 12 | - conda-forge::sphinxcontrib-programoutput 13 | - pytest 14 | - scipy 15 | -------------------------------------------------------------------------------- /notes/weights.txt: -------------------------------------------------------------------------------- 1 | 2 | 3 | * Batteries: 4 | * tenergy 380mAh: 11g 5 | * Crazyflie: 7.2g 6 | * turnigy: 7g 7 | * Crazyfire: 7g 8 | * 7.9mm Marker: 0.4g 9 | * CF (no bat, ne led ring): 21g 10 | * led-ring: 3g 11 | 12 | Hence: 13 | * typical config: 33g with standard battery, led ring, and 4 markers 14 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/conftest.py: -------------------------------------------------------------------------------- 1 | import subprocess 2 | 3 | import pytest 4 | 5 | 6 | HAVE_ROS = subprocess.call("type roslaunch", shell=True) == 0 7 | 8 | 9 | def pytest_runtest_setup(item): 10 | markers = [mark.name for mark in item.iter_markers()] 11 | if "ros" in markers and not HAVE_ROS: 12 | pytest.skip("ROS is not installed.") 13 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/takeoff.csv: -------------------------------------------------------------------------------- 1 | Duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7 2 | 1.15839,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1.06614,-1.57474,0.868105,-0.172428,0,0,0,0,0,0,0,0 3 | 1.13522,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.25,0.399333,3.03717e-14,-0.0697493,0.0551273,-0.643041,0.742072,-0.234074,0,0,0,0,0,0,0,0 4 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/visualizer/visNull.py: -------------------------------------------------------------------------------- 1 | """No-op visualizer for real hardware runs, so script can be oblivious.""" 2 | 3 | class VisNull: 4 | def __init__(self): 5 | pass 6 | 7 | def setGraph(self, edges): 8 | pass 9 | 10 | def showEllipsoids(self, radii): 11 | pass 12 | 13 | def update(self, t, crazyflies): 14 | pass 15 | 16 | def render(self): 17 | return None 18 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/launch/mocap_helper.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | # tracking 7 | motion_capture_type: "vicon" # one of vicon,optitrack,optitrack_closed_source,qualisys,vrpn 8 | motion_capture_host_name: "vicon" 9 | # motion_capture_interface_ip: "" # optional for optitrack with multiple interfaces 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/individual_hover.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from __future__ import print_function 4 | 5 | from pycrazyswarm import * 6 | 7 | def main(): 8 | swarm = Crazyswarm() 9 | timeHelper = swarm.timeHelper 10 | allcfs = swarm.allcfs 11 | 12 | for cf in allcfs.crazyflies: 13 | print(cf.id) 14 | cf.takeoff(1.0, 2.5) 15 | print("press button to continue") 16 | swarm.input.waitUntilButtonPressed() 17 | cf.land(0.04, 2.5) 18 | 19 | 20 | if __name__ == "__main__": 21 | main() 22 | -------------------------------------------------------------------------------- /.readthedocs.yml: -------------------------------------------------------------------------------- 1 | # .readthedocs.yml 2 | # Read the Docs configuration file 3 | # See https://docs.readthedocs.io/en/stable/config-file/v2.html for details 4 | 5 | # Required 6 | version: 2 7 | 8 | # Build documentation in the docs/ directory with Sphinx 9 | sphinx: 10 | configuration: docs/conf.py 11 | 12 | # Do not checkout any submodules for building the docs 13 | submodules: 14 | include: [] 15 | recursive: false 16 | 17 | # Add a nonstandard Sphinx extension 18 | python: 19 | version: 3.7 20 | install: 21 | - requirements: docs/requirements.txt 22 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/cfsim/Makefile: -------------------------------------------------------------------------------- 1 | firmdir = ../../../../../../crazyflie-firmware 2 | modinc = $(firmdir)/src/modules/interface 3 | modsrc = $(firmdir)/src/modules/src 4 | 5 | swig: setup.py cffirmware_wrap.c $(modsrc)/*.c 6 | ifeq ($(CSW_PYTHON),) 7 | $(error Environment variable CSW_PYTHON must be set to "python2" or "python3") 8 | endif 9 | $(CSW_PYTHON) setup.py build_ext --inplace 10 | 11 | cffirmware_wrap.c: cffirmware.i $(modinc)/*.h 12 | swig -python -I$(modinc) cffirmware.i 13 | 14 | clean: 15 | rm -f cffirmware.py _cffirmware.so *.pyc cffirmware_wrap.c 16 | rm -rf build 17 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/hello_world.py: -------------------------------------------------------------------------------- 1 | """Takeoff-hover-land for one CF. Useful to validate hardware config.""" 2 | 3 | from pycrazyswarm import Crazyswarm 4 | 5 | 6 | TAKEOFF_DURATION = 2.5 7 | HOVER_DURATION = 5.0 8 | 9 | 10 | def main(): 11 | swarm = Crazyswarm() 12 | timeHelper = swarm.timeHelper 13 | cf = swarm.allcfs.crazyflies[0] 14 | 15 | cf.takeoff(targetHeight=1.0, duration=TAKEOFF_DURATION) 16 | timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION) 17 | cf.land(targetHeight=0.04, duration=2.5) 18 | timeHelper.sleep(TAKEOFF_DURATION) 19 | 20 | 21 | if __name__ == "__main__": 22 | main() 23 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/output.py: -------------------------------------------------------------------------------- 1 | import os 2 | import math 3 | import numpy as np 4 | 5 | 6 | class Output: 7 | def __init__(self): 8 | self.data = dict() 9 | self.starttime = None 10 | 11 | def update(self, t, crazyflies): 12 | for cf in crazyflies: 13 | x, y, z = cf.position() 14 | roll, pitch, yaw = cf.rpy() 15 | if cf.id not in self.data: 16 | self.data[cf.id] = np.empty((0, 7), float) 17 | self.starttime = t 18 | self.data[cf.id] = np.vstack([self.data[cf.id], np.array([t - self.starttime, x, y, z, roll, pitch, yaw])]) 19 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "crazyflie-firmware"] 2 | path = crazyflie-firmware 3 | url = https://github.com/bitcraze/crazyflie-firmware.git 4 | branch = master 5 | [submodule "ros_ws/src/externalDependencies/libobjecttracker"] 6 | path = ros_ws/src/externalDependencies/libobjecttracker 7 | url = https://github.com/USC-ACTLab/libobjecttracker 8 | [submodule "ros_ws/src/externalDependencies/libmotioncapture"] 9 | path = ros_ws/src/crazyswarm/externalDependencies/libmotioncapture 10 | url = https://github.com/IMRCLab/libmotioncapture.git 11 | [submodule "ros_ws/src/crazyflie_tools"] 12 | path = ros_ws/src/crazyflie_tools 13 | url = https://github.com/whoenig/crazyflie_tools.git 14 | -------------------------------------------------------------------------------- /docs/generate_install_deps_code.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | import yaml 4 | 5 | 6 | def main(): 7 | workflow_path = sys.argv[1] 8 | with open(workflow_path) as f: 9 | workflow = yaml.load(f, Loader=yaml.CLoader) 10 | steps = workflow["jobs"]["build"]["steps"] 11 | depsteps = [s for s in steps if s["name"] == "Install Dependencies"] 12 | assert len(depsteps) == 1 13 | depstep = depsteps[0] 14 | code = depstep["run"] 15 | code = code.replace( 16 | "${{ matrix.pyVer }}", "${CSW_PYTHON}" 17 | ).replace( 18 | "${{ matrix.ros }}", "[ROS version]" 19 | ) 20 | print(code.strip()) 21 | 22 | 23 | if __name__ == "__main__": 24 | main() 25 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/niceHover.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | from pycrazyswarm import * 5 | 6 | Z = 1.0 7 | 8 | if __name__ == "__main__": 9 | swarm = Crazyswarm() 10 | timeHelper = swarm.timeHelper 11 | allcfs = swarm.allcfs 12 | 13 | allcfs.takeoff(targetHeight=Z, duration=1.0+Z) 14 | timeHelper.sleep(1.5+Z) 15 | for cf in allcfs.crazyflies: 16 | pos = np.array(cf.initialPosition) + np.array([0, 0, Z]) 17 | cf.goTo(pos, 0, 1.0) 18 | 19 | print("press button to continue...") 20 | swarm.input.waitUntilButtonPressed() 21 | 22 | allcfs.land(targetHeight=0.02, duration=1.0+Z) 23 | timeHelper.sleep(1.0+Z) 24 | -------------------------------------------------------------------------------- /.github/workflows/ci-docs.yml: -------------------------------------------------------------------------------- 1 | name: Docs build 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | runs-on: ubuntu-latest 8 | steps: 9 | 10 | - name: Checkout 11 | uses: actions/checkout@v2 12 | 13 | # Handle dependencies here instead of conda_env.yaml 14 | # because some packages are conda-forge only, 15 | # which makes the env build much more slowly. 16 | 17 | - name: Install dependencies 18 | run: | 19 | sudo apt install texlive-latex-extra dvipng 20 | sudo pip3 install -U setuptools 21 | sudo pip3 install -U -r docs/requirements.txt 22 | 23 | - name: Build Docs 24 | run: | 25 | cd docs 26 | make html 27 | -------------------------------------------------------------------------------- /pc_permissions.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Sets PC permissions 4 | 5 | sudo groupadd plugdev 6 | sudo usermod -a -G plugdev $USER 7 | 8 | echo -e "# Crazyradio (normal operation) \nSUBSYSTEM==\"usb\", ATTRS{idVendor}==\"1915\", ATTRS{idProduct}==\"7777\", MODE=\"0664\", GROUP=\"plugdev\" \n# Bootloader \nSUBSYSTEM==\"usb\", ATTRS{idVendor}==\"1915\", ATTRS{idProduct}==\"0101\", MODE=\"0664\", GROUP=\"plugdev\"" | sudo tee /etc/udev/rules.d/99-crazyradio.rules 9 | 10 | 11 | echo -e "SUBSYSTEM==\"usb\", ATTRS{idVendor}==\"0483\", ATTRS{idProduct}==\"5740\", MODE=\"0664\", GROUP=\"plugdev\"" | sudo tee /etc/udev/rules.d/99-crazyflie.rules 12 | 13 | sudo udevadm control --reload-rules && udevadm trigger 14 | 15 | 16 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/example_cmd_pos.py: -------------------------------------------------------------------------------- 1 | from pycrazyswarm import * 2 | import numpy as np 3 | 4 | swarm = Crazyswarm() 5 | timeHelper = swarm.timeHelper 6 | allcfs = swarm.allcfs 7 | 8 | Z = 0 9 | # takeoff 10 | while Z < 1.0: 11 | for cf in allcfs.crazyflies: 12 | pos = np.array(cf.initialPosition) + np.array([0, 0, Z]) 13 | cf.cmdPosition(pos) 14 | timeHelper.sleep(0.1) 15 | Z += 0.05 16 | 17 | # land 18 | while Z > 0.0: 19 | for cf in allcfs.crazyflies: 20 | pos = np.array(cf.initialPosition) + np.array([0, 0, Z]) 21 | cf.cmdPosition(pos) 22 | timeHelper.sleep(0.1) 23 | Z -= 0.05 24 | 25 | # turn-off motors 26 | for cf in allcfs.crazyflies: 27 | cf.cmdStop() 28 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/joystick.py: -------------------------------------------------------------------------------- 1 | import time 2 | import rospy 3 | from sensor_msgs.msg import Joy 4 | 5 | class Joystick: 6 | def __init__(self): 7 | self.lastButtonState = 0 8 | self.buttonWasPressed = False 9 | rospy.Subscriber("/joy", Joy, self.joyChanged) 10 | 11 | def joyChanged(self, data): 12 | if (not self.buttonWasPressed and 13 | data.buttons[5] == 1 and 14 | self.lastButtonState == 0): 15 | self.buttonWasPressed = True 16 | self.lastButtonState = data.buttons[5] 17 | 18 | def waitUntilButtonPressed(self): 19 | while not rospy.is_shutdown() and not self.buttonWasPressed: 20 | time.sleep(0.01) 21 | self.buttonWasPressed = False 22 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/testCrazyflieSim.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from numpy.testing import assert_almost_equal 3 | 4 | from cfsim import CrazyflieSim, TimeHelper 5 | 6 | def main(): 7 | 8 | time = TimeHelper() 9 | cf_id = 42 10 | initial = np.array([1, 1, 0]) 11 | cf = CrazyflieSim(id, initial, time) 12 | 13 | duration = 2.5 14 | takeoff_height = 1.0 15 | cf.takeoff(takeoff_height, duration) 16 | 17 | p0 = cf.position() 18 | assert_almost_equal(p0, initial, decimal=3) 19 | 20 | time.step(duration / 2) 21 | pHalf = cf.position() 22 | assert(pHalf[2] > initial[2]) 23 | assert(pHalf[2] < takeoff_height) 24 | 25 | time.step(duration / 2) 26 | p1 = cf.position() 27 | assert_almost_equal(p1[2], takeoff_height, decimal=3) 28 | 29 | 30 | if __name__ == "__main__": 31 | main() 32 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/test_yamlString.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | from pycrazyswarm import * 5 | 6 | def test_yaml_string_load(): 7 | 8 | crazyflies_yaml = """ 9 | crazyflies: 10 | - channel: 100 11 | id: 1 12 | initialPosition: [1.0, 0.0, 0.0] 13 | - channel: 100 14 | id: 10 15 | initialPosition: [0.0, -1.0, 0.0] 16 | """ 17 | 18 | swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args="--sim --vis null") 19 | timeHelper = swarm.timeHelper 20 | cfs = swarm.allcfs.crazyflies 21 | byId = swarm.allcfs.crazyfliesById 22 | 23 | assert len(cfs) == 2 24 | cf1 = byId[1] 25 | assert np.all(cf1.initialPosition == [1.0, 0.0, 0.0]) 26 | 27 | cf10 = byId[10] 28 | assert np.all(cf10.initialPosition == [0.0, -1.0, 0.0]) 29 | -------------------------------------------------------------------------------- /notes/todo.md: -------------------------------------------------------------------------------- 1 | TODO near 2 | ========= 3 | - think about how we want to do real-time control 4 | - think about how to do large-swarm position initialization 5 | - characterize full system latency 6 | - tune attitude controller w/ sinusoid and/or step inputs (done??) 7 | - figure out why ellipse tracking is poor 8 | - measure vicon delay with JTAG debugger 9 | - look into delay compensation in EKF 10 | - version-aware "flash all" script to avoid unneeded flashing 11 | - what to do when interactive object disappears and it's suddenly fit to something else? 12 | 13 | TODO far 14 | ========= 15 | - navigation through obstacles with grouped team 16 | - more automated method to compute ctr of mass from marker config (worth it??) 17 | - increase object tracker robustness to outliers 18 | - compute x-staggered positions to render text from a side view 19 | - create "conductor wand" planner/controller 20 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/keyboard.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import select 3 | import termios 4 | 5 | 6 | class KeyPoller(): 7 | def __enter__(self): 8 | # Save the terminal settings 9 | self.fd = sys.stdin.fileno() 10 | self.new_term = termios.tcgetattr(self.fd) 11 | self.old_term = termios.tcgetattr(self.fd) 12 | 13 | # New terminal setting unbuffered 14 | self.new_term[3] = (self.new_term[3] & ~termios.ICANON & ~termios.ECHO) 15 | termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.new_term) 16 | 17 | return self 18 | 19 | def __exit__(self, type, value, traceback): 20 | termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old_term) 21 | 22 | def poll(self): 23 | dr,dw,de = select.select([sys.stdin], [], [], 0) 24 | if not dr == []: 25 | return sys.stdin.read(1) 26 | return None 27 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/led_colors.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | from pycrazyswarm import * 6 | import uav_trajectory 7 | 8 | if __name__ == "__main__": 9 | swarm = Crazyswarm() 10 | timeHelper = swarm.timeHelper 11 | allcfs = swarm.allcfs 12 | 13 | # Configure the CFs so that the LED ring displays the solid color. 14 | # Overrides the launch file and the firmware default. 15 | for cf in allcfs.crazyflies: 16 | cf.setParam("ring/effect", 7) 17 | 18 | # Generate all possible on/off rgb values. 19 | rgb_bits = [tuple((x >> k) & 0x1 for k in range(3)) for x in range(8)] 20 | 21 | allcfs.takeoff(targetHeight=1.0, duration=2.0) 22 | timeHelper.sleep(2.5) 23 | 24 | TRIALS = 1 25 | for i in range(TRIALS): 26 | for rgb in rgb_bits: 27 | for cf in allcfs.crazyflies: 28 | cf.setLEDColor(*rgb) 29 | timeHelper.sleep(1.0) 30 | 31 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/waypoints_simple.py: -------------------------------------------------------------------------------- 1 | """Single CF: takeoff, follow absolute-coords waypoints, land.""" 2 | 3 | import numpy as np 4 | 5 | from pycrazyswarm import Crazyswarm 6 | 7 | 8 | Z = 1.0 9 | TAKEOFF_DURATION = 2.5 10 | GOTO_DURATION = 3.0 11 | WAYPOINTS = np.array([ 12 | (1.0, 0.0, Z), 13 | (1.0, 1.0, Z), 14 | (0.0, 1.0, Z), 15 | (0.0, 0.0, Z), 16 | ]) 17 | 18 | 19 | def main(): 20 | swarm = Crazyswarm() 21 | timeHelper = swarm.timeHelper 22 | cf = swarm.allcfs.crazyflies[0] 23 | 24 | cf.takeoff(targetHeight=Z, duration=TAKEOFF_DURATION) 25 | timeHelper.sleep(TAKEOFF_DURATION + 1.0) 26 | 27 | for p in WAYPOINTS: 28 | cf.goTo(cf.initialPosition + p, yaw=0.0, duration=GOTO_DURATION) 29 | timeHelper.sleep(GOTO_DURATION + 1.0) 30 | 31 | cf.land(targetHeight=0.05, duration=TAKEOFF_DURATION) 32 | timeHelper.sleep(TAKEOFF_DURATION + 1.0) 33 | 34 | 35 | if __name__ == "__main__": 36 | main() 37 | -------------------------------------------------------------------------------- /docs/gettingstarted.rst: -------------------------------------------------------------------------------- 1 | Getting Started 2 | =============== 3 | 4 | #. Read the Crazyswarm paper to understand the different components of the system. A preprint is available `here `__. 5 | #. Learn more about the Crazyflie platform, e.g. by reading sections 1 and 2 of "Flying Multiple UAVs Using ROS", Springer Book on Robot Operating System (ROS), 2017. A preprint is available `here `__ or by following the `official getting started guide `__. 6 | #. Learn about the Robot Operating System (ROS), e.g. by reading part 1 of "Programming Robots with ROS. A Practical Introduction to the Robot Operating System" by Morgan Quigley, Brian Gerkey, William D. Smart, O'Reilly, 2015. 7 | #. If you use a motion capture for external localization, read the respective user manual. Proper tuning of the system is essential to achieve good tracking performance. -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/cfsim/setup.py: -------------------------------------------------------------------------------- 1 | """Compiles the cffirmware C extension.""" 2 | 3 | from distutils.core import setup, Extension 4 | import os 5 | 6 | import numpy as np 7 | 8 | 9 | fw_dir = "../../../../../../crazyflie-firmware" 10 | include = [ 11 | os.path.join(fw_dir, "src/modules/interface"), 12 | os.path.join(fw_dir, "src/hal/interface"), 13 | os.path.join(fw_dir, "src/utils/interface/lighthouse"), 14 | np.get_include(), 15 | ] 16 | 17 | modules = [ 18 | "collision_avoidance.c", 19 | "planner.c", 20 | "pptraj.c", 21 | "pptraj_compressed.c", 22 | ] 23 | fw_sources = [os.path.join(fw_dir, "src/modules/src", mod) for mod in modules] 24 | 25 | cffirmware = Extension( 26 | "_cffirmware", 27 | include_dirs=include, 28 | sources=fw_sources + ["cffirmware_wrap.c"], 29 | extra_compile_args=[ 30 | "-O3", 31 | "-D__fp16=uint16_t", 32 | ], 33 | ) 34 | 35 | setup(name="cffirmware", version="1.0", ext_modules=[cffirmware]) 36 | -------------------------------------------------------------------------------- /.github/workflows/ci-simonly.yml: -------------------------------------------------------------------------------- 1 | name: Sim-Only Conda CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | strategy: 8 | fail-fast: false 9 | matrix: 10 | os: [ubuntu-18.04, ubuntu-20.04, macos-latest] 11 | python: 12 | - python-version: "3.7" 13 | pyVer: python3 14 | - python-version: "2.7" 15 | pyVer: python2 16 | 17 | runs-on: ${{ matrix.os }} 18 | 19 | steps: 20 | 21 | - name: Checkout 22 | uses: actions/checkout@v2 23 | 24 | - name: Conda 25 | uses: conda-incubator/setup-miniconda@v2.0.0 26 | with: 27 | channels: conda-forge,defaults 28 | mamba-version: "*" 29 | python-version: ${{ matrix.python.python-version }} 30 | environment-file: conda_env.yaml 31 | activate-environment: crazyswarm 32 | 33 | - name: Build 34 | shell: bash -l {0} 35 | run: | 36 | CSW_PYTHON=${{ matrix.python.pyVer }} ./buildSimOnly.sh 37 | 38 | - name: Test 39 | shell: bash -l {0} 40 | run: | 41 | cd ros_ws/src/crazyswarm/scripts 42 | python -m pytest 43 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 whoenig 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/figure8_csv.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | from pycrazyswarm import * 6 | import uav_trajectory 7 | 8 | if __name__ == "__main__": 9 | swarm = Crazyswarm() 10 | timeHelper = swarm.timeHelper 11 | allcfs = swarm.allcfs 12 | 13 | traj1 = uav_trajectory.Trajectory() 14 | traj1.loadcsv("figure8.csv") 15 | 16 | TRIALS = 1 17 | TIMESCALE = 1.0 18 | for i in range(TRIALS): 19 | for cf in allcfs.crazyflies: 20 | cf.uploadTrajectory(0, 0, traj1) 21 | 22 | allcfs.takeoff(targetHeight=1.0, duration=2.0) 23 | timeHelper.sleep(2.5) 24 | for cf in allcfs.crazyflies: 25 | pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) 26 | cf.goTo(pos, 0, 2.0) 27 | timeHelper.sleep(2.5) 28 | 29 | allcfs.startTrajectory(0, timescale=TIMESCALE) 30 | timeHelper.sleep(traj1.duration * TIMESCALE + 2.0) 31 | allcfs.startTrajectory(0, timescale=TIMESCALE, reverse=True) 32 | timeHelper.sleep(traj1.duration * TIMESCALE + 2.0) 33 | 34 | allcfs.land(targetHeight=0.06, duration=2.0) 35 | timeHelper.sleep(3.0) 36 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | crazyswarm 4 | 0.0.3 5 | Crazyswarm: A swarm of Crazyflie-based quadrotors 6 | 7 | Wolfgang Hoenig 8 | 9 | MIT 10 | 11 | http://crazyswarm.readthedocs.io 12 | https://github.com/USC-ACTLab/crazyswarm/issues 13 | 14 | Wolfgang Hoenig 15 | 16 | catkin 17 | 18 | message_generation 19 | std_msgs 20 | tf 21 | crazyflie_cpp 22 | libobjecttracker 23 | libmotioncapture 24 | tf_conversion 25 | 26 | message_runtime 27 | std_msgs 28 | tf 29 | crazyflie_cpp 30 | libobjecttracker 31 | libmotioncapture 32 | 33 | 34 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/cmdFullState.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | from pycrazyswarm import * 6 | import uav_trajectory 7 | 8 | 9 | def executeTrajectory(timeHelper, cf, trajpath, rate=100, offset=np.zeros(3)): 10 | traj = uav_trajectory.Trajectory() 11 | traj.loadcsv(trajpath) 12 | 13 | start_time = timeHelper.time() 14 | while not timeHelper.isShutdown(): 15 | t = timeHelper.time() - start_time 16 | if t > traj.duration: 17 | break 18 | 19 | e = traj.eval(t) 20 | cf.cmdFullState( 21 | e.pos + np.array(cf.initialPosition) + offset, 22 | e.vel, 23 | e.acc, 24 | e.yaw, 25 | e.omega) 26 | 27 | timeHelper.sleepForRate(rate) 28 | 29 | 30 | if __name__ == "__main__": 31 | swarm = Crazyswarm() 32 | timeHelper = swarm.timeHelper 33 | cf = swarm.allcfs.crazyflies[0] 34 | 35 | rate = 30.0 36 | Z = 0.5 37 | 38 | cf.takeoff(targetHeight=Z, duration=Z+1.0) 39 | timeHelper.sleep(Z+2.0) 40 | 41 | executeTrajectory(timeHelper, cf, "figure8.csv", rate, offset=np.array([0, 0, 0.5])) 42 | 43 | cf.notifySetpointsStop() 44 | cf.land(targetHeight=0.03, duration=Z+1.0) 45 | timeHelper.sleep(Z+2.0) 46 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/udp_multicast.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import socket 4 | import struct 5 | 6 | # see https://stackoverflow.com/questions/603852/multicast-in-python 7 | class UdpMulticastSender: 8 | def __init__(self, MCAST_GRP = '224.1.1.1', MCAST_PORT = 5007): 9 | self.MCAST_GRP = MCAST_GRP 10 | self.MCAST_PORT = MCAST_PORT 11 | self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 12 | self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 2) 13 | 14 | def send(self, msg): 15 | self.sock.sendto(msg, (self.MCAST_GRP, self.MCAST_PORT)) 16 | 17 | 18 | class UdpMulticastReceiver: 19 | def __init__(self, MCAST_GRP = '224.1.1.1', MCAST_PORT = 5007): 20 | self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP) 21 | self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 22 | self.sock.bind(('', MCAST_PORT)) # use MCAST_GRP instead of '' to listen only 23 | # to MCAST_GRP, not all groups on MCAST_PORT 24 | mreq = struct.pack("4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY) 25 | self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq) 26 | 27 | def recv(self, bufsize=4096): 28 | return self.sock.recv(bufsize) 29 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/cmdVelocityCircle.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | from pycrazyswarm import * 5 | 6 | 7 | Z = 1.0 8 | sleepRate = 30 9 | 10 | 11 | def goCircle(timeHelper, cf, totalTime, radius, kPosition): 12 | startTime = timeHelper.time() 13 | pos = cf.position() 14 | startPos = cf.initialPosition + np.array([0, 0, Z]) 15 | center_circle = startPos - np.array([radius, 0, 0]) 16 | while True: 17 | time = timeHelper.time() - startTime 18 | omega = 2 * np.pi / totalTime 19 | vx = -radius * omega * np.sin(omega * time) 20 | vy = radius * omega * np.cos(omega * time) 21 | desiredPos = center_circle + radius * np.array( 22 | [np.cos(omega * time), np.sin(omega * time), 0]) 23 | errorX = desiredPos - cf.position() 24 | cf.cmdVelocityWorld(np.array([vx, vy, 0] + kPosition * errorX), yawRate=0) 25 | timeHelper.sleepForRate(sleepRate) 26 | 27 | 28 | if __name__ == "__main__": 29 | swarm = Crazyswarm() 30 | timeHelper = swarm.timeHelper 31 | allcfs = swarm.allcfs 32 | 33 | allcfs.takeoff(targetHeight=Z, duration=1.0+Z) 34 | timeHelper.sleep(2 + Z) 35 | goCircle(timeHelper, allcfs.crazyflies[0], totalTime=4, radius=1, kPosition=1) 36 | -------------------------------------------------------------------------------- /notes/stm32timerclocks.txt: -------------------------------------------------------------------------------- 1 | Note on timer clocks in STM32 2 | James Preiss, March 2016 3 | 4 | Each timer in the STM32 is driven by one of two Advanced Peripheral Bus (APB) clocks. 5 | The APB clocks are divided down from the system clock, but the valid range of divisors 6 | depends on the configured system clock speed. If the CPU is underclocked, 7 | it can be possible for the APB clocks to run as fast as the system clock (divisor of 1). 8 | However, if the CPU is running at full speed, a divisor of 1 is not possible. 9 | The APB clocks must run slower than the system clock. 10 | Furthermore, APB1's maximum speed is slower than APB2's. 11 | 12 | In the Crazyflie, the STM32F405 is running at its maximum possible speed of 168MHz. 13 | The APBs both run at their maximum speeds: APB1 runs at /4 = 42MHz and APB2 runs at /2 = 84 MHz. 14 | 15 | You would think, then, that a timer on APB1 with a clock divider of 1 and prescaler of 1 would run at 42MHz. 16 | Surprisingly this is not true! The STM32 hardware has a "special case" behavior. 17 | When an APB clock divider is > 1, the input frequency to the timers on that APB is *doubled*. 18 | Therefore, our hypothetical timer will count at 84MHz - double the APB1 speed of 42MHz. 19 | 20 | I guess this behavior is designed to enable timers running at full CPU frequency. 21 | It is quite confusing though. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ![Crazyswarm ROS CI](https://github.com/USC-ACTLab/crazyswarm/workflows/Crazyswarm%20ROS%20CI/badge.svg) 2 | ![Sim-Only Conda CI](https://github.com/USC-ACTLab/crazyswarm/workflows/Sim-Only%20Conda%20CI/badge.svg) 3 | [![Documentation Status](https://readthedocs.org/projects/crazyswarm/badge/?version=latest)](https://crazyswarm.readthedocs.io/en/latest/?badge=latest) 4 | 5 | # Crazyswarm 6 | A Large Nano-Quadcopter Swarm. 7 | 8 | 9 | > **Warning** 10 | > This repository is not actively maintained anymore. For new projects, please consider [Crazyswarm2](https://imrclab.github.io/crazyswarm2/). 11 | 12 | 13 | The documentation is available here: http://crazyswarm.readthedocs.io/en/latest/. 14 | 15 | ## Troubleshooting 16 | Please start a [Discussion](https://github.com/USC-ACTLab/crazyswarm/discussions) for... 17 | 18 | - Getting Crazyswarm to work with your hardware setup. 19 | - Advice on how to use the [Crazyswarm Python API](https://crazyswarm.readthedocs.io/en/latest/api.html) to achieve your goals. 20 | - Rough ideas for a new feature. 21 | 22 | Please open an [Issue](https://github.com/USC-ACTLab/crazyswarm/issues) if you believe that fixing your problem will involve a **change in the Crazyswarm source code**, rather than your own configuration files. For example... 23 | 24 | - Bug reports. 25 | - New feature proposals with details. 26 | 27 | -------------------------------------------------------------------------------- /.github/workflows/ci-ros.yml: -------------------------------------------------------------------------------- 1 | name: Crazyswarm ROS CI 2 | 3 | on: [push, pull_request] 4 | 5 | env: 6 | ROS_CI_DESKTOP: "`lsb_release -cs`" # gives Ubuntu name, e.g. [precise|trusty|...] 7 | ROS_PARALLEL_JOBS: "-j8 -l6" 8 | 9 | jobs: 10 | build: 11 | 12 | strategy: 13 | fail-fast: false 14 | matrix: 15 | include: 16 | 17 | - os: ubuntu-18.04 18 | ros: melodic 19 | pyVer: python 20 | 21 | - os: ubuntu-20.04 22 | ros: noetic 23 | pyVer: python3 24 | 25 | runs-on: ${{ matrix.os }} 26 | 27 | steps: 28 | 29 | - name: Checkout 30 | uses: actions/checkout@v2 31 | 32 | - name: Install ROS 33 | uses: ros-tooling/setup-ros@v0.2 34 | with: 35 | required-ros-distributions: ${{ matrix.ros }} 36 | 37 | - name: Install Dependencies 38 | run: | 39 | sudo apt install -y ros-${{ matrix.ros }}-tf ros-${{ matrix.ros }}-tf-conversions ros-${{ matrix.ros }}-joy 40 | sudo apt install -y libpcl-dev libusb-1.0-0-dev 41 | sudo apt install -y swig lib${{ matrix.pyVer }}-dev ${{ matrix.pyVer }}-pip 42 | ${{ matrix.pyVer }} -m pip install pytest numpy PyYAML scipy 43 | 44 | - name: Build 45 | run: | 46 | source /opt/ros/${{ matrix.ros }}/setup.bash 47 | CSW_PYTHON=${{ matrix.pyVer }} ./build.sh 48 | 49 | - name: Test 50 | run: | 51 | source ros_ws/devel/setup.bash 52 | cd ros_ws/src/crazyswarm/scripts 53 | ${{ matrix.pyVer }} -m pytest --version 54 | ${{ matrix.pyVer }} -m pytest 55 | -------------------------------------------------------------------------------- /notes/firmware-code-review.txt: -------------------------------------------------------------------------------- 1 | IMU Bias Computation 2 | -------------------- 3 | The Crazyflie firmware estimates IMU biases with a circular buffer of the past N IMU readings. 4 | It continually computes the variance of the gyroscope buffer to detect when the CF is moving. 5 | When the variance becomes low, the CF recognizes that it is stationary, and takes the mean 6 | of the circular buffer to get the bias value that it will use for the rest of the flight. 7 | As long as the variance is too high, the CF will remain in an uncalibrated state and refuse to fly. 8 | 9 | Experiments show that it takes a very large sample size to get a high-quality estimate of IMU biases. 10 | Recently (May 2016) Bitcraze increased the buffer size from 128 to 1024. 11 | However, for really high-quality results, an even larger sample is desired. 12 | This would require more time for initialization. 13 | More importantly, it would consume a lot of static memory. 14 | 15 | An online algorithm exists[1] to compute the variance of a stream with a constant amount of memory. 16 | This seems like it would be useful, but it can't throw away old values like a circular buffer. 17 | With a big sample, like 5 seconds' worth, we really don't want to discard an entire buffer 18 | because its variance was too high. 19 | 20 | So an ideal solution would be to use the online algorithm for small chunks, like 0.5 seconds' worth. 21 | Then we would keep these chunks in a circular buffer. 22 | It would save significant amount of static memory over the current implementation, 23 | but it would also be very complicated. 24 | Maybe not worth it. 25 | 26 | [1] https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Online_algorithm 27 | -------------------------------------------------------------------------------- /docs/tutorials/hover.rstinclude: -------------------------------------------------------------------------------- 1 | .. _tutorial_hover: 2 | 3 | Hovering (hello, world) 4 | ------------------------ 5 | 6 | After completing :ref:`Installation` and :ref:`Configuration`, 7 | you are ready to test your setup for the first time! 8 | 9 | First, run the test script in simulation mode to make sure your Python interpreter is set up correctly:: 10 | 11 | python hello_world.py --sim 12 | 13 | In the 3D visualization, you should see a Crazyflie take off, hover for a few seconds, and then land. 14 | 15 | If the script runs in simulation, you can move on to real hardware. 16 | First, start the ``crazyswarm_server``:: 17 | 18 | source ros_ws/devel/setup.bash 19 | roslaunch crazyswarm hover_swarm.launch 20 | 21 | It should only take a few seconds to connect to the CFs. 22 | If you have the LED ring extension installed, you can see the connectivity by the color (green=good connectivity; red=bad connectivity). 23 | Furthermore, ``rviz`` will show the estimated pose of all CFs. 24 | If there is an error, such as a faulty configuration or a turned-off Crazyflie, an error message will be shown and the application exits. 25 | If there is a problem in the communication between the motion capture system and the Crazyswarm server, the application will not exit but the positions of the Crazyflies will not appear in rviz. 26 | 27 | If ``crazyswarm_server`` is running correctly and you see CF pose(s) in ``rviz``, 28 | open a new terminal (remember to ``source devel/setup.bash``) and run the test script:: 29 | 30 | python hello_world.py 31 | 32 | You should see the same behavior in real life. 33 | If you have more than one Crazyflie in your ``crazyflies.yaml``, 34 | the script will select one arbitrarily. 35 | -------------------------------------------------------------------------------- /docs/overview.rst: -------------------------------------------------------------------------------- 1 | .. _overview: 2 | 3 | Overview 4 | ======== 5 | 6 | Crazyswarm has the following software architecture. 7 | 8 | .. figure:: images/software_architecture.png 9 | :align: center 10 | :scale: 70% 11 | 12 | 13 | - **crazyflie_tools** 14 | These are command line tools that can be used using `rosrun crazyflie_tools `. These tools include methods to list logging variables, parameters, and to reboot individual crazyflies. 15 | - **crazyswarm_server** 16 | This application is the core of the Crazyswarm. It provides the ROS interface, communicates with the robots and the motion capture system. 17 | - **pycrazyswarm** 18 | This is a simplified Python library to use the Crazyswarm. It has two backends: the physical backend (communicating with the crazyswarm_server) and the simulation backend. The simulator uses parts of the official firmware for software-in-the-loop simulation. For performance reasons, the simulation does not include the dynamics and rather visualizes the setpoints. 19 | - **Helper libraries** 20 | We provide a unified interface for different motion capture systems (`libMotionCapture`), a way to track rigid bodies frame-by-frame even with unique marker configurations (`libObjectTracker`), and a library for the low-level communication with the Crazyflie robots (`crazyflie_cpp`). 21 | 22 | More Information 23 | ---------------- 24 | 25 | - Talk at the `BAM days 2021 `__ 26 | * `Slides `__ [pdf]. 27 | * `Video `__ [youtube]. 28 | - Talk at the `Aerial Swarms Workshop `__ at IROS 2019 29 | * `Slides `__ [pdf]. 30 | -------------------------------------------------------------------------------- /docs/glossary.rst: -------------------------------------------------------------------------------- 1 | Glossary 2 | ======== 3 | 4 | .. glossary:: 5 | 6 | Piecewise Polynomial 7 | A piecewise function is function of the form 8 | 9 | .. math:: 10 | 11 | f(t) = \begin{cases} 12 | f_1(t) :& 0 \leq t < t_1 \\ 13 | f_2(t) :& t_1 \leq t < t_2 \\ 14 | \hfill \vdots \hfill & \hfill \vdots \hfill \hfill \\ 15 | f_n(t) :& t_{n - 1} \leq t \leq t_n. 16 | \end{cases} 17 | 18 | where :math:`0 < t_1 < \cdots < t_n`. It is naturally possible to define 19 | piecewise functions on unbounded domains too, but in typical applications 20 | the domain is bounded. 21 | 22 | A piecewise polynomial is a piecewise function where each of the 23 | :math:`f_i(t)` is a polynomial, possibly vector-valued. 24 | With suitable number of pieces and polynomial degree, piecewise polynomials 25 | make a highly expressive function class that is still simple 26 | and numerically stable. 27 | 28 | .. figure:: images/piecewise_poly.png 29 | :width: 80% 30 | :align: center 31 | 32 | A piecewise polynomial with four pieces. At the point 33 | :math:`\alpha = 2`, this function is continuous but not 34 | differentiable. Such a nonsmooth polynomial would not be used for 35 | quadrotor trajectory planning. 36 | 37 | In Crazyswarm, we use degree-7 polynomials with 4-dimensional 38 | vector-valued output: (x, y, z) position and yaw angle. The quadrotor's 39 | `differential flatness `_ 40 | property makes it possible to compute other states (attitude, acceleration, 41 | angular velocity) from these four values. 42 | 43 | 44 | Setpoint 45 | A collection of desired values for some or all of the quadrotor's state that the feedback controller should try to achieve. 46 | For example, a setpoint may specify position, velocity, and acceleration -- or just position. 47 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/swap6v.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | 5 | from pycrazyswarm import * 6 | import pycrazyswarm.cfsim.cffirmware as firm 7 | import uav_trajectory 8 | 9 | TIMESCALE = 1.0 10 | 11 | POSITIONS = [ 12 | [0, 1.0, 1.5], 13 | [0.25, 0.5, 2.37], 14 | [0.25, -0.5, 2.37], 15 | [-0.25, 0.5, 0.63], 16 | [-0.25, -0.5, 0.63], 17 | [0, -1.0, 1.5] 18 | ] 19 | 20 | OFFSET = [0.0, 0.0, -0.3] 21 | 22 | if __name__ == "__main__": 23 | swarm = Crazyswarm() 24 | timeHelper = swarm.timeHelper 25 | allcfs = swarm.allcfs 26 | 27 | ids = [15, 16, 17, 18, 19, 20] #[15, 16, 17] #range(1, 6+1) 28 | trajIds = [1, 2, 3, 4, 5, 6] #[1, 2, 3, 4, 5, 6] 29 | 30 | cfs = [allcfs.crazyfliesById[i] for i in ids] 31 | root = 'swap6v_pps' 32 | fnames = ['{0}/pp{1}.csv'.format(root, i) for i in trajIds] 33 | # trajs = [piecewise.loadcsv(fname) for fname in fnames] 34 | 35 | T = 0 36 | for cf, fname in zip(cfs, fnames): 37 | traj = uav_trajectory.Trajectory() 38 | traj.loadcsv(fname) 39 | cf.uploadTrajectory(0, 0, traj) 40 | T = max(T, traj.duration) 41 | print("T: ", T * TIMESCALE) 42 | 43 | allcfs.takeoff(targetHeight=1.0, duration=2.0) 44 | timeHelper.sleep(2.5) 45 | 46 | for cf, trajId in zip(cfs, trajIds): 47 | pos = POSITIONS[trajId - 1] 48 | print(pos) 49 | cf.goTo(np.array(pos) + np.array(OFFSET), 0, 3.0) 50 | timeHelper.sleep(3.5) 51 | 52 | allcfs.startTrajectory(0, timescale=TIMESCALE) 53 | timeHelper.sleep(T + 3.0) 54 | allcfs.startTrajectory(0, timescale=TIMESCALE, reverse=True) 55 | timeHelper.sleep(T + 3.0) 56 | 57 | for cf in cfs: 58 | pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0]) 59 | cf.goTo(pos, 0, 3.0) 60 | timeHelper.sleep(3.5) 61 | 62 | allcfs.land(targetHeight=0.06, duration=2.0) 63 | timeHelper.sleep(3.0) 64 | 65 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/test_simOnly.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """Tests for simulation-only functionality.""" 4 | 5 | import numpy as np 6 | 7 | from pycrazyswarm import * 8 | 9 | 10 | def setUp(): 11 | crazyflies_yaml = """ 12 | crazyflies: 13 | - channel: 100 14 | id: 1 15 | initialPosition: [1.0, 0.0, 0.0] 16 | """ 17 | swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args="--sim --vis null") 18 | timeHelper = swarm.timeHelper 19 | return swarm.allcfs, timeHelper 20 | 21 | 22 | def test_attitudeRPY(): 23 | """Checks differential flatness and roll/pitch/yaw calculations.""" 24 | 25 | end = 0.99 * np.pi # Not trying to deal with wrapping here. 26 | yaws = np.linspace(-end, end, 11) 27 | 28 | for yaw in yaws: 29 | allcfs, timeHelper = setUp() 30 | cf = allcfs.crazyflies[0] 31 | Z = 1.0 32 | 33 | cf.takeoff(targetHeight=Z, duration=1.0+Z) 34 | timeHelper.sleep(1.5+Z) 35 | cf.goTo(np.zeros(3), yaw=yaw, duration=1.0, relative=True) 36 | timeHelper.sleep(1.5) 37 | 38 | c = np.cos(yaw) 39 | s = np.sin(yaw) 40 | Ryaw = np.array([ 41 | [c, -s, 0], 42 | [s, c, 0], 43 | [0, 0, 1], 44 | ]) 45 | forward, left, up = Ryaw.T 46 | 47 | dirAngleSigns = [ 48 | ( forward, 1, 1), 49 | (-forward, 1, -1), 50 | ( left, 0, -1), 51 | ( -left, 0, 1), 52 | ] 53 | 54 | for direction, angleIdx, sign in dirAngleSigns: 55 | cf.goTo(direction, yaw=0.0, duration=1.0, relative=True) 56 | timeHelper.sleep(0.25) 57 | rpy = cf.rpy() 58 | assert rpy[angleIdx] * sign > np.radians(20) 59 | assert np.abs(rpy[1 ^ angleIdx]) < np.radians(0.001) 60 | assert np.abs(rpy[2] - yaw) < np.radians(0.001) 61 | timeHelper.sleep(1.0) 62 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/graphVisualization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """Demonstrates the graph visualization feature of the 3D visualizer.""" 4 | 5 | import numpy as np 6 | 7 | from pycrazyswarm import * 8 | 9 | crazyflies_yaml = """ 10 | crazyflies: 11 | - id: 1 12 | channel: 110 13 | initialPosition: [-1.0, 0.0, 0.0] 14 | - id: 2 15 | channel: 120 16 | initialPosition: [-0.5, 0.0, 0.0] 17 | - id: 3 18 | channel: 100 19 | initialPosition: [0.0, 0.0, 0.0] 20 | - id: 4 21 | channel: 110 22 | initialPosition: [0.5, 0.0, 0.0] 23 | - id: 5 24 | channel: 120 25 | initialPosition: [1.0, 0.0, 0.0] 26 | """ 27 | 28 | # Cycle between pentagon and star to show graph visualization. 29 | t = np.linspace(0, 2 * np.pi, 6)[:-1] 30 | positions = np.column_stack([np.cos(t), np.sin(t), np.ones_like(t)]) 31 | permutations = np.stack([np.arange(5), [0, 2, 4, 1, 3]]) 32 | graph_edges_pentagon = ( 33 | (0, 1), 34 | (1, 2), 35 | (2, 3), 36 | (3, 4), 37 | (4, 0), 38 | ) 39 | graph_edges_star = ( 40 | (0, 2), 41 | (0, 3), 42 | (1, 3), 43 | (1, 4), 44 | (2, 4), 45 | ) 46 | 47 | if __name__ == "__main__": 48 | swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml) 49 | timeHelper = swarm.timeHelper 50 | allcfs = swarm.allcfs 51 | visualizer = timeHelper.visualizer 52 | visualizer.setGraph(graph_edges_star) 53 | 54 | cfs = allcfs.crazyflies 55 | 56 | allcfs.takeoff(targetHeight=1.0, duration=2.0) 57 | timeHelper.sleep(2.5) 58 | 59 | # First, cycle between pentagon and star by moving. 60 | n_cycles = 4 61 | for cycle in range(n_cycles): 62 | for i in range(5): 63 | index = permutations[cycle % 2][i] 64 | pos = positions[index] 65 | cfs[i].goTo(pos, 0, 3.0) 66 | 67 | timeHelper.sleep(6.0) 68 | 69 | # Then, cycle between pentagon and star by changing the graph. 70 | edges = (graph_edges_pentagon, graph_edges_star) 71 | for cycle in range(n_cycles): 72 | visualizer.setGraph(edges[cycle % 2]) 73 | timeHelper.sleep(6.0) 74 | -------------------------------------------------------------------------------- /docs/api.rst: -------------------------------------------------------------------------------- 1 | .. _api: 2 | 3 | Python API Reference 4 | ==================== 5 | 6 | The module ``pycrazyswarm``, contained in ``/ros_ws/src/crazyswarm/scripts``, 7 | is the main high-level interface to the Crazyswarm platform. 8 | 9 | The goal of the Crazyswarm project is to reach a state where many diverse 10 | multi-quadrotor research projects can be implemented without going any "deeper" 11 | than writing a ``pycrazyswarm`` script and modifying configuration files. 12 | New projects should try this approach first. If it becomes necessary to 13 | modify Crazyswarm or its submodules, we encourage users to contribute those 14 | changes back via Github pull request. 15 | 16 | All classes in ``pycrazyswarm.crazyflie`` are mirrored by an identically named 17 | class in ``pycrazyswarm.crazyflieSim``. The ``Sim`` version allows testing 18 | ``pycrazyswarm`` scripts in simulation with a 3D visualization before running 19 | anything on real hardware. Since the APIs are identical, the documentation 20 | only refers to the non-``Sim`` versions. 21 | 22 | 23 | ``Crazyflie`` class 24 | ------------------- 25 | .. autoclass:: pycrazyswarm.crazyflie.Crazyflie 26 | :members: 27 | 28 | ``CrazyflieServer`` class 29 | ------------------------- 30 | .. autoclass:: pycrazyswarm.crazyflie.CrazyflieServer 31 | :members: 32 | 33 | ``TimeHelper`` class 34 | -------------------- 35 | .. autoclass:: pycrazyswarm.crazyflie.TimeHelper 36 | :members: 37 | 38 | 39 | Switching between simulation and real hardware 40 | ---------------------------------------------- 41 | Correct ``pycrazyswarm`` scripts should be able to run in both simulation 42 | and on real hardware without modification. Enable simulation and control the 43 | simulation parameters with the command-line flags listed below. 44 | 45 | .. argparse:: 46 | :module: pycrazyswarm.crazyswarm_py 47 | :func: build_argparser 48 | :prog: python2 49 | 50 | Note: rendering video output with the ``--video`` option requires an 51 | installation of `ffmpeg `_ with the ``libx264`` encoder. 52 | This is provided in the Anaconda environment, but must be installed manually 53 | otherwise. 54 | -------------------------------------------------------------------------------- /docs/howto/git_integration.rstinclude: -------------------------------------------------------------------------------- 1 | .. _tutorial_git_integration: 2 | 3 | Crazyswarm Integration with Git 4 | ------------------------------- 5 | 6 | In this tutorial we discuss ways to use the Crazyswarm in your own projects, while properly 7 | version-controlling you custom launch files and scripts. 8 | 9 | 10 | Option 1: Fork 11 | ^^^^^^^^^^^^^^ 12 | 13 | The most straight-forward approach is to fork the Crazyswarm repository. This fork can be 14 | included as a submodule in your own projects and your own launch files and scripts can 15 | be in `ros_ws/src/crazyswarm` along with the provided examples. 16 | 17 | The only downside of this approach is that it becomes rather difficult to update with the 18 | latest version upstream, especially if you have changes in one of the submodules (like 19 | the firmware). 20 | 21 | Option 2: Custom ROS Package 22 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 23 | 24 | If you only have custom launch files, scripts, or firmware, you can simplify the process 25 | by creating a custom out-of-source ROS package: :: 26 | 27 | 28 | any/folder$ catkin_create_pkg my-crazyswarm-pkg 29 | 30 | In particular, you can create this package in any folder, including another git repository. 31 | 32 | Then, add this package to your crazyswarm workspace by using a symbolic link: :: 33 | 34 | crazyswarm/ros_ws/src/userPackages$ ln -s /path/to/my-crazyswarm-pkg . 35 | 36 | In you package, you can have your own launch files, yaml configuration files, and scripts, 37 | similar to the folder structure in the crazyswarm package. 38 | 39 | To run `chooser.py`, you can use its optional arguments:: 40 | 41 | crazyswarm/ros_ws/crazyswarm/scripts$ python chooser.py --configpath ../../userPackages/my-crazyswarm-pkg/launch/ --stm32Fw /path/to/cf2.bin 42 | 43 | To run your own launch file, simply use: :: 44 | 45 | $ roslaunch my-crazyswarm-pkg my-custom-launch-file.launch 46 | 47 | To run your own scripts, you need to adjust the Python package search path so that our 48 | helper library is found: :: 49 | 50 | my-crazyswarm-pkg/scripts$ export PYTHONPATH=$PYTHONPATH:/path/to/crazyswarm/ros_ws/src/crazyswarm/scripts 51 | my-crazyswarm-pkg/scripts$ examplescript.py 52 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/videowriter.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | 3 | import ffmpeg 4 | import numpy as np 5 | 6 | 7 | class VideoWriter: 8 | 9 | def __init__(self, path, dt, shape): 10 | """Initializes video output to a file. 11 | 12 | Args: 13 | path (str): Filesystem path to write output. 14 | dt (float): Time duration in seconds of frame, 1/fps. 15 | shape (tuple(int, int)): Height, width dimensions of video frames. 16 | """ 17 | height, width = shape 18 | size_str = "{}x{}".format(width, height) 19 | # crf=0 option specifies libx264's lossless mode, which still gives 20 | # surprisingly small files. 21 | self.ffmpegProcess = ( 22 | ffmpeg 23 | .input("pipe:", format="rawvideo", pix_fmt="rgb24", s=size_str, r=1.0/dt) 24 | .output(path, vcodec="libx264", crf=0) 25 | .overwrite_output() 26 | .run_async(pipe_stdin=True) 27 | ) 28 | self.path = path 29 | self.shape = shape 30 | self.frames = 0 31 | 32 | def writeFrame(self, frame): 33 | """Appends a video frame to the file. 34 | 35 | Args: 36 | frame (np.ndarray): RGB image of shape `shape + (3,)`, where 37 | `shape` is the constructor arg. 38 | """ 39 | assert len(frame.shape) == 3 40 | assert frame.shape[2] == 3 41 | if frame.shape[:2] != self.shape: 42 | msg = ( 43 | "Rendered frame shape changed after video output started: " 44 | "from {} to {}.".format(self.shape, frame.shape[:2]) 45 | ) 46 | raise ValueError(msg) 47 | framebytes = frame.astype(np.uint8).tobytes() 48 | self.ffmpegProcess.stdin.write(framebytes) 49 | self.frames += 1 50 | 51 | def close(self): 52 | """Closes the video output and prints a status message to console.""" 53 | self.ffmpegProcess.stdin.close() 54 | self.ffmpegProcess.wait() 55 | self.ffmpegProcess = None 56 | print("wrote {} frames to {}".format(self.frames, self.path)) 57 | 58 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/testVispy.py: -------------------------------------------------------------------------------- 1 | # -*-coding: utf-8 -*- 2 | # vispy: gallery 30 3 | # ----------------------------------------------------------------------------- 4 | # Copyright (c) 2015, Vispy Development Team. All Rights Reserved. 5 | # Distributed under the (new) BSD License. See LICENSE.txt for more info. 6 | # ----------------------------------------------------------------------------- 7 | """ 8 | Simple use of SceneCanvas to display a cube with an arcball camera. 9 | """ 10 | import sys 11 | 12 | from vispy import scene, app, io 13 | from vispy.color import Color 14 | from vispy.visuals import transforms 15 | import time 16 | 17 | canvas = scene.SceneCanvas(keys='interactive', size=(800, 600), show=True) 18 | 19 | # Set up a viewbox to display the cube with interactive arcball 20 | view = canvas.central_widget.add_view() 21 | view.bgcolor = '#efefef' 22 | view.camera = 'turntable' 23 | view.padding = 100 24 | 25 | color = Color("#3f51b5") 26 | 27 | # cube = scene.visuals.Cube(size=1, color=color, edge_color="black", 28 | # parent=view.scene) 29 | 30 | 31 | verts, faces, normals, nothin = io.read_mesh("crazyflie2.obj.gz") 32 | 33 | cfs = [] 34 | # cftransforms = [] 35 | for i in range(0, 10): 36 | mesh = scene.visuals.Mesh(vertices=verts, shading='smooth', faces=faces, parent=view.scene) 37 | mesh.transform = transforms.MatrixTransform() 38 | cfs.append(mesh) 39 | 40 | # cube_transform = transforms.MatrixTransform() 41 | # mesh.transform = cube_transform 42 | theta = 0 43 | lastTime = time.time() 44 | 45 | def update(ev): 46 | global cfs, theta, lastTime 47 | x = 0 48 | for cf in cfs: 49 | cf.transform.reset() 50 | cf.transform.rotate(theta, (0, 0, 1)) 51 | cf.transform.scale((0.01, 0.01, 0.01)) 52 | cf.transform.translate((x, 0, 0)) 53 | x += 1.0 54 | # cube_transform.reset() 55 | # cube_transform.rotate(theta, (0, 0, 1)) 56 | theta += 1.0 57 | now = time.time() 58 | dt = now - lastTime 59 | print(1 / dt) 60 | lastTime = now 61 | 62 | timer = app.Timer() 63 | timer.connect(update) 64 | timer.start(0) 65 | 66 | if __name__ == '__main__' and sys.flags.interactive == 0: 67 | canvas.app.run() 68 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/3/18.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.646850,0.000000,0.000000,0.000000,-0.023283,0.025426,-0.011784,0.002158,0.213840,0.000000,0.000000,-0.000000,-0.010754,0.011743,-0.005443,0.000997,1.908800,0.000000,0.000000,-0.000000,0.006363,-0.006956,0.003225,-0.000591,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.639366,-0.021605,-0.016892,0.000963,0.002606,-0.002181,0.000925,-0.000165,0.210383,-0.009980,-0.007803,0.000444,0.001203,-0.001007,0.000427,-0.000076,1.910842,0.005890,0.004595,-0.000275,-0.000712,0.000597,-0.000253,0.000045,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.603016,-0.048589,-0.009774,0.002292,-0.000208,0.000150,-0.000059,0.000010,0.193592,-0.022448,-0.004519,0.001056,-0.000097,0.000069,-0.000027,0.000005,1.920730,0.013190,0.002621,-0.000633,0.000059,-0.000041,0.000016,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.546837,-0.061627,-0.003315,0.002142,0.000020,-0.000010,0.000004,-0.000001,0.167631,-0.028486,-0.001543,0.000987,0.000010,-0.000004,0.000002,-0.000001,1.935939,0.016642,0.000851,-0.000581,-0.000002,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.484050,-0.061786,0.003166,0.002162,-0.000010,0.000013,-0.000014,0.000011,0.138596,-0.028587,0.001449,0.001001,-0.000003,0.000006,-0.000006,0.000005,1.952850,0.016599,-0.000891,-0.000576,0.000005,-0.000004,0.000004,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.427592,-0.048952,0.009737,0.002346,0.000217,-0.000103,0.000235,-0.000168,0.112460,-0.022674,0.004499,0.001091,0.000101,-0.000048,0.000109,-0.000078,1.967984,0.013093,-0.002630,-0.000618,-0.000057,0.000027,-0.000062,0.000045,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.390904,-0.021851,0.017051,0.001015,-0.002642,0.000036,-0.003374,0.002191,0.095460,-0.010129,0.007901,0.000475,-0.001225,0.000017,-0.001566,0.001016,1.977782,0.005825,-0.004553,-0.000262,0.000703,-0.000009,0.000896,-0.000582,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/18.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.429160,0.000000,0.000000,0.000000,-0.030662,0.033500,-0.015530,0.002844,-0.427340,0.000000,0.000000,0.000000,-0.005032,0.005497,-0.002548,0.000467,1.704900,0.000000,0.000000,0.000000,0.049491,-0.054072,0.025067,-0.004591,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.419312,-0.028419,-0.022195,0.001296,0.003432,-0.002875,0.001219,-0.000218,-0.428956,-0.004663,-0.003642,0.000213,0.000563,-0.000472,0.000200,-0.000036,1.720795,0.045871,0.035825,-0.002092,-0.005540,0.004640,-0.001967,0.000351,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.371552,-0.063778,-0.012752,0.003033,-0.000279,0.000197,-0.000078,0.000014,-0.436793,-0.010465,-0.002092,0.000498,-0.000046,0.000032,-0.000013,0.000002,1.797884,0.102943,0.020582,-0.004896,0.000450,-0.000318,0.000126,-0.000022,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.297910,-0.080683,-0.004234,0.002812,0.000019,-0.000013,0.000005,-0.000001,-0.448877,-0.013239,-0.000694,0.000461,0.000003,-0.000002,0.000001,-0.000000,1.916748,0.130227,0.006834,-0.004538,-0.000031,0.000020,-0.000008,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.215815,-0.080683,0.004234,0.002812,-0.000019,0.000018,-0.000018,0.000014,-0.462347,-0.013238,0.000695,0.000461,-0.000003,0.000003,-0.000003,0.000002,2.049255,0.130227,-0.006834,-0.004538,0.000031,-0.000030,0.000030,-0.000022,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.142172,-0.063779,0.012752,0.003034,0.000279,-0.000133,0.000305,-0.000218,-0.474429,-0.010463,0.002092,0.000498,0.000046,-0.000022,0.000050,-0.000036,2.168118,0.102941,-0.020582,-0.004896,-0.000450,0.000215,-0.000492,0.000351,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.094411,-0.028420,0.022196,0.001296,-0.003432,0.000046,-0.004379,0.002844,-0.482264,-0.004662,0.003641,0.000213,-0.000563,0.000008,-0.000718,0.000467,2.245205,0.045870,-0.035825,-0.002092,0.005540,-0.000075,0.007067,-0.004590,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/2/18.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.220850,0.000000,0.000000,0.000000,-0.025545,0.027910,-0.012938,0.002369,0.711940,0.000000,0.000000,-0.000000,-0.033860,0.036994,-0.017150,0.003141,1.641000,0.000000,0.000000,0.000000,-0.013498,0.014748,-0.006837,0.001252,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.212645,-0.023677,-0.018492,0.001080,0.002859,-0.002395,0.001015,-0.000181,0.701065,-0.031384,-0.024511,0.001431,0.003790,-0.003175,0.001346,-0.000240,1.636665,-0.012511,-0.009771,0.000571,0.001511,-0.001266,0.000536,-0.000096,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.172855,-0.053135,-0.010624,0.002527,-0.000232,0.000164,-0.000065,0.000011,0.648323,-0.070431,-0.014082,0.003350,-0.000308,0.000218,-0.000086,0.000015,1.615639,-0.028077,-0.005614,0.001335,-0.000123,0.000087,-0.000034,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.111502,-0.067219,-0.003528,0.002342,0.000016,-0.000011,0.000004,-0.000001,0.566998,-0.089100,-0.004676,0.003105,0.000021,-0.000014,0.000006,-0.000002,1.583220,-0.035519,-0.001864,0.001238,0.000008,-0.000006,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.043107,-0.067219,0.003527,0.002342,-0.000016,0.000015,-0.000015,0.000011,0.476337,-0.089101,0.004675,0.003105,-0.000021,0.000020,-0.000020,0.000015,1.547080,-0.035519,0.001864,0.001238,-0.000008,0.000008,-0.000008,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.018247,-0.053135,0.010624,0.002527,0.000232,-0.000111,0.000254,-0.000181,0.395010,-0.070434,0.014082,0.003350,0.000308,-0.000147,0.000337,-0.000240,1.514660,-0.028077,0.005614,0.001335,0.000123,-0.000059,0.000134,-0.000096,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.058037,-0.023677,0.018492,0.001080,-0.002859,0.000038,-0.003648,0.002370,0.342266,-0.031386,0.024512,0.001432,-0.003790,0.000051,-0.004836,0.003141,1.493635,-0.012511,0.009771,0.000571,-0.001511,0.000020,-0.001928,0.001252,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/4/18.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.052640,0.000000,0.000000,0.000000,-0.026678,0.029147,-0.013512,0.002474,0.096959,0.000000,0.000000,0.000000,-0.004730,0.005167,-0.002395,0.000439,1.939800,0.000000,0.000000,0.000000,0.011821,-0.012915,0.005987,-0.001096,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.061208,-0.024727,-0.019312,0.001127,0.002986,-0.002501,0.001060,-0.000189,0.095440,-0.004384,-0.003424,0.000200,0.000529,-0.000443,0.000188,-0.000034,1.943597,0.010956,0.008556,-0.000500,-0.001323,0.001108,-0.000470,0.000084,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.102765,-0.055495,-0.011097,0.002639,-0.000243,0.000172,-0.000068,0.000012,0.088073,-0.009838,-0.001967,0.000468,-0.000043,0.000030,-0.000012,0.000002,1.962008,0.024585,0.004914,-0.001170,0.000108,-0.000076,0.000030,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.166845,-0.070210,-0.003688,0.002446,0.000017,-0.000011,0.000004,-0.000001,0.076714,-0.012445,-0.000653,0.000434,0.000003,-0.000002,0.000001,-0.000000,1.990393,0.031098,0.001630,-0.001084,-0.000007,0.000005,-0.000002,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.238288,-0.070218,0.003680,0.002447,-0.000016,0.000016,-0.000016,0.000012,0.064051,-0.012445,0.000653,0.000433,-0.000003,0.000003,-0.000003,0.000002,2.022033,0.031092,-0.001635,-0.001084,0.000008,-0.000007,0.000007,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.302383,-0.055514,0.011095,0.002642,0.000243,-0.000116,0.000265,-0.000189,0.052691,-0.009839,0.001966,0.000468,0.000043,-0.000021,0.000047,-0.000034,2.050408,0.024572,-0.004916,-0.001168,-0.000107,0.000051,-0.000117,0.000084,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.343957,-0.024740,0.019321,0.001130,-0.002988,0.000040,-0.003812,0.002476,0.045323,-0.004385,0.003424,0.000200,-0.000530,0.000007,-0.000676,0.000439,2.068807,0.010947,-0.008550,-0.000498,0.001322,-0.000018,0.001686,-0.001095,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/6/18.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.338020,0.000000,0.000000,0.000000,-0.019191,0.020968,-0.009720,0.001780,0.119870,0.000000,0.000000,0.000000,-0.047168,0.051534,-0.023890,0.004375,1.264500,0.000000,0.000000,0.000000,0.025902,-0.028299,0.013119,-0.002403,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.331856,-0.017787,-0.013892,0.000811,0.002148,-0.001799,0.000763,-0.000136,0.104721,-0.043718,-0.034144,0.001994,0.005280,-0.004422,0.001875,-0.000335,1.272819,0.024007,0.018750,-0.001095,-0.002899,0.002428,-0.001029,0.000184,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.301963,-0.039918,-0.007981,0.001899,-0.000175,0.000123,-0.000049,0.000009,0.031250,-0.098111,-0.019616,0.004666,-0.000429,0.000303,-0.000120,0.000021,1.313165,0.053877,0.010772,-0.002563,0.000236,-0.000167,0.000066,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.255871,-0.050499,-0.002650,0.001760,0.000012,-0.000008,0.000003,-0.000001,-0.082036,-0.124116,-0.006513,0.004325,0.000029,-0.000019,0.000008,-0.000002,1.375375,0.068157,0.003577,-0.002375,-0.000016,0.000011,-0.000004,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.204489,-0.050499,0.002650,0.001760,-0.000012,0.000011,-0.000011,0.000009,-0.208324,-0.124116,0.006513,0.004325,-0.000029,0.000028,-0.000028,0.000021,1.444725,0.068157,-0.003577,-0.002375,0.000016,-0.000015,0.000015,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.158396,-0.039918,0.007981,0.001899,0.000175,-0.000083,0.000191,-0.000136,-0.321610,-0.098111,0.019616,0.004666,0.000429,-0.000205,0.000469,-0.000335,1.506935,0.053877,-0.010772,-0.002563,-0.000236,0.000113,-0.000258,0.000184,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.128504,-0.017787,0.013892,0.000811,-0.002148,0.000029,-0.002741,0.001780,-0.395081,-0.043718,0.034144,0.001994,-0.005280,0.000071,-0.006736,0.004375,1.547281,0.024008,-0.018750,-0.001095,0.002899,-0.000039,0.003699,-0.002403,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/5/18.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.320880,0.000000,0.000000,0.000000,-0.017721,0.019361,-0.008976,0.001644,0.369080,0.000000,0.000000,0.000000,-0.050122,0.054761,-0.025386,0.004649,1.342000,0.000000,0.000000,0.000000,0.000729,-0.000797,0.000369,-0.000068,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.326572,-0.016425,-0.012828,0.000749,0.001984,-0.001661,0.000704,-0.000126,0.352982,-0.046456,-0.036282,0.002119,0.005610,-0.004699,0.001992,-0.000356,1.342234,0.000676,0.000528,-0.000031,-0.000082,0.000068,-0.000029,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.354174,-0.036860,-0.007369,0.001753,-0.000161,0.000114,-0.000045,0.000008,0.274910,-0.104256,-0.020845,0.004959,-0.000456,0.000322,-0.000127,0.000022,1.343370,0.001517,0.000303,-0.000072,0.000007,-0.000005,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.396734,-0.046629,-0.002447,0.001625,0.000011,-0.000007,0.000003,-0.000001,0.154530,-0.131889,-0.006921,0.004596,0.000031,-0.000021,0.000008,-0.000002,1.345122,0.001920,0.000101,-0.000067,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.444179,-0.046628,0.002447,0.001625,-0.000011,0.000011,-0.000011,0.000008,0.020331,-0.131889,0.006921,0.004596,-0.000031,0.000030,-0.000030,0.000022,1.347076,0.001920,-0.000101,-0.000067,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.486738,-0.036858,0.007369,0.001753,0.000161,-0.000077,0.000176,-0.000126,-0.100050,-0.104256,0.020845,0.004959,0.000456,-0.000218,0.000498,-0.000356,1.348829,0.001518,-0.000303,-0.000072,-0.000007,0.000003,-0.000007,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.514339,-0.016424,0.012827,0.000749,-0.001983,0.000027,-0.002530,0.001644,-0.178122,-0.046456,0.036282,0.002119,-0.005610,0.000075,-0.007158,0.004649,1.349966,0.000677,-0.000528,-0.000031,0.000082,-0.000001,0.000104,-0.000068,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/7/18.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.196790,0.000000,0.000000,0.000000,-0.028094,0.030698,-0.014231,0.002606,-0.326900,0.000000,0.000000,0.000000,-0.026880,0.029379,-0.013622,0.002495,1.389000,0.000000,0.000000,0.000000,0.046388,-0.050680,0.023494,-0.004303,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.205812,-0.026033,-0.020328,0.001193,0.003145,-0.002635,0.001117,-0.000199,-0.335528,-0.024891,-0.019423,0.001156,0.003009,-0.002522,0.001069,-0.000191,1.403899,0.042996,0.033581,-0.001960,-0.005192,0.004349,-0.001844,0.000329,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.249553,-0.058400,-0.011663,0.002781,-0.000257,0.000180,-0.000071,0.000013,-0.377320,-0.055763,-0.011093,0.002671,-0.000249,0.000172,-0.000068,0.000012,1.476159,0.096497,0.019296,-0.004589,0.000422,-0.000298,0.000118,-0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.316971,-0.073847,-0.003860,0.002573,0.000016,-0.000012,0.000005,-0.000001,-0.441638,-0.070395,-0.003620,0.002457,0.000011,-0.000011,0.000005,-0.000001,1.587584,0.122081,0.006409,-0.004255,-0.000029,0.000019,-0.000008,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.392097,-0.073822,0.003885,0.002570,-0.000018,0.000017,-0.000017,0.000013,-0.513192,-0.070255,0.003751,0.002439,-0.000020,0.000016,-0.000016,0.000012,1.711803,0.122083,-0.006407,-0.004255,0.000029,-0.000028,0.000028,-0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.459468,-0.058341,0.011670,0.002773,0.000255,-0.000122,0.000279,-0.000199,-0.577264,-0.055445,0.011125,0.002623,0.000241,-0.000115,0.000264,-0.000189,1.823233,0.096504,-0.019295,-0.004590,-0.000422,0.000202,-0.000461,0.000329,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.503153,-0.025992,0.020302,0.001184,-0.003139,0.000042,-0.004004,0.002601,-0.618760,-0.024677,0.019284,0.001112,-0.002978,0.000040,-0.003797,0.002467,1.895499,0.043001,-0.033584,-0.001961,0.005193,-0.000070,0.006625,-0.004303,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/waypoints.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | from pycrazyswarm import * 5 | 6 | class Waypoint: 7 | def __init__(self, agent, x, y, z, arrival, duration): 8 | self.agent = agent 9 | self.x = x 10 | self.y = y 11 | self.z = z 12 | self.arrival = arrival 13 | self.duration = duration 14 | 15 | def __lt__(self, other): 16 | return self.arrival < other.arrival 17 | 18 | def __repr__(self): 19 | return "Ag {} at {} s. [{}, {}, {}]".format(self.agent, self.arrival, self.x, self.y, self.z) 20 | 21 | 22 | if __name__ == "__main__": 23 | 24 | # load csv file 25 | data = np.loadtxt("waypoints.csv", skiprows=1, delimiter=',') 26 | 27 | # sort by agents 28 | data[data[:,0].argsort()] 29 | 30 | # convert to internal data structure 31 | waypoints = [] 32 | lastAgent = None 33 | for row in data: 34 | if lastAgent is None or lastAgent != row[0]: 35 | lastTime = 0.0 36 | waypoints.append(Waypoint( 37 | int(row[0]), 38 | row[1], 39 | row[2], 40 | row[3], 41 | row[4], 42 | row[4] - lastTime)) 43 | lastTime = row[4] 44 | lastAgent = int(row[0]) 45 | 46 | # sort waypoints by arrival time 47 | waypoints.sort() 48 | 49 | # print waypoints 50 | print(waypoints) 51 | 52 | # execute waypoints 53 | swarm = Crazyswarm() 54 | timeHelper = swarm.timeHelper 55 | allcfs = swarm.allcfs 56 | 57 | allcfs.takeoff(targetHeight=1.0, duration=2.0) 58 | timeHelper.sleep(2.0) 59 | lastTime = 0.0 60 | for waypoint in waypoints: 61 | if waypoint.arrival == 0: 62 | pos = [waypoint.x, waypoint.y, waypoint.z] 63 | # print(waypoint.agent, pos, 2.0) 64 | cf = allcfs.crazyfliesById[waypoint.agent] 65 | cf.goTo(pos, 0, 2.0) 66 | elif waypoint.duration > 0: 67 | timeHelper.sleep(waypoint.arrival - lastTime) 68 | lastTime = waypoint.arrival 69 | pos = [waypoint.x, waypoint.y, waypoint.z] 70 | # print(waypoint.agent, pos, waypoint.duration) 71 | cf = allcfs.crazyfliesById[waypoint.agent] 72 | cf.goTo(pos, 0, waypoint.duration) 73 | 74 | # land 75 | allcfs.land(targetHeight=0.02, duration=2.0) 76 | timeHelper.sleep(2.0) 77 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/src/mocap_helper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | 6 | // Motion Capture 7 | #include 8 | 9 | int main(int argc, char **argv) 10 | { 11 | ros::init(argc, argv, "mocap_helper"); 12 | 13 | ros::NodeHandle nl("~"); 14 | 15 | std::string motionCaptureType; 16 | nl.param("motion_capture_type", motionCaptureType, "vicon"); 17 | 18 | std::map cfg; 19 | std::string hostname; 20 | nl.getParam("motion_capture_host_name", hostname); 21 | cfg["hostname"] = hostname; 22 | if (nl.hasParam("motion_capture_interface_ip")) { 23 | std::string interface_ip; 24 | nl.param("motion_capture_interface_ip", interface_ip); 25 | cfg["interface_ip"] = interface_ip; 26 | } 27 | 28 | std::unique_ptr mocap(libmotioncapture::MotionCapture::connect(motionCaptureType, cfg)); 29 | if (!mocap) { 30 | throw std::runtime_error("Unknown motion capture type!"); 31 | } 32 | 33 | for (size_t frameId = 0; ros::ok(); ++frameId) { 34 | std::cout << "frame " << frameId << ":" << std::endl; 35 | // Get a frame 36 | mocap->waitForNextFrame(); 37 | const auto& markers = mocap->pointCloud(); 38 | 39 | std::cout << " points:" << std::endl; 40 | 41 | for (size_t i = 0; i < markers.rows(); ++i) { 42 | const auto& point = markers.row(i); 43 | std::cout << " \"" << i << "\": [" << point(0) << "," << point(1) << "," << point(2) << "]" << std::endl; 44 | } 45 | 46 | if (mocap->supportsRigidBodyTracking()) { 47 | const auto& rigidBodies = mocap->rigidBodies(); 48 | 49 | std::cout << " rigidBodies:" << std::endl; 50 | 51 | for (auto const& kv: rigidBodies) { 52 | const auto& body = kv.second; 53 | std::cout << " \"" << body.name() << "\":" << std::endl; 54 | 55 | Eigen::Vector3f position = body.position(); 56 | Eigen::Quaternionf rotation = body.rotation(); 57 | std::cout << " position: [" << position(0) << ", " << position(1) << ", " << position(2) << "]" << std::endl; 58 | std::cout << " rotation: [" << rotation.w() << ", " << rotation.vec()(0) << ", " 59 | << rotation.vec()(1) << ", " << rotation.vec()(2) << "]" << std::endl; 60 | } 61 | } 62 | 63 | 64 | ros::spinOnce(); 65 | } 66 | 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/17.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.314610,0.000000,0.000000,0.000000,0.007182,-0.007778,0.003592,-0.000656,0.354000,0.000000,0.000000,0.000000,-0.051742,0.056156,-0.025957,0.004745,1.731100,0.000000,0.000000,0.000000,-0.001494,0.001611,-0.000743,0.000136,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.316950,0.006796,0.005410,-0.000181,-0.000796,0.000663,-0.000280,0.000050,0.337201,-0.048719,-0.038610,0.001517,0.005749,-0.004792,0.002024,-0.000360,1.730610,-0.001426,-0.001145,0.000026,0.000165,-0.000137,0.000058,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.328611,0.015872,0.003567,-0.000590,0.000064,-0.000045,0.000018,-0.000003,0.254010,-0.112730,-0.024691,0.004461,-0.000465,0.000326,-0.000128,0.000022,1.728140,-0.003388,-0.000796,0.000112,-0.000013,0.000009,-0.000004,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.347493,0.021350,0.001932,-0.000537,-0.000003,0.000003,-0.000002,-0.000000,0.120806,-0.149569,-0.012286,0.004087,0.000030,-0.000017,0.000006,-0.000001,1.724061,-0.004670,-0.000490,0.000100,0.000000,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.370236,0.023595,0.000304,-0.000560,-0.000021,-0.000011,0.000004,0.000000,-0.036945,-0.161818,0.000050,0.004118,-0.000003,-0.000004,0.000002,-0.000001,1.719000,-0.005349,-0.000184,0.000112,0.000012,0.000007,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.393547,0.022410,-0.001549,-0.000672,-0.000014,0.000007,0.000001,-0.000003,-0.194601,-0.149393,0.012350,0.004064,-0.000033,0.000031,-0.000029,0.000022,1.713596,-0.005313,0.000258,0.000182,0.000010,-0.000006,0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.413726,0.017257,-0.003630,-0.000747,-0.000072,0.000034,-0.000079,0.000057,-0.327589,-0.112499,0.024680,0.004435,0.000464,-0.000216,0.000497,-0.000359,1.708729,-0.004229,0.000835,0.000206,0.000018,-0.000009,0.000020,-0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.426546,0.007563,-0.005954,-0.000286,0.000902,-0.000010,0.001144,-0.000746,-0.410587,-0.048591,0.038520,0.001499,-0.005732,0.000053,-0.007232,0.004730,1.705556,-0.001892,0.001475,0.000090,-0.000229,0.000003,-0.000293,0.000190,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/5/5.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.697910,0.000000,0.000000,0.000000,-0.029776,0.032314,-0.014936,0.002730,0.218770,0.000000,0.000000,0.000000,0.012789,-0.013878,0.006414,-0.001172,2.182900,0.000000,0.000000,-0.000000,-0.048751,0.052906,-0.024454,0.004470,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.688242,-0.028039,-0.022224,0.000870,0.003308,-0.002757,0.001165,-0.000207,0.222923,0.012044,0.009547,-0.000372,-0.001421,0.001184,-0.000500,0.000089,2.167071,-0.045907,-0.036386,0.001424,0.005417,-0.004514,0.001907,-0.000340,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.640358,-0.064893,-0.014222,0.002564,-0.000267,0.000187,-0.000074,0.000013,0.243495,0.027883,0.006115,-0.001100,0.000115,-0.000081,0.000032,-0.000006,2.088673,-0.106246,-0.023285,0.004199,-0.000438,0.000307,-0.000120,0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.563666,-0.086128,-0.007092,0.002349,0.000017,-0.000010,0.000004,-0.000001,0.276453,0.037021,0.003057,-0.001008,-0.000008,0.000004,-0.000002,0.000000,1.963110,-0.141012,-0.011611,0.003846,0.000028,-0.000016,0.000006,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.472806,-0.093228,-0.000000,0.002369,0.000000,-0.000001,0.000001,-0.000001,0.315519,0.040096,0.000014,-0.001017,-0.000000,0.000000,-0.000000,0.000000,1.814350,-0.152636,-0.000000,0.003878,0.000000,-0.000002,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.381945,-0.086128,0.007091,0.002349,-0.000017,0.000017,-0.000017,0.000013,0.354612,0.037074,-0.003034,-0.001011,0.000006,-0.000007,0.000007,-0.000006,1.665590,-0.141012,0.011611,0.003846,-0.000028,0.000027,-0.000027,0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.305253,-0.064894,0.014222,0.002565,0.000268,-0.000125,0.000287,-0.000207,0.387641,0.027966,-0.006111,-0.001110,-0.000117,0.000054,-0.000124,0.000090,1.540028,-0.106246,0.023285,0.004199,0.000438,-0.000205,0.000470,-0.000340,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.257368,-0.028040,0.022224,0.000870,-0.003308,0.000031,-0.004175,0.002730,0.408288,0.012097,-0.009583,-0.000383,0.001429,-0.000013,0.001804,-0.001179,1.461629,-0.045907,0.036386,0.001425,-0.005417,0.000051,-0.006836,0.004470,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/6/17.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.254730,0.000000,0.000000,0.000000,0.064803,-0.073303,0.034491,-0.006377,-0.258670,0.000000,0.000000,0.000000,0.008550,-0.008104,0.003503,-0.000611,1.382000,0.000000,0.000000,0.000000,-0.044359,0.049985,-0.023481,0.004337,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.274344,0.055003,0.039233,-0.007196,-0.007544,0.006494,-0.002838,0.000521,-0.255332,0.010421,0.009973,0.001836,-0.000808,0.000578,-0.000179,0.000020,1.368482,-0.038040,-0.027447,0.004582,0.005142,-0.004414,0.001924,-0.000352,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.358016,0.100792,0.005690,-0.010960,0.000590,-0.000547,0.000484,-0.000087,-0.233492,0.034596,0.014141,0.001493,0.000086,0.000086,-0.000347,0.000073,1.309876,-0.071612,-0.005528,0.007157,-0.000403,0.000368,-0.000316,0.000057,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.453978,0.081210,-0.023693,-0.007442,0.002063,-0.000560,0.000189,0.000010,-0.183362,0.066563,0.016325,-0.001691,-0.002139,0.000463,0.000205,-0.000081,1.239599,-0.062467,0.013659,0.004898,-0.001307,0.000351,-0.000110,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.505755,0.018157,-0.036188,-0.000648,0.002459,-0.000793,0.000291,-0.000016,-0.103717,0.088564,0.004425,-0.004350,0.000416,-0.000016,-0.000038,0.000019,1.194615,-0.024646,0.022197,0.000691,-0.001491,0.000484,-0.000178,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.489016,-0.048664,-0.027288,0.006511,0.002297,-0.000584,-0.000136,0.000090,-0.014697,0.085850,-0.006468,-0.002952,0.000422,-0.000142,0.000014,-0.000006,1.191681,0.017275,0.017697,-0.003650,-0.001395,0.000356,0.000081,-0.000053,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.421241,-0.077627,0.000038,0.010297,0.000498,-0.000377,0.000725,-0.000455,0.062021,0.065079,-0.014125,-0.002607,-0.000280,0.000115,-0.000281,0.000207,1.221992,0.038031,0.002031,-0.005917,-0.000266,0.000212,-0.000401,0.000248,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.354341,-0.045388,0.031463,0.007086,-0.006446,0.000273,-0.008784,0.005476,0.110129,0.028231,-0.022330,-0.000933,0.003340,-0.000030,0.004222,-0.002759,1.255930,0.023668,-0.016004,-0.004202,0.003459,-0.000162,0.004761,-0.002950,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/2/17.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.267730,0.000000,0.000000,0.000000,0.031749,-0.034430,0.015909,-0.002907,0.219250,0.000000,0.000000,-0.000000,0.031908,-0.034597,0.015985,-0.002921,1.292500,0.000000,0.000000,-0.000000,0.022528,-0.024424,0.011285,-0.002062,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.257410,0.029946,0.023771,-0.000884,-0.003525,0.002936,-0.001240,0.000221,0.229625,0.030110,0.023911,-0.000877,-0.003542,0.002950,-0.001246,0.000222,1.299826,0.021261,0.016886,-0.000616,-0.002500,0.002082,-0.000879,0.000157,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.206184,0.069526,0.015371,-0.002692,0.000285,-0.000200,0.000079,-0.000014,0.281154,0.069964,0.015503,-0.002694,0.000287,-0.000201,0.000081,-0.000014,1.336215,0.049414,0.010957,-0.001899,0.000202,-0.000142,0.000057,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.123828,0.092710,0.007900,-0.002458,-0.000018,0.000013,-0.000011,0.000002,0.364079,0.093411,0.008033,-0.002456,-0.000017,0.000015,-0.000017,0.000003,1.394794,0.065998,0.005689,-0.001732,-0.000012,0.000010,-0.000011,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.025689,0.101079,0.000427,-0.002548,-0.000046,0.000013,-0.000002,0.000000,0.463052,0.102037,0.000528,-0.002595,-0.000078,0.000019,0.000005,-0.000002,1.464737,0.072130,0.000401,-0.001825,-0.000052,0.000013,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.073233,0.094158,-0.007391,-0.002636,-0.000003,-0.000010,0.000018,-0.000015,0.562966,0.095107,-0.007502,-0.002686,0.000023,-0.000016,0.000016,-0.000014,1.535405,0.067317,-0.005251,-0.001903,0.000006,-0.000009,0.000012,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.157355,0.071410,-0.015458,-0.002904,-0.000296,0.000140,-0.000320,0.000231,0.647894,0.072058,-0.015630,-0.002916,-0.000299,0.000139,-0.000321,0.000232,1.595566,0.051086,-0.011046,-0.002082,-0.000212,0.000100,-0.000229,0.000165,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.210157,0.030990,-0.024511,-0.001026,0.003669,-0.000037,0.004637,-0.003029,0.701159,0.031250,-0.024724,-0.001025,0.003698,-0.000036,0.004673,-0.003053,1.633347,0.022178,-0.017538,-0.000739,0.002626,-0.000026,0.003320,-0.002169,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/3/17.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.338460,0.000000,0.000000,0.000000,0.093860,-0.103662,0.048286,-0.008871,0.551810,0.000000,0.000000,0.000000,0.024154,-0.029168,0.014092,-0.002649,1.914800,0.000000,0.000000,0.000000,0.030940,-0.035559,0.016845,-0.003128,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.308847,0.084749,0.064539,-0.005944,-0.010643,0.009010,-0.003892,0.000709,0.558238,0.016779,0.008981,-0.005958,-0.003037,0.002762,-0.001311,0.000260,1.923897,0.025135,0.017026,-0.004426,-0.003674,0.003216,-0.001455,0.000277,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.170320,0.180080,0.029451,-0.011450,0.000835,-0.000762,0.000674,-0.000131,0.576712,0.012478,-0.013708,-0.007615,0.000202,-0.000444,0.000815,-0.000165,1.959995,0.040500,-0.002147,-0.006373,0.000268,-0.000387,0.000575,-0.000115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.028377,0.207291,-0.000144,-0.006825,0.002559,-0.000561,-0.000178,0.000086,0.568275,-0.035462,-0.031028,-0.000735,0.004419,-0.000983,-0.000323,0.000151,1.992315,0.018864,-0.017330,-0.001714,0.002916,-0.000649,-0.000206,0.000098,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.230605,0.193495,-0.011733,-0.002743,0.000097,-0.000142,0.000082,-0.000017,0.504313,-0.087848,-0.018236,0.005916,-0.000074,-0.000192,0.000159,-0.000040,1.994294,-0.013069,-0.012497,0.002771,0.000012,-0.000140,0.000101,-0.000024,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.409645,0.161851,-0.019930,-0.002737,0.000013,-0.000008,0.000015,-0.000018,0.403999,-0.107153,-0.001307,0.005479,-0.000051,0.000069,-0.000056,0.000029,1.971447,-0.029968,-0.004512,0.002583,-0.000026,0.000036,-0.000027,0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.548830,0.113755,-0.028294,-0.003088,-0.000423,0.000188,-0.000441,0.000325,0.301010,-0.093320,0.015281,0.005851,0.000456,-0.000229,0.000511,-0.000357,1.939545,-0.031246,0.003288,0.002724,0.000178,-0.000093,0.000206,-0.000140,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.630853,0.046786,-0.037986,-0.000314,0.005304,-0.000011,0.006563,-0.004345,0.229203,-0.043957,0.033451,0.003112,-0.005520,0.000112,-0.007165,0.004604,1.914460,-0.016004,0.011728,0.001700,-0.002118,0.000061,-0.002810,0.001781,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/4/5.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.177800,0.000000,0.000000,-0.000000,-0.039443,0.042806,-0.019786,0.003617,0.300990,0.000000,0.000000,0.000000,-0.009963,0.010812,-0.004998,0.000914,1.177100,0.000000,0.000000,0.000000,-0.028646,0.031088,-0.014370,0.002627,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.164993,-0.037143,-0.029439,0.001153,0.004382,-0.003652,0.001543,-0.000275,0.297755,-0.009382,-0.007436,0.000291,0.001107,-0.000923,0.000390,-0.000069,1.167799,-0.026975,-0.021380,0.000837,0.003183,-0.002653,0.001120,-0.000200,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.101562,-0.085962,-0.018839,0.003397,-0.000354,0.000248,-0.000097,0.000017,0.281733,-0.021714,-0.004759,0.000858,-0.000089,0.000063,-0.000025,0.000004,1.121731,-0.062431,-0.013682,0.002467,-0.000257,0.000180,-0.000071,0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.000029,-0.114091,-0.009394,0.003111,0.000023,-0.000013,0.000005,-0.000001,0.256071,-0.028819,-0.002373,0.000786,0.000006,-0.000003,0.000001,-0.000000,1.047950,-0.082859,-0.006822,0.002260,0.000017,-0.000010,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.120389,-0.123496,-0.000000,0.003138,0.000000,-0.000002,0.000001,-0.000001,0.225668,-0.031196,-0.000000,0.000793,0.000000,-0.000000,0.000000,-0.000000,0.960539,-0.089689,0.000000,0.002279,-0.000000,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.240749,-0.114092,0.009394,0.003112,-0.000023,0.000022,-0.000022,0.000017,0.195264,-0.028821,0.002373,0.000786,-0.000006,0.000006,-0.000006,0.000004,0.873128,-0.082858,0.006822,0.002260,-0.000017,0.000016,-0.000016,0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.342341,-0.085963,0.018839,0.003397,0.000354,-0.000166,0.000381,-0.000275,0.169600,-0.021716,0.004759,0.000858,0.000090,-0.000042,0.000096,-0.000069,0.799348,-0.062430,0.013682,0.002467,0.000257,-0.000120,0.000276,-0.000200,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.405773,-0.037143,0.029439,0.001153,-0.004383,0.000041,-0.005531,0.003617,0.153575,-0.009383,0.007437,0.000291,-0.001107,0.000010,-0.001397,0.000914,0.753281,-0.026975,0.021380,0.000837,-0.003183,0.000030,-0.004017,0.002627,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/7/17.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.798290,0.000000,0.000000,0.000000,0.003011,-0.001662,0.000443,-0.000043,0.047356,0.000000,0.000000,-0.000000,-0.035098,0.038444,-0.017842,0.003270,1.731900,0.000000,0.000000,0.000000,-0.018535,0.019936,-0.009178,0.001673,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.796541,0.006089,0.007186,0.002779,-0.000156,0.000032,0.000011,-0.000004,0.036130,-0.032333,-0.025107,0.001658,0.003940,-0.003307,0.001405,-0.000251,1.725796,-0.017817,-0.014385,0.000222,0.002039,-0.001689,0.000711,-0.000126,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.780604,0.028371,0.014982,0.002542,0.000015,-0.000032,0.000069,-0.000024,-0.017866,-0.071681,-0.013772,0.003643,-0.000324,0.000234,-0.000102,0.000020,1.694751,-0.042828,-0.010364,0.001284,-0.000165,0.000118,-0.000052,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.734680,0.066112,0.022917,0.002828,0.000051,0.000153,-0.000491,0.000096,-0.099846,-0.088886,-0.003541,0.003365,0.000028,-0.000051,0.000089,-0.000025,1.642753,-0.060017,-0.006890,0.001121,0.000005,-0.000022,0.000054,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.643014,0.119124,0.027885,-0.001905,-0.003198,0.000799,0.000004,-0.000060,-0.188868,-0.085661,0.007014,0.003862,0.000225,0.000063,-0.000346,0.000075,1.576993,-0.070275,-0.003128,0.001634,0.000340,-0.000088,0.000006,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.500365,0.159987,0.009768,-0.008737,-0.001254,0.000389,0.000039,-0.000058,-0.263636,-0.060383,0.016963,0.001093,-0.002029,0.000447,0.000153,-0.000070,1.505488,-0.070636,0.003131,0.002407,0.000161,-0.000039,-0.000017,0.000016,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.340229,0.150075,-0.020700,-0.011102,-0.000744,0.000423,-0.000917,0.000624,-0.307463,-0.028633,0.013361,-0.001943,0.000054,0.000093,-0.000073,0.000004,1.440509,-0.056703,0.010987,0.002853,0.000248,-0.000126,0.000282,-0.000198,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.222571,0.073370,-0.054902,-0.006366,0.009440,-0.000236,0.012381,-0.007904,-0.324601,-0.007470,0.007779,-0.002108,-0.000426,-0.000094,-0.000270,0.000290,1.397852,-0.025506,0.019833,0.001274,-0.003102,0.000047,-0.003970,0.002573,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/2/5.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-1.213100,0.000000,0.000000,0.000000,0.013506,-0.014658,0.006775,-0.001238,-0.280880,0.000000,0.000000,0.000000,0.030053,-0.032615,0.015075,-0.002756,1.782100,0.000000,0.000000,0.000000,-0.034723,0.037683,-0.017418,0.003184,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-1.208715,0.012718,0.010080,-0.000395,-0.001501,0.001251,-0.000528,0.000094,-0.271122,0.028300,0.022430,-0.000878,-0.003339,0.002783,-0.001176,0.000209,1.770826,-0.032697,-0.025916,0.001015,0.003858,-0.003215,0.001358,-0.000242,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-1.186995,0.029434,0.006450,-0.001163,0.000121,-0.000085,0.000033,-0.000006,-0.222793,0.065496,0.014354,-0.002588,0.000270,-0.000189,0.000074,-0.000013,1.714986,-0.075674,-0.016585,0.002991,-0.000312,0.000219,-0.000086,0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-1.152210,0.039065,0.003216,-0.001065,-0.000008,0.000005,-0.000002,0.000000,-0.145389,0.086928,0.007157,-0.002371,-0.000017,0.000010,-0.000004,0.000001,1.625554,-0.100436,-0.008270,0.002739,0.000020,-0.000012,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-1.110999,0.042285,-0.000000,-0.001074,-0.000000,0.000001,-0.000000,0.000000,-0.053685,0.094094,-0.000000,-0.002391,0.000000,0.000001,-0.000001,0.000001,1.519600,-0.108715,0.000000,0.002762,-0.000000,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-1.069788,0.039064,-0.003216,-0.001065,0.000008,-0.000008,0.000008,-0.000006,0.038019,0.086928,-0.007157,-0.002371,0.000017,-0.000017,0.000017,-0.000013,1.413646,-0.100436,0.008270,0.002739,-0.000020,0.000019,-0.000019,0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-1.035004,0.029433,-0.006451,-0.001163,-0.000121,0.000057,-0.000130,0.000094,0.115423,0.065496,-0.014354,-0.002588,-0.000270,0.000126,-0.000290,0.000209,1.324214,-0.075674,0.016585,0.002991,0.000312,-0.000146,0.000335,-0.000242,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-1.013285,0.012718,-0.010080,-0.000395,0.001501,-0.000014,0.001894,-0.001238,0.163752,0.028300,-0.022430,-0.000878,0.003339,-0.000031,0.004214,-0.002756,1.268374,-0.032697,0.025916,0.001015,-0.003858,0.000036,-0.004869,0.003184,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/3/5.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.374240,0.000000,0.000000,0.000000,-0.009160,0.009940,-0.004594,0.000840,0.077058,0.000000,0.000000,0.000000,-0.037278,0.040455,-0.018699,0.003418,1.642200,0.000000,0.000000,0.000000,-0.031010,0.033653,-0.015555,0.002843,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.377214,-0.008626,-0.006838,0.000267,0.001018,-0.000848,0.000358,-0.000064,0.064954,-0.035104,-0.027824,0.001089,0.004142,-0.003452,0.001458,-0.000260,1.632132,-0.029200,-0.023143,0.000907,0.003445,-0.002872,0.001213,-0.000216,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.391948,-0.019970,-0.004380,0.000788,-0.000082,0.000058,-0.000023,0.000004,0.005004,-0.081247,-0.017807,0.003210,-0.000335,0.000235,-0.000092,0.000016,1.582266,-0.067576,-0.014808,0.002671,-0.000279,0.000195,-0.000077,0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.415554,-0.026515,-0.002190,0.000722,0.000005,-0.000003,0.000001,-0.000000,-0.091016,-0.107836,-0.008882,0.002940,0.000022,-0.000013,0.000005,-0.000001,1.502407,-0.089681,-0.007380,0.002447,0.000018,-0.000010,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.443533,-0.028718,-0.000011,0.000728,0.000000,-0.000000,0.000000,-0.000000,-0.204782,-0.116734,-0.000005,0.002965,0.000000,-0.000001,0.000001,-0.000001,1.407804,-0.097063,0.000007,0.002467,-0.000000,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.471535,-0.026557,0.002172,0.000725,-0.000004,0.000005,-0.000005,0.000004,-0.318556,-0.107855,0.008875,0.002942,-0.000021,0.000021,-0.000021,0.000016,1.313214,-0.089654,0.007391,0.002444,-0.000019,0.000017,-0.000017,0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.495195,-0.020032,0.004380,0.000796,0.000083,-0.000039,0.000089,-0.000064,-0.414599,-0.081272,0.017808,0.003213,0.000335,-0.000157,0.000360,-0.000260,1.233391,-0.067536,0.014807,0.002666,0.000278,-0.000130,0.000299,-0.000216,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.509983,-0.008663,0.006863,0.000272,-0.001023,0.000010,-0.001291,0.000844,-0.474571,-0.035118,0.027834,0.001091,-0.004144,0.000039,-0.005230,0.003420,1.183560,-0.029177,0.023127,0.000903,-0.003442,0.000032,-0.004344,0.002841,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/4/17.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.143480,0.000000,0.000000,0.000000,0.006017,-0.006531,0.003019,-0.000552,-0.127930,0.000000,0.000000,0.000000,0.014871,-0.016139,0.007460,-0.001364,1.980100,0.000000,0.000000,0.000000,-0.002662,0.002888,-0.001335,0.000244,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.141526,0.005665,0.004490,-0.000177,-0.000669,0.000557,-0.000235,0.000042,-0.123101,0.014004,0.011100,-0.000434,-0.001652,0.001377,-0.000582,0.000104,1.979236,-0.002507,-0.001987,0.000077,0.000296,-0.000246,0.000104,-0.000019,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.131853,0.013108,0.002870,-0.000519,0.000054,-0.000038,0.000015,-0.000003,-0.099185,0.032412,0.007104,-0.001281,0.000134,-0.000094,0.000037,-0.000006,1.974954,-0.005803,-0.001273,0.000229,-0.000024,0.000017,-0.000007,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.116366,0.017389,0.001427,-0.000475,-0.000003,0.000002,-0.000001,0.000000,-0.060879,0.043020,0.003543,-0.001173,-0.000009,0.000005,-0.000002,0.000000,1.968094,-0.007706,-0.000636,0.000210,0.000002,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.098027,0.018811,-0.000006,-0.000478,0.000000,0.000000,-0.000000,0.000000,-0.015494,0.046569,0.000001,-0.001183,-0.000000,0.000001,-0.000000,0.000000,1.959963,-0.008345,-0.000002,0.000212,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.079699,0.017368,-0.001435,-0.000473,0.000004,-0.000003,0.000003,-0.000003,0.029894,0.043025,-0.003541,-0.001174,0.000009,-0.000008,0.000008,-0.000006,1.951827,-0.007714,0.000633,0.000211,-0.000001,0.000001,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.064238,0.013080,-0.002869,-0.000516,-0.000054,0.000025,-0.000058,0.000042,0.068206,0.032419,-0.007104,-0.001281,-0.000134,0.000062,-0.000144,0.000104,1.944957,-0.005814,0.001273,0.000230,0.000024,-0.000011,0.000026,-0.000019,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.054588,0.005650,-0.004479,-0.000174,0.000666,-0.000006,0.000841,-0.000550,0.092129,0.014008,-0.011103,-0.000435,0.001653,-0.000015,0.002086,-0.001364,1.940667,-0.002513,0.001991,0.000078,-0.000297,0.000003,-0.000374,0.000245,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/5/17.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.422630,0.000000,0.000000,0.000000,0.006563,-0.007114,0.003286,-0.000601,-0.461090,0.000000,0.000000,0.000000,0.054930,-0.059613,0.027554,-0.005037,1.416900,0.000000,0.000000,0.000000,-0.005019,0.005450,-0.002520,0.000461,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.420495,0.006197,0.004923,-0.000177,-0.000728,0.000606,-0.000256,0.000046,-0.443255,0.051726,0.040997,-0.001606,-0.006103,0.005087,-0.002149,0.000383,1.415272,-0.004720,-0.003736,0.000152,0.000558,-0.000465,0.000197,-0.000035,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.409885,0.014413,0.003202,-0.000552,0.000058,-0.000041,0.000016,-0.000003,-0.354921,0.119708,0.026232,-0.004733,0.000493,-0.000346,0.000136,-0.000024,1.407223,-0.010895,-0.002370,0.000438,-0.000045,0.000032,-0.000012,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.392792,0.019264,0.001665,-0.000509,-0.000006,0.000001,-0.000001,0.000000,-0.213455,0.158864,0.013069,-0.004337,-0.000032,0.000019,-0.000007,0.000001,1.394372,-0.014405,-0.001155,0.000399,0.000002,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.372377,0.021046,0.000106,-0.000530,-0.000005,-0.000000,-0.000000,0.000000,-0.045878,0.171924,-0.000023,-0.004371,0.000002,0.000002,-0.000002,0.000001,1.379211,-0.015517,0.000042,0.000396,-0.000002,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.351760,0.019645,-0.001518,-0.000550,-0.000001,-0.000003,0.000004,-0.000003,0.121656,0.158784,-0.013098,-0.004328,0.000033,-0.000031,0.000031,-0.000024,1.364129,-0.014258,0.001211,0.000383,-0.000005,0.000003,-0.000003,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.334187,0.014941,-0.003211,-0.000614,-0.000065,0.000030,-0.000067,0.000049,0.263024,0.119601,-0.026229,-0.004721,-0.000492,0.000230,-0.000529,0.000382,1.351463,-0.010692,0.002365,0.000414,0.000043,-0.000020,0.000047,-0.000034,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.323125,0.006502,-0.005135,-0.000225,0.000771,-0.000008,0.000977,-0.000638,0.351267,0.051664,-0.040954,-0.001596,0.006095,-0.000057,0.007690,-0.005029,1.343586,-0.004604,0.003655,0.000135,-0.000542,0.000005,-0.000683,0.000447,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/6/5.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.017272,0.000000,0.000000,0.000000,-0.002217,0.002400,-0.001108,0.000202,0.020560,0.000000,0.000000,-0.000000,-0.024810,0.026925,-0.012445,0.002275,2.337400,0.000000,0.000000,0.000000,-0.036909,0.040054,-0.018513,0.003384,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.017995,-0.002101,-0.001674,0.000054,0.000246,-0.000204,0.000086,-0.000015,0.012505,-0.023361,-0.018515,0.000726,0.002757,-0.002298,0.000970,-0.000173,2.325415,-0.034759,-0.027552,0.001076,0.004101,-0.003418,0.001444,-0.000257,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.021603,-0.004916,-0.001110,0.000182,-0.000019,0.000014,-0.000005,0.000001,-0.027388,-0.054060,-0.011844,0.002138,-0.000223,0.000156,-0.000061,0.000011,2.266051,-0.080457,-0.017640,0.003177,-0.000331,0.000232,-0.000091,0.000016,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.027456,-0.006621,-0.000598,0.000172,0.000003,-0.000001,0.000000,-0.000000,-0.091272,-0.071740,-0.005902,0.001957,0.000014,-0.000008,0.000003,-0.000001,2.170957,-0.106803,-0.008805,0.002911,0.000022,-0.000012,0.000005,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.034500,-0.007289,-0.000065,0.000183,0.000003,-0.000000,0.000000,-0.000000,-0.166949,-0.077645,0.000004,0.001972,-0.000000,-0.000001,0.000001,-0.000001,2.058274,-0.115631,-0.000012,0.002938,0.000001,-0.000002,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.041668,-0.006857,0.000503,0.000195,0.000002,0.000001,-0.000001,0.000001,-0.242619,-0.071726,0.005908,0.001955,-0.000014,0.000014,-0.000014,0.000011,1.945567,-0.106848,0.008787,0.002916,-0.000021,0.000021,-0.000021,0.000016,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.047823,-0.005253,0.001112,0.000222,0.000024,-0.000011,0.000024,-0.000017,-0.306484,-0.054039,0.011844,0.002135,0.000223,-0.000104,0.000239,-0.000173,1.850417,-0.080520,0.017640,0.003185,0.000332,-0.000155,0.000357,-0.000257,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.051722,-0.002298,0.001810,0.000085,-0.000274,0.000003,-0.000347,0.000226,-0.346359,-0.023349,0.018506,0.000724,-0.002755,0.000026,-0.003477,0.002273,1.790998,-0.034796,0.027577,0.001082,-0.004106,0.000038,-0.005182,0.003389,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/7/5.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.218250,0.000000,0.000000,0.000000,-0.041345,0.044869,-0.020739,0.003791,-0.858890,0.000000,0.000000,0.000000,-0.007144,0.007753,-0.003584,0.000655,2.482100,0.000000,0.000000,-0.000000,-0.012302,0.013350,-0.006171,0.001128,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.204826,-0.038933,-0.030858,0.001208,0.004594,-0.003829,0.001617,-0.000288,-0.861210,-0.006728,-0.005332,0.000209,0.000794,-0.000662,0.000279,-0.000050,2.478106,-0.011584,-0.009182,0.000359,0.001367,-0.001139,0.000481,-0.000086,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.138338,-0.090105,-0.019747,0.003561,-0.000371,0.000260,-0.000102,0.000018,-0.872699,-0.015570,-0.003412,0.000615,-0.000064,0.000045,-0.000018,0.000003,2.458323,-0.026810,-0.005876,0.001060,-0.000111,0.000077,-0.000030,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.031851,-0.119589,-0.009847,0.003261,0.000024,-0.000014,0.000005,-0.000001,-0.891100,-0.020665,-0.001701,0.000564,0.000004,-0.000002,0.000001,-0.000000,2.426638,-0.035583,-0.002930,0.000970,0.000007,-0.000004,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.094310,-0.129448,-0.000000,0.003289,0.000000,-0.000002,0.000001,-0.000001,-0.912900,-0.022368,0.000000,0.000568,-0.000000,-0.000000,0.000000,-0.000000,2.389100,-0.038516,-0.000000,0.000979,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.220470,-0.119589,0.009847,0.003261,-0.000024,0.000023,-0.000023,0.000018,-0.934701,-0.020665,0.001702,0.000564,-0.000004,0.000004,-0.000004,0.000003,2.351562,-0.035583,0.002930,0.000970,-0.000007,0.000007,-0.000007,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.326957,-0.090106,0.019747,0.003561,0.000371,-0.000174,0.000399,-0.000288,-0.953101,-0.015570,0.003412,0.000615,0.000064,-0.000030,0.000069,-0.000050,2.319877,-0.026810,0.005876,0.001060,0.000111,-0.000052,0.000119,-0.000086,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.393446,-0.038933,0.030858,0.001208,-0.004594,0.000043,-0.005797,0.003791,-0.964590,-0.006727,0.005332,0.000209,-0.000794,0.000007,-0.001002,0.000655,2.300094,-0.011584,0.009182,0.000359,-0.001367,0.000013,-0.001725,0.001128,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/5.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.428140,0.000000,0.000000,0.000000,-0.022189,0.024080,-0.011130,0.002035,-0.492990,0.000000,0.000000,0.000000,0.013340,-0.014477,0.006692,-0.001223,2.064200,0.000000,0.000000,-0.000000,-0.016439,0.017840,-0.008246,0.001507,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.435344,-0.020894,-0.016560,0.000649,0.002465,-0.002055,0.000868,-0.000155,-0.488659,0.012562,0.009957,-0.000390,-0.001482,0.001235,-0.000522,0.000093,2.058862,-0.015480,-0.012270,0.000480,0.001826,-0.001522,0.000643,-0.000115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.471026,-0.048355,-0.010596,0.001911,-0.000199,0.000140,-0.000055,0.000010,-0.467205,0.029075,0.006373,-0.001149,0.000120,-0.000084,0.000033,-0.000006,2.032425,-0.035829,-0.007853,0.001416,-0.000148,0.000104,-0.000041,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.528170,-0.064174,-0.005282,0.001751,0.000013,-0.000008,0.000003,-0.000000,-0.432843,0.038591,0.003179,-0.001052,-0.000008,0.000004,-0.000002,0.000000,1.990081,-0.047554,-0.003917,0.001297,0.000010,-0.000006,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.595868,-0.069459,0.000003,0.001765,-0.000000,-0.000001,0.000001,-0.000000,-0.392130,0.041775,0.000002,-0.001061,-0.000000,0.000001,-0.000000,0.000000,1.939912,-0.051478,-0.000002,0.001308,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.663560,-0.064163,0.005286,0.001749,-0.000013,0.000012,-0.000012,0.000010,-0.351414,0.038597,-0.003176,-0.001053,0.000008,-0.000007,0.000007,-0.000006,1.889739,-0.047562,0.003914,0.001297,-0.000009,0.000009,-0.000009,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.720690,-0.048340,0.010596,0.001910,0.000199,-0.000093,0.000214,-0.000154,-0.317044,0.029084,-0.006373,-0.001150,-0.000120,0.000056,-0.000129,0.000093,1.847386,-0.035839,0.007853,0.001417,0.000148,-0.000069,0.000159,-0.000115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.756359,-0.020885,0.016554,0.000647,-0.002464,0.000023,-0.003110,0.002033,-0.295583,0.012567,-0.009960,-0.000390,0.001483,-0.000014,0.001871,-0.001224,1.820940,-0.015486,0.012274,0.000481,-0.001827,0.000017,-0.002306,0.001508,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/backgroundComputation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """Demonstrate running a slow computation without blocking the main thread. 4 | 5 | Computations that take more than a few milliseconds, such as multi-robot 6 | planning algorithms, often cannot run in the main script thread: 7 | 8 | 1. In simulation, the 3d graphics will not update during a blocking call. 9 | 2. In a streaming setpoint mode such as cmdVelocityWorld, the hardware expects 10 | to receive setpoints several times per second, even if the setpoint is 11 | constant. 12 | 13 | Therefore, we must run the computations in a separate thread or process while 14 | the main thread (1) allows the visualizer to update, and/or (2) sends a 15 | setpoint on the radio. 16 | 17 | Due to the global interpreter lock (GIL) in CPython, it's better to use 18 | processs instead of threads. We illustrate one method here, including passing 19 | arguments to the long-running function and receiving outputs using a queue. 20 | """ 21 | 22 | from __future__ import print_function 23 | import multiprocessing 24 | import time 25 | 26 | import numpy as np 27 | 28 | from pycrazyswarm import * 29 | 30 | 31 | crazyflies_yaml = """ 32 | crazyflies: 33 | - channel: 100 34 | id: 1 35 | initialPosition: [1.0, 0.0, 0.0] 36 | """ 37 | 38 | Z = 1.0 # Altitude. 39 | 40 | 41 | def slow(output_queue, seconds): 42 | time.sleep(seconds) 43 | queue.put("OK") 44 | 45 | 46 | if __name__ == "__main__": 47 | 48 | # The amount of time the slow computation will take. Also demonstrates how 49 | # to pass args. 50 | computation_time = 4.0 51 | 52 | # The queue is used to get outputs back from the background process. 53 | queue = multiprocessing.Queue() 54 | 55 | swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml) 56 | timeHelper = swarm.timeHelper 57 | cf = swarm.allcfs.crazyflies[0] 58 | 59 | # Demonstration using high-level commands: timeHelper.sleep loops for us. 60 | p = multiprocessing.Process(target=slow, args=(queue, computation_time)) 61 | p.start() 62 | print("first process started.") 63 | cf.takeoff(targetHeight=Z, duration=2.0) 64 | timeHelper.sleep(3.0) 65 | p.join() 66 | print("result:", queue.get()) 67 | 68 | # Demonstration using low-level setpoints. 69 | p = multiprocessing.Process(target=slow, args=(queue, computation_time)) 70 | p.start() 71 | print("second process started") 72 | 73 | t0 = timeHelper.time() 74 | while p.is_alive(): 75 | t = timeHelper.time() - t0 76 | x = np.cos(t) 77 | y = np.sin(t) 78 | cf.cmdPosition([x, y, Z], yaw=0.0) 79 | timeHelper.sleep(timeHelper.dt) 80 | 81 | print("result:", queue.get()) 82 | -------------------------------------------------------------------------------- /docs/changelog.rst: -------------------------------------------------------------------------------- 1 | Changelog 2 | ========= 3 | 4 | October 4th, 2019 5 | ----------------- 6 | 7 | 1. We switched to the official firmware. Our own EKF is not used anymore, instead we contributed a full pose update in the official EKF. 8 | 9 | a. We now support single-marker tracking, where only the position and not the pose is tracked by an external system. Attitude is estimated on-board. 10 | b. The EKF is pre-tuned to work with motion capture systems and the Mellinger controller. 11 | 12 | 2. There can be now more than 255 parameters in the firmware. Parameters can be specified by Crazyflie (and not only per type as before). Parameters can be updated using broadcasts mid-flight (e.g., to switch the controller mid-flight). 13 | 14 | 3. We now have full support for OptiTrack, including for using Motive 2.x. Both object tracking by Motive and custom object tracking are supported. 15 | 16 | April 22nd, 2018 17 | ---------------- 18 | 19 | 1. More features have been merged into the official firmware. The high-level execution (takeoff, landing, trajectory execution) is now part of the official firmware. Custom firmware changes still include: 20 | 21 | a. Kalman filter that support full-pose update 22 | b. Improved handling after crashes (particularly important for big quads) 23 | c. Custom LED-ring effect 24 | 25 | 2. The Crazyswarm now uses the latest official crazyflie_ros instead of its own fork. Crazyswarm specific code (e.g, crazyswarm_server) has been moved into ``ros_ws/src/crazyswarm/src``. 26 | 27 | 3. Getting the code into the official code base unfortunately required some API changes: 28 | 29 | a. Support for ellipsoids has been removed. If you need that feature please let us know via github issues. 30 | b. Support for canned trajectories has been removed. 31 | c. The new API allows to upload multiple trajectories and does not have a piece number limit (instead, it is limited by total memory size of all trajectories only). 32 | d. ``Hover`` has been renamed to ``GoTo`` 33 | 34 | March 2nd, 2018 35 | --------------- 36 | 37 | #. Support for heterogeneous swarms, i.e., bigger quadrotors can be used with the CF as control board and the bigQuad deck. Types can be specified in ``ros_ws/src/crazyswarm/launch/crazyflieTypes.yaml``. The list of all CFs has now a type field in ``ros_ws/src/crazyswarm/launch/allCrazyflies.yaml`` (previously ``all49.yaml``). Each type can have its own marker configuration. 38 | 39 | #. Firmware now re-based from the latest official firmware. Some parts are already in the official firmware (e.g., controller). The firmware now supports the use of our Kalman filter (KalmanUSC) or the official Kalman filter, although both have different feature sets. The prebuilt firmware uses our Kalman filter and comes with bigQuad-deck support enabled, i.e., it can be used for both standard Crazyflies and custom ones. 40 | 41 | #. Removed support for avoid-target mode 42 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/crazyswarm_py.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import atexit 3 | 4 | import numpy as np 5 | 6 | from . import genericJoystick 7 | 8 | # Building the parser in a separate function allows sphinx-argparse to 9 | # auto-generate the documentation for the command-line flags. 10 | def build_argparser(parent_parsers=[]): 11 | parser = argparse.ArgumentParser( 12 | formatter_class=argparse.ArgumentDefaultsHelpFormatter, 13 | parents=parent_parsers 14 | ) 15 | parser.add_argument("--sim", help="Run using simulation.", action="store_true") 16 | 17 | group = parser.add_argument_group("Simulation-only", "") 18 | group.add_argument("--vis", help="Visualization backend.", choices=['mpl', 'vispy', 'null'], default="mpl") 19 | group.add_argument("--dt", help="Duration of seconds between rendered visualization frames.", type=float, default=0.1) 20 | group.add_argument("--writecsv", help="Enable CSV output.", action="store_true") 21 | group.add_argument("--disturbance", help="Simulate Gaussian-distributed disturbance when using cmdVelocityWorld.", type=float, default=0.0) 22 | group.add_argument("--maxvel", help="Limit simulated velocity (meters/sec).", type=float, default=np.inf) 23 | group.add_argument("--video", help="Video output path.", type=str) 24 | 25 | return parser 26 | 27 | 28 | class Crazyswarm: 29 | def __init__(self, crazyflies_yaml=None, parent_parser=None, args=None): 30 | if parent_parser is not None: 31 | parents = [parent_parser] 32 | else: 33 | parents = [] 34 | parser = build_argparser(parents) 35 | if isinstance(args, str): 36 | args = args.split() 37 | args, unknown = parser.parse_known_args(args) 38 | 39 | if crazyflies_yaml is None: 40 | crazyflies_yaml = "../launch/crazyflies.yaml" 41 | if crazyflies_yaml.endswith(".yaml"): 42 | crazyflies_yaml = open(crazyflies_yaml, 'r').read() 43 | 44 | if args.sim: 45 | from .crazyflieSim import TimeHelper, CrazyflieServer 46 | self.timeHelper = TimeHelper(args.vis, args.dt, args.writecsv, disturbanceSize=args.disturbance, maxVel=args.maxvel, videopath=args.video) 47 | self.allcfs = CrazyflieServer(self.timeHelper, crazyflies_yaml) 48 | atexit.register(self.timeHelper._atexit) 49 | else: 50 | from .crazyflie import TimeHelper, CrazyflieServer 51 | self.allcfs = CrazyflieServer(crazyflies_yaml) 52 | self.timeHelper = TimeHelper() 53 | if args.writecsv: 54 | print("WARNING: writecsv argument ignored! This is only available in simulation.") 55 | if args.video: 56 | print("WARNING: video argument ignored! This is only available in simulation.") 57 | 58 | self.input = genericJoystick.Joystick(self.timeHelper) 59 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/3/9.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.208980,0.000000,0.000000,0.000000,0.019598,-0.021160,0.009758,-0.001781,0.666460,0.000000,0.000000,0.000000,-0.017295,0.018674,-0.008612,0.001572,1.237900,0.000000,0.000000,0.000000,0.013119,-0.014165,0.006532,-0.001192,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.215395,0.018674,0.014960,-0.000379,-0.002165,0.001798,-0.000757,0.000135,0.660799,-0.016480,-0.013203,0.000334,0.001911,-0.001586,0.000668,-0.000119,1.242194,0.012501,0.010015,-0.000254,-0.001449,0.001203,-0.000507,0.000090,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.247661,0.044184,0.010276,-0.001498,0.000174,-0.000122,0.000047,-0.000008,0.632324,-0.038993,-0.009069,0.001322,-0.000154,0.000107,-0.000042,0.000007,1.263793,0.029577,0.006879,-0.001003,0.000117,-0.000081,0.000032,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.300716,0.060560,0.006151,-0.001357,-0.000011,0.000007,-0.000002,0.000000,0.585503,-0.053444,-0.005428,0.001197,0.000010,-0.000006,0.000002,-0.000000,1.299309,0.040539,0.004118,-0.000908,-0.000008,0.000004,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.366063,0.068769,0.002053,-0.001369,0.000001,-0.000000,0.000000,-0.000000,0.527833,-0.060689,-0.001812,0.001208,-0.000001,0.000000,-0.000000,0.000000,1.343053,0.046034,0.001374,-0.000916,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.435516,0.068769,-0.002053,-0.001369,-0.000001,0.000001,-0.000001,0.000000,0.466541,-0.060689,0.001811,0.001208,0.000001,-0.000001,0.000000,-0.000000,1.389545,0.046035,-0.001374,-0.000916,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.500864,0.060560,-0.006151,-0.001357,0.000011,-0.000011,0.000010,-0.000008,0.408870,-0.053445,0.005428,0.001197,-0.000010,0.000009,-0.000009,0.000007,1.433290,0.040540,-0.004117,-0.000908,0.000007,-0.000007,0.000007,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.553919,0.044185,-0.010276,-0.001498,-0.000174,0.000080,-0.000185,0.000135,0.362048,-0.038994,0.009069,0.001322,0.000154,-0.000071,0.000163,-0.000119,1.468806,0.029578,-0.006879,-0.001003,-0.000117,0.000054,-0.000124,0.000090,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.586185,0.018675,-0.014961,-0.000379,0.002165,-0.000013,0.002710,-0.001781,0.333572,-0.016481,0.013203,0.000335,-0.001911,0.000012,-0.002391,0.001572,1.490405,0.012501,-0.010015,-0.000254,0.001449,-0.000009,0.001814,-0.001192,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/4/3.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.864310,0.000000,0.000000,0.000000,0.010585,-0.011428,0.005270,-0.000962,0.522230,0.000000,0.000000,-0.000000,0.024215,-0.026145,0.012057,-0.002201,0.774710,0.000000,0.000000,0.000000,0.016500,-0.017816,0.008216,-0.001500,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.867775,0.010086,0.008080,-0.000205,-0.001169,0.000971,-0.000409,0.000073,0.530157,0.023074,0.018485,-0.000468,-0.002675,0.002221,-0.000936,0.000166,0.780111,0.015723,0.012596,-0.000319,-0.001823,0.001514,-0.000638,0.000113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.885201,0.023864,0.005550,-0.000809,0.000094,-0.000066,0.000026,-0.000004,0.570024,0.054594,0.012697,-0.001851,0.000215,-0.000150,0.000059,-0.000010,0.807277,0.037201,0.008652,-0.001261,0.000147,-0.000102,0.000040,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.913856,0.032708,0.003322,-0.000733,-0.000006,0.000004,-0.000001,0.000000,0.635577,0.074826,0.007600,-0.001676,-0.000014,0.000008,-0.000003,0.000001,0.851947,0.050989,0.005179,-0.001142,-0.000009,0.000006,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.949149,0.037141,0.001109,-0.000739,0.000000,-0.000000,0.000000,-0.000000,0.716319,0.084969,0.002536,-0.001691,0.000001,-0.000000,0.000000,-0.000000,0.906967,0.057900,0.001728,-0.001152,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.986660,0.037141,-0.001109,-0.000739,-0.000000,0.000001,-0.000000,0.000000,0.802133,0.084968,-0.002536,-0.001691,-0.000001,0.000001,-0.000001,0.000001,0.965443,0.057900,-0.001728,-0.001152,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,1.021954,0.032708,-0.003322,-0.000733,0.000006,-0.000006,0.000006,-0.000004,0.882874,0.074826,-0.007600,-0.001676,0.000014,-0.000013,0.000013,-0.000010,1.020463,0.050989,-0.005179,-0.001142,0.000009,-0.000009,0.000009,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,1.050609,0.023864,-0.005550,-0.000809,-0.000094,0.000043,-0.000100,0.000073,0.948427,0.054593,-0.012697,-0.001851,-0.000215,0.000099,-0.000228,0.000166,1.065132,0.037201,-0.008652,-0.001261,-0.000147,0.000067,-0.000155,0.000113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,1.068035,0.010086,-0.008080,-0.000205,0.001169,-0.000007,0.001463,-0.000962,0.988294,0.023073,-0.018485,-0.000468,0.002675,-0.000017,0.003348,-0.002201,1.092299,0.015723,-0.012596,-0.000319,0.001823,-0.000011,0.002281,-0.001500,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/4/8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.223550,0.000000,0.000000,-0.000000,-0.011005,0.011882,-0.005480,0.001000,0.586740,0.000000,0.000000,0.000000,0.023098,-0.024939,0.011501,-0.002099,1.613300,0.000000,0.000000,0.000000,0.009947,-0.010740,0.004953,-0.000904,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.219948,-0.010486,-0.008401,0.000213,0.001216,-0.001009,0.000425,-0.000076,0.594301,0.022009,0.017632,-0.000447,-0.002552,0.002119,-0.000892,0.000159,1.616556,0.009477,0.007592,-0.000193,-0.001099,0.000912,-0.000384,0.000068,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.201829,-0.024813,-0.005772,0.000841,-0.000098,0.000068,-0.000027,0.000005,0.632327,0.052072,0.012110,-0.001766,0.000205,-0.000143,0.000056,-0.000010,1.632930,0.022421,0.005213,-0.000761,0.000088,-0.000062,0.000024,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.172034,-0.034011,-0.003456,0.000762,0.000006,-0.000004,0.000001,-0.000000,0.694852,0.071367,0.007247,-0.001599,-0.000013,0.000008,-0.000003,0.000000,1.659849,0.030725,0.003118,-0.000689,-0.000005,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.135333,-0.038623,-0.001154,0.000769,-0.000000,0.000000,-0.000000,0.000000,0.771860,0.081037,0.002417,-0.001613,0.000001,-0.000000,0.000000,-0.000000,1.693001,0.034884,0.001039,-0.000694,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.096325,-0.038625,0.001152,0.000769,0.000000,-0.000001,0.000000,-0.000000,0.853702,0.081035,-0.002420,-0.001613,-0.000001,0.000001,-0.000001,0.000000,1.728230,0.034881,-0.001043,-0.000694,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.059621,-0.034015,0.003455,0.000762,-0.000006,0.000006,-0.000006,0.000005,0.930705,0.071360,-0.007249,-0.001598,0.000013,-0.000012,0.000012,-0.000010,1.761375,0.030715,-0.003121,-0.000688,0.000006,-0.000005,0.000005,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.029821,-0.024818,0.005772,0.000841,0.000098,-0.000045,0.000104,-0.000076,0.993221,0.052064,-0.012109,-0.001765,-0.000205,0.000094,-0.000218,0.000159,1.788283,0.022409,-0.005212,-0.000760,-0.000088,0.000041,-0.000094,0.000068,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.011698,-0.010489,0.008403,0.000213,-0.001216,0.000008,-0.001522,0.001000,1.031241,0.022004,-0.017628,-0.000446,0.002551,-0.000016,0.003193,-0.002099,1.804647,0.009471,-0.007587,-0.000192,0.001098,-0.000007,0.001374,-0.000903,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/5/3.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,1.470100,0.000000,0.000000,0.000000,-0.001574,0.001718,-0.000796,0.000146,0.437480,0.000000,0.000000,0.000000,-0.018271,0.019727,-0.009098,0.001661,1.656400,0.000000,0.000000,0.000000,-0.002994,0.003232,-0.001491,0.000272,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,1.469594,-0.001462,-0.001144,0.000063,0.000175,-0.000148,0.000062,-0.000011,0.431499,-0.017410,-0.013948,0.000353,0.002019,-0.001676,0.000706,-0.000125,1.655420,-0.002853,-0.002285,0.000058,0.000331,-0.000275,0.000116,-0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,1.467130,-0.003302,-0.000682,0.000140,-0.000021,0.000009,-0.000004,0.000001,0.401418,-0.041193,-0.009581,0.001397,-0.000163,0.000113,-0.000044,0.000008,1.650491,-0.006749,-0.001570,0.000229,-0.000027,0.000019,-0.000007,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,1.463271,-0.004301,-0.000334,0.000101,-0.000005,0.000000,0.000001,-0.000000,0.351955,-0.056460,-0.005735,0.001265,0.000010,-0.000006,0.000002,-0.000000,1.642387,-0.009251,-0.000940,0.000207,0.000002,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,1.458733,-0.004681,-0.000052,0.000089,-0.000001,0.000000,-0.000000,0.000000,0.291031,-0.064113,-0.001914,0.001276,-0.000001,0.000000,-0.000000,0.000000,1.632405,-0.010505,-0.000314,0.000209,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,1.454088,-0.004522,0.000210,0.000086,-0.000001,-0.000000,-0.000000,0.000000,0.226280,-0.064114,0.001914,0.001276,0.000001,-0.000001,0.000000,-0.000000,1.621796,-0.010505,0.000314,0.000209,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,1.449861,-0.003851,0.000456,0.000077,-0.000003,0.000001,-0.000000,0.000000,0.165356,-0.056461,0.005735,0.001265,-0.000010,0.000010,-0.000010,0.000008,1.611813,-0.009251,0.000940,0.000207,-0.000002,0.000002,-0.000002,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,1.446541,-0.002718,0.000675,0.000076,0.000008,-0.000004,0.000011,-0.000008,0.115892,-0.041194,0.009581,0.001397,0.000163,-0.000075,0.000172,-0.000125,1.603709,-0.006750,0.001570,0.000229,0.000027,-0.000012,0.000028,-0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,1.444582,-0.001117,0.000907,0.000007,-0.000127,0.000000,-0.000156,0.000104,0.085810,-0.017410,0.013948,0.000353,-0.002019,0.000012,-0.002526,0.001661,1.598780,-0.002853,0.002285,0.000058,-0.000331,0.000002,-0.000414,0.000272,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/5/9.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.714750,0.000000,0.000000,0.000000,-0.012652,0.013661,-0.006300,0.001150,0.337050,0.000000,0.000000,0.000000,-0.008082,0.008726,-0.004024,0.000734,1.609600,0.000000,0.000000,-0.000000,0.028880,-0.031182,0.014380,-0.002625,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.710608,-0.012056,-0.009658,0.000245,0.001398,-0.001161,0.000489,-0.000087,0.334405,-0.007701,-0.006169,0.000156,0.000893,-0.000741,0.000312,-0.000055,1.619054,0.027519,0.022046,-0.000559,-0.003191,0.002649,-0.001116,0.000198,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.689778,-0.028525,-0.006635,0.000967,-0.000113,0.000078,-0.000031,0.000005,0.321099,-0.018220,-0.004237,0.000618,-0.000072,0.000050,-0.000020,0.000003,1.666601,0.065111,0.015143,-0.002208,0.000257,-0.000179,0.000070,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.655525,-0.039098,-0.003971,0.000876,0.000007,-0.000004,0.000002,-0.000000,0.299222,-0.024972,-0.002536,0.000560,0.000005,-0.000003,0.000001,-0.000000,1.744784,0.089242,0.009064,-0.001999,-0.000017,0.000010,-0.000003,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.613337,-0.044398,-0.001325,0.000884,-0.000000,0.000000,-0.000000,0.000000,0.272277,-0.028355,-0.000846,0.000564,-0.000000,0.000000,-0.000000,0.000000,1.841080,0.101338,0.003024,-0.002017,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.568497,-0.044398,0.001325,0.000884,0.000000,-0.000001,0.000000,-0.000000,0.243640,-0.028354,0.000847,0.000564,0.000000,-0.000000,0.000000,-0.000000,1.943426,0.101337,-0.003025,-0.002017,-0.000001,0.000002,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.526307,-0.039099,0.003971,0.000876,-0.000007,0.000007,-0.000007,0.000005,0.216698,-0.024967,0.002537,0.000559,-0.000005,0.000004,-0.000004,0.000003,2.039721,0.089240,-0.009065,-0.001999,0.000017,-0.000016,0.000015,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.492054,-0.028527,0.006635,0.000967,0.000113,-0.000052,0.000119,-0.000087,0.194826,-0.018215,0.004237,0.000617,0.000072,-0.000033,0.000076,-0.000055,2.117901,0.065109,-0.015143,-0.002207,-0.000257,0.000118,-0.000272,0.000198,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.471222,-0.012057,0.009659,0.000245,-0.001398,0.000009,-0.001749,0.001150,0.181525,-0.007698,0.006167,0.000156,-0.000893,0.000006,-0.001117,0.000734,2.165447,0.027518,-0.022045,-0.000558,0.003191,-0.000020,0.003993,-0.002625,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/3/10.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.592600,0.000000,0.000000,0.000000,-0.044891,0.048462,-0.022348,0.004079,0.327910,0.000000,0.000000,0.000000,0.007363,-0.007971,0.003680,-0.000672,1.494700,0.000000,0.000000,0.000000,-0.002701,0.002926,-0.001351,0.000247,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.577902,-0.042789,-0.034289,0.000856,0.004959,-0.004117,0.001734,-0.000308,0.330310,0.006972,0.005554,-0.000181,-0.000816,0.000679,-0.000286,0.000051,1.493821,-0.002554,-0.002031,0.000070,0.000299,-0.000249,0.000105,-0.000019,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.503950,-0.101296,-0.023592,0.003422,-0.000398,0.000278,-0.000109,0.000019,0.342283,0.016311,0.003687,-0.000592,0.000070,-0.000045,0.000018,-0.000003,1.489441,-0.005958,-0.001338,0.000219,-0.000027,0.000016,-0.000007,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.382275,-0.138932,-0.014161,0.003104,0.000027,-0.000015,0.000005,-0.000001,0.361729,0.022050,0.002084,-0.000514,0.000003,0.000003,-0.000001,0.000000,1.482348,-0.008035,-0.000752,0.000186,-0.000002,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.232303,-0.157880,-0.004773,0.003139,0.000000,0.000001,0.000000,-0.000000,0.385355,0.024699,0.000577,-0.000490,0.000006,-0.000001,0.000000,-0.000000,1.473744,-0.008992,-0.000210,0.000176,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.072790,-0.158005,0.004652,0.003147,0.000004,-0.000003,0.000001,-0.000001,0.410146,0.024404,-0.000863,-0.000471,0.000003,-0.000000,0.000000,0.000000,1.464717,-0.008891,0.000308,0.000170,-0.000001,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.077414,-0.139256,0.014089,0.003128,-0.000024,0.000024,-0.000024,0.000019,0.433219,0.021277,-0.002257,-0.000456,0.000006,-0.000004,0.000004,-0.000003,1.456304,-0.007766,0.000815,0.000167,-0.000002,0.000001,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.199457,-0.101677,0.023613,0.003460,0.000403,-0.000185,0.000426,-0.000310,0.451786,0.015400,-0.003636,-0.000500,-0.000059,0.000027,-0.000063,0.000046,1.449520,-0.005633,0.001324,0.000185,0.000022,-0.000010,0.000023,-0.000017,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.273727,-0.042999,0.034437,0.000886,-0.004988,0.000031,-0.006244,0.004104,0.463000,0.006470,-0.005198,-0.000112,0.000746,-0.000004,0.000932,-0.000613,1.445414,-0.002371,0.001903,0.000043,-0.000274,0.000002,-0.000342,0.000225,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/3/3.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.543600,0.000000,0.000000,0.000000,-0.004536,0.004900,-0.002260,0.000413,-0.122760,0.000000,0.000000,0.000000,0.036899,-0.039841,0.018374,-0.003354,1.117100,0.000000,0.000000,0.000000,0.000784,-0.000846,0.000390,-0.000071,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.542117,-0.004317,-0.003454,0.000092,0.000501,-0.000417,0.000175,-0.000031,-0.110682,0.035159,0.028165,-0.000715,-0.004077,0.003385,-0.001426,0.000253,1.117357,0.000749,0.000601,-0.000014,-0.000087,0.000072,-0.000030,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.534667,-0.010192,-0.002359,0.000349,-0.000041,0.000028,-0.000011,0.000002,-0.049937,0.083180,0.019342,-0.002821,0.000328,-0.000229,0.000089,-0.000016,1.118653,0.001777,0.000416,-0.000059,0.000007,-0.000005,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.522443,-0.013940,-0.001403,0.000312,0.000002,-0.000001,0.000001,-0.000000,0.049938,0.113998,0.011574,-0.002554,-0.000021,0.000012,-0.000004,0.000001,1.120790,0.002442,0.000251,-0.000055,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.507414,-0.015806,-0.000462,0.000314,-0.000000,0.000000,-0.000000,0.000000,0.172944,0.129441,0.003859,-0.002576,0.000001,-0.000001,0.000000,-0.000000,1.123428,0.002779,0.000085,-0.000056,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.491459,-0.015790,0.000478,0.000313,0.000000,-0.000000,0.000000,-0.000000,0.303668,0.129434,-0.003866,-0.002576,-0.000001,0.000002,-0.000001,0.000001,1.126237,0.002783,-0.000082,-0.000056,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.476459,-0.013896,0.001416,0.000310,-0.000003,0.000002,-0.000002,0.000002,0.426661,0.113979,-0.011579,-0.002553,0.000021,-0.000020,0.000020,-0.000016,1.128882,0.002453,-0.000248,-0.000055,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.464289,-0.010133,0.002359,0.000343,0.000040,-0.000018,0.000042,-0.000031,0.526513,0.083156,-0.019342,-0.002819,-0.000328,0.000151,-0.000348,0.000253,1.131032,0.001791,-0.000416,-0.000061,-0.000007,0.000003,-0.000007,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.456891,-0.004281,0.003430,0.000086,-0.000496,0.000003,-0.000621,0.000408,0.587237,0.035145,-0.028156,-0.000713,0.004075,-0.000025,0.005099,-0.003352,1.132340,0.000757,-0.000606,-0.000016,0.000088,-0.000001,0.000110,-0.000072,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/3/8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.377930,0.000000,0.000000,-0.000000,0.031343,-0.033928,0.015665,-0.002861,0.003553,0.000000,0.000000,-0.000000,0.034024,-0.036746,0.016949,-0.003094,1.147300,0.000000,0.000000,0.000000,0.001343,-0.001242,0.000530,-0.000091,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.367711,0.029692,0.023663,-0.000759,-0.003473,0.002890,-0.001221,0.000218,0.014686,0.032401,0.025942,-0.000676,-0.003760,0.003123,-0.001316,0.000234,1.147840,0.001701,0.001664,0.000345,-0.000124,0.000087,-0.000027,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.316702,0.069494,0.015700,-0.002556,0.000278,-0.000202,0.000095,-0.000017,0.070633,0.076572,0.017755,-0.002619,0.000303,-0.000212,0.000085,-0.000015,1.151489,0.005862,0.002483,0.000284,0.000012,0.000010,-0.000043,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.233909,0.093782,0.008750,-0.002159,0.000095,-0.000015,-0.000012,0.000004,0.162501,0.104779,0.010553,-0.002354,-0.000007,0.000008,-0.000005,0.000001,1.160105,0.011581,0.003046,-0.000120,-0.000273,0.000061,0.000019,-0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.133463,0.105071,0.002612,-0.002007,-0.000001,-0.000005,0.000004,-0.000001,0.275477,0.118818,0.003486,-0.002355,0.000001,-0.000001,0.000001,-0.000000,1.174410,0.016580,0.001759,-0.000532,0.000004,0.000012,-0.000010,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.027791,0.104260,-0.003428,-0.002017,-0.000001,0.000002,-0.000001,0.000001,0.395426,0.118724,-0.003580,-0.002356,-0.000001,0.000002,-0.000001,0.000001,1.192226,0.018535,0.000209,-0.000509,0.000001,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.071024,0.091356,-0.009468,-0.001999,0.000017,-0.000016,0.000016,-0.000012,0.508214,0.104499,-0.010636,-0.002335,0.000019,-0.000018,0.000018,-0.000014,1.210462,0.017428,-0.001315,-0.000506,0.000003,-0.000003,0.000003,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.150918,0.066419,-0.015547,-0.002210,-0.000261,0.000119,-0.000276,0.000201,0.599747,0.076216,-0.017738,-0.002579,-0.000301,0.000138,-0.000318,0.000232,1.226070,0.013280,-0.002850,-0.000550,-0.000056,0.000026,-0.000060,0.000043,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.199364,0.028002,-0.022460,-0.000534,0.003240,-0.000019,0.004051,-0.002664,0.655397,0.032204,-0.025803,-0.000650,0.003733,-0.000023,0.004671,-0.003071,1.235904,0.005781,-0.004566,-0.000200,0.000686,-0.000007,0.000868,-0.000567,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/4/9.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.008094,0.000000,0.000000,0.000000,-0.012228,0.013203,-0.006089,0.001111,1.038800,0.000000,0.000000,0.000000,-0.028527,0.030801,-0.014205,0.002593,1.807900,0.000000,0.000000,0.000000,0.008522,-0.009201,0.004243,-0.000774,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.004091,-0.011652,-0.009335,0.000236,0.001351,-0.001122,0.000472,-0.000084,1.029462,-0.027183,-0.021777,0.000552,0.003152,-0.002617,0.001102,-0.000196,1.810689,0.008120,0.006505,-0.000165,-0.000941,0.000782,-0.000329,0.000059,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.016041,-0.027570,-0.006412,0.000935,-0.000109,0.000076,-0.000030,0.000005,0.982495,-0.064316,-0.014959,0.002180,-0.000254,0.000177,-0.000069,0.000012,1.824719,0.019212,0.004468,-0.000651,0.000076,-0.000053,0.000021,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.049146,-0.037787,-0.003838,0.000846,0.000007,-0.000004,0.000001,-0.000000,0.905267,-0.088152,-0.008954,0.001975,0.000016,-0.000010,0.000003,-0.000001,1.847789,0.026332,0.002674,-0.000590,-0.000005,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.089920,-0.042909,-0.001281,0.000854,-0.000000,0.000000,-0.000000,0.000000,0.810145,-0.100102,-0.002988,0.001992,-0.000001,0.000000,-0.000000,0.000000,1.876203,0.029901,0.000892,-0.000595,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.133256,-0.042909,0.001281,0.000854,0.000000,-0.000001,0.000000,-0.000000,0.709048,-0.100102,0.002988,0.001992,0.000001,-0.000001,0.000001,-0.000001,1.906401,0.029901,-0.000893,-0.000595,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.174031,-0.037787,0.003838,0.000847,-0.000007,0.000007,-0.000007,0.000005,0.613925,-0.088153,0.008953,0.001975,-0.000016,0.000015,-0.000015,0.000012,1.934815,0.026331,-0.002675,-0.000590,0.000005,-0.000005,0.000005,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.207135,-0.027569,0.006412,0.000935,0.000109,-0.000050,0.000115,-0.000084,0.536696,-0.064317,0.014959,0.002181,0.000254,-0.000116,0.000269,-0.000196,1.957882,0.019211,-0.004468,-0.000651,-0.000076,0.000035,-0.000080,0.000058,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.227267,-0.011652,0.009335,0.000236,-0.001351,0.000008,-0.001691,0.001111,0.489728,-0.027184,0.021777,0.000552,-0.003152,0.000019,-0.003944,0.002593,1.971911,0.008119,-0.006504,-0.000165,0.000941,-0.000006,0.001178,-0.000774,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/5/8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,1.042900,0.000000,0.000000,0.000000,-0.016801,0.018145,-0.008369,0.001528,-0.167320,0.000000,0.000000,0.000000,0.025765,-0.027819,0.012829,-0.002342,2.052200,0.000000,0.000000,0.000000,-0.022611,0.024413,-0.011259,0.002055,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,1.037403,-0.016000,-0.012811,0.000333,0.001856,-0.001542,0.000650,-0.000115,-0.158886,0.024551,0.019669,-0.000498,-0.002847,0.002363,-0.000996,0.000177,2.044799,-0.021545,-0.017261,0.000437,0.002498,-0.002074,0.000874,-0.000155,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,1.009772,-0.037823,-0.008782,0.001284,-0.000151,0.000105,-0.000041,0.000007,-0.116466,0.058090,0.013511,-0.001969,0.000229,-0.000160,0.000062,-0.000011,2.007572,-0.050977,-0.011856,0.001728,-0.000201,0.000140,-0.000055,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.964372,-0.051810,-0.005252,0.001159,0.000009,-0.000005,0.000002,-0.000000,-0.046713,0.079620,0.008087,-0.001784,-0.000015,0.000009,-0.000003,0.000001,1.946361,-0.069870,-0.007097,0.001565,0.000013,-0.000008,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.908475,-0.058817,-0.001750,0.001170,-0.000000,0.000000,-0.000000,0.000000,0.039201,0.090413,0.002699,-0.001799,0.000001,-0.000000,0.000000,-0.000000,1.870966,-0.079341,-0.002368,0.001579,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.849077,-0.058809,0.001758,0.001170,0.000001,-0.000001,0.000000,-0.000000,0.130515,0.090415,-0.002698,-0.001800,-0.000001,0.000001,-0.000001,0.000001,1.790836,-0.079342,0.002368,0.001579,0.000001,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.793196,-0.051786,0.005261,0.001160,-0.000010,0.000009,-0.000009,0.000007,0.216432,0.079624,-0.008086,-0.001784,0.000015,-0.000014,0.000014,-0.000011,1.715441,-0.069871,0.007097,0.001565,-0.000013,0.000012,-0.000012,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.747827,-0.037783,0.008787,0.001281,0.000149,-0.000068,0.000158,-0.000115,0.286190,0.058095,-0.013511,-0.001970,-0.000229,0.000105,-0.000243,0.000177,1.654229,-0.050978,0.011856,0.001728,0.000201,-0.000092,0.000213,-0.000155,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.720236,-0.015969,0.012793,0.000324,-0.001852,0.000011,-0.002317,0.001523,0.328615,0.024555,-0.019671,-0.000499,0.002847,-0.000018,0.003563,-0.002342,1.617002,-0.021546,0.017261,0.000437,-0.002498,0.000015,-0.003126,0.002055,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/launch/hover_swarm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | # Logging configuration (Use enable_logging to actually enable logging) 11 | genericLogTopics: ["log1"] 12 | genericLogTopicFrequencies: [10] 13 | genericLogTopic_log1_Variables: ["stateEstimate.x", "ctrltarget.x"] 14 | # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or 15 | # allCrazyflies.yaml to set per drone) 16 | firmwareParams: 17 | commander: 18 | enHighLevel: 1 19 | stabilizer: 20 | estimator: 2 # 1: complementary, 2: kalman 21 | controller: 2 # 1: PID, 2: mellinger 22 | ring: 23 | effect: 16 # 6: double spinner, 7: solid color, 16: packetRate 24 | solidBlue: 255 # if set to solid color 25 | solidGreen: 0 # if set to solid color 26 | solidRed: 0 # if set to solid color 27 | headlightEnable: 0 28 | locSrv: 29 | extPosStdDev: 1e-3 30 | extQuatStdDev: 0.5e-1 31 | kalman: 32 | resetEstimation: 1 33 | # tracking 34 | motion_capture_type: "vicon" # one of none,vicon,optitrack,optitrack_closed_source,qualisys,vrpn 35 | object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker 36 | send_position_only: False # set to False to send position+orientation; set to True to send position only 37 | motion_capture_host_name: "vicon" 38 | # motion_capture_interface_ip: "" # optional for optitrack with multiple interfaces 39 | save_point_clouds: "/dev/null" # set to a valid path to log mocap point cloud binary file. 40 | print_latency: False 41 | write_csvs: False 42 | force_no_cache: False 43 | enable_parameters: True 44 | enable_logging: False 45 | enable_logging_pose: True 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.133590,0.000000,0.000000,0.000000,-0.018087,0.019534,-0.009010,0.001645,-0.025934,0.000000,0.000000,0.000000,0.024455,-0.026403,0.012176,-0.002222,1.811100,0.000000,0.000000,0.000000,-0.006099,0.006571,-0.003028,0.000552,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.139508,-0.017223,-0.013790,0.000360,0.001999,-0.001660,0.000699,-0.000124,-0.017928,0.023304,0.018671,-0.000471,-0.002702,0.002243,-0.000945,0.000168,1.809097,-0.005840,-0.004699,0.000093,0.000672,-0.000557,0.000234,-0.000042,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.169247,-0.040702,-0.009437,0.001393,-0.000161,0.000112,-0.000044,0.000008,0.022340,0.055147,0.012831,-0.001868,0.000218,-0.000152,0.000059,-0.000010,1.798958,-0.013943,-0.003318,0.000441,-0.000054,0.000037,-0.000014,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.218078,-0.055689,-0.005600,0.001260,0.000007,-0.000008,0.000003,-0.000000,0.088565,0.075601,0.007688,-0.001692,-0.000015,0.000008,-0.000003,0.000001,1.782109,-0.019353,-0.002106,0.000405,0.000012,0.000003,-0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.278106,-0.063107,-0.001824,0.001251,-0.000004,0.000002,-0.000001,0.000000,0.170153,0.085869,0.002572,-0.001710,0.000000,-0.000000,0.000000,-0.000000,1.761069,-0.022295,-0.000808,0.000458,0.000008,-0.000005,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.341789,-0.063012,0.001918,0.001247,0.000001,-0.000001,0.000000,-0.000000,0.256884,0.085884,-0.002557,-0.001710,-0.000001,0.000001,-0.000001,0.000001,1.738428,-0.022521,0.000584,0.000465,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.401636,-0.055436,0.005653,0.001236,-0.000010,0.000010,-0.000010,0.000008,0.338501,0.075641,-0.007679,-0.001695,0.000014,-0.000013,0.000013,-0.000010,1.716956,-0.019960,0.001975,0.000460,-0.000004,0.000004,-0.000004,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.450186,-0.040419,0.009412,0.001365,0.000159,-0.000073,0.000169,-0.000123,0.404771,0.055192,-0.012835,-0.001872,-0.000218,0.000100,-0.000231,0.000168,1.699430,-0.014628,0.003375,0.000507,0.000058,-0.000027,0.000062,-0.000045,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.479695,-0.017075,0.013682,0.000342,-0.001979,0.000012,-0.002476,0.001628,0.445076,0.023328,-0.018688,-0.000474,0.002705,-0.000017,0.003385,-0.002225,1.688732,-0.006202,0.004961,0.000135,-0.000721,0.000005,-0.000903,0.000593,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/2/10.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.074477,0.000000,0.000000,0.000000,-0.032392,0.034971,-0.016127,0.002944,0.435880,0.000000,0.000000,-0.000000,-0.005973,0.006452,-0.002976,0.000543,1.284200,0.000000,0.000000,0.000000,-0.019973,0.021569,-0.009948,0.001816,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.085082,-0.030872,-0.024737,0.000621,0.003578,-0.002971,0.001251,-0.000222,0.433926,-0.005687,-0.004553,0.000120,0.000660,-0.000548,0.000231,-0.000041,1.277664,-0.019025,-0.015236,0.000393,0.002207,-0.001833,0.000772,-0.000137,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.138433,-0.073070,-0.017008,0.002473,-0.000287,0.000201,-0.000078,0.000014,0.424107,-0.013436,-0.003114,0.000459,-0.000054,0.000037,-0.000014,0.000003,1.244805,-0.044982,-0.010445,0.001531,-0.000179,0.000124,-0.000048,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.226189,-0.100187,-0.010192,0.002244,0.000020,-0.000011,0.000004,-0.000001,0.407988,-0.018385,-0.001853,0.000413,0.000003,-0.000002,0.000001,-0.000000,1.190813,-0.061608,-0.006237,0.001381,0.000010,-0.000006,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.334313,-0.113798,-0.003409,0.002266,-0.000001,0.000000,-0.000000,0.000000,0.388163,-0.020850,-0.000611,0.000414,-0.000001,0.000000,-0.000000,0.000000,1.124356,-0.069917,-0.002070,0.001390,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.449253,-0.113818,0.003390,0.002267,0.000001,-0.000002,0.000001,-0.000001,0.367116,-0.020831,0.000629,0.000413,0.000000,-0.000000,0.000000,-0.000000,1.053758,-0.069889,0.002098,0.001389,0.000000,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.557415,-0.100242,0.010177,0.002247,-0.000019,0.000018,-0.000017,0.000014,0.347327,-0.018335,0.001866,0.000410,-0.000003,0.000003,-0.000003,0.000003,0.987356,-0.061529,0.006257,0.001377,-0.000011,0.000011,-0.000011,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.645237,-0.073141,0.017009,0.002481,0.000289,-0.000132,0.000306,-0.000223,0.331266,-0.013373,0.003112,0.000453,0.000053,-0.000024,0.000056,-0.000041,0.933458,-0.044882,0.010443,0.001520,0.000177,-0.000081,0.000187,-0.000137,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.698650,-0.030914,0.024766,0.000628,-0.003584,0.000022,-0.004486,0.002949,0.321501,-0.005651,0.004528,0.000114,-0.000655,0.000004,-0.000820,0.000539,0.900685,-0.018966,0.015195,0.000383,-0.002199,0.000014,-0.002751,0.001809,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/2/9.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.467730,0.000000,0.000000,0.000000,0.020090,-0.021691,0.010003,-0.001826,0.771400,0.000000,0.000000,0.000000,-0.017141,0.018508,-0.008535,0.001558,1.053800,0.000000,0.000000,0.000000,0.011769,-0.012707,0.005860,-0.001070,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.461154,0.019143,0.015336,-0.000389,-0.002220,0.001843,-0.000776,0.000138,0.765789,-0.016334,-0.013085,0.000332,0.001894,-0.001572,0.000662,-0.000118,1.057653,0.011215,0.008984,-0.000228,-0.001300,0.001080,-0.000455,0.000081,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.428078,0.045294,0.010534,-0.001536,0.000179,-0.000125,0.000049,-0.000008,0.737568,-0.038646,-0.008988,0.001310,-0.000152,0.000106,-0.000041,0.000007,1.077029,0.026535,0.006172,-0.000900,0.000105,-0.000073,0.000028,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.373691,0.062080,0.006306,-0.001391,-0.000012,0.000007,-0.000002,0.000000,0.691163,-0.052968,-0.005380,0.001187,0.000010,-0.000006,0.000002,-0.000000,1.108892,0.036370,0.003694,-0.000815,-0.000007,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.306702,0.070495,0.002104,-0.001403,0.000001,-0.000000,0.000000,-0.000000,0.634008,-0.060147,-0.001795,0.001197,-0.000001,0.000000,-0.000000,0.000000,1.148137,0.041300,0.001233,-0.000822,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.235505,0.070496,-0.002104,-0.001403,-0.000001,0.000001,-0.000001,0.000000,0.573263,-0.060146,0.001796,0.001197,0.000000,-0.000001,0.000000,-0.000000,1.189849,0.041302,-0.001232,-0.000822,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.168517,0.062081,-0.006306,-0.001391,0.000012,-0.000011,0.000011,-0.000008,0.516109,-0.052965,0.005380,0.001186,-0.000010,0.000009,-0.000009,0.000007,1.229098,0.036374,-0.003693,-0.000815,0.000007,-0.000006,0.000006,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.114129,0.045294,-0.010535,-0.001536,-0.000179,0.000082,-0.000189,0.000138,0.469709,-0.038642,0.008988,0.001310,0.000152,-0.000070,0.000161,-0.000118,1.260965,0.026540,-0.006172,-0.000900,-0.000105,0.000048,-0.000111,0.000081,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.081053,0.019143,-0.015336,-0.000388,0.002220,-0.000014,0.002778,-0.001826,0.441490,-0.016332,0.013084,0.000331,-0.001894,0.000012,-0.002370,0.001558,1.280346,0.011218,-0.008987,-0.000228,0.001301,-0.000008,0.001628,-0.001070,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/4/10.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.231270,0.000000,0.000000,0.000000,-0.045714,0.049375,-0.022774,0.004157,0.480390,0.000000,0.000000,-0.000000,0.003558,-0.003855,0.001781,-0.000325,1.974700,0.000000,0.000000,0.000000,-0.025473,0.027487,-0.012673,0.002313,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.246226,-0.043525,-0.034844,0.000914,0.005051,-0.004197,0.001768,-0.000314,0.481548,0.003364,0.002676,-0.000092,-0.000394,0.000329,-0.000139,0.000025,1.966354,-0.024306,-0.019496,0.000464,0.002813,-0.002333,0.000983,-0.000175,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.321372,-0.102840,-0.023840,0.003513,-0.000411,0.000284,-0.000111,0.000019,0.487316,0.007847,0.001762,-0.000289,0.000035,-0.000022,0.000009,-0.000001,1.924304,-0.057644,-0.013481,0.001930,-0.000222,0.000158,-0.000062,0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.444758,-0.140738,-0.014187,0.003166,0.000021,-0.000016,0.000006,-0.000001,0.496656,0.010574,0.000982,-0.000249,0.000003,0.000002,-0.000001,0.000000,1.854993,-0.079209,-0.008142,0.001765,0.000020,-0.000008,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.596508,-0.159582,-0.004655,0.003175,-0.000005,0.000001,-0.000000,-0.000000,0.507966,0.011804,0.000257,-0.000234,0.000004,-0.000000,-0.000000,0.000000,1.769421,-0.090148,-0.002781,0.001799,0.000002,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.757574,-0.159383,0.004847,0.003163,-0.000001,-0.000002,0.000001,-0.000001,0.519797,0.011630,-0.000426,-0.000223,0.000002,0.000000,-0.000000,0.000000,1.678293,-0.090305,0.002628,0.001807,0.000002,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.908951,-0.140216,0.014305,0.003127,-0.000027,0.000025,-0.000024,0.000019,0.530780,0.010118,-0.001082,-0.000214,0.000003,-0.000002,0.000002,-0.000001,1.592423,-0.079628,0.008044,0.001794,-0.000014,0.000014,-0.000014,0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-1.031742,-0.102221,0.023810,0.003451,0.000403,-0.000185,0.000427,-0.000311,0.539603,0.007313,-0.001731,-0.000236,-0.000028,0.000013,-0.000030,0.000022,1.522629,-0.058153,0.013501,0.001981,0.000230,-0.000106,0.000243,-0.000177,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-1.106368,-0.043178,0.034601,0.000864,-0.005004,0.000030,-0.006261,0.004116,0.544927,0.003070,-0.002468,-0.000052,0.000354,-0.000002,0.000442,-0.000291,1.480150,-0.024595,0.019697,0.000507,-0.002853,0.000018,-0.003572,0.002347,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/5/10.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.467080,0.000000,0.000000,0.000000,-0.059113,0.063827,-0.029436,0.005373,0.178880,0.000000,0.000000,0.000000,-0.007261,0.007858,-0.003628,0.000663,2.174900,0.000000,0.000000,-0.000000,-0.016719,0.018043,-0.008319,0.001518,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.447731,-0.056323,-0.045118,0.001147,0.006531,-0.005423,0.002284,-0.000406,0.176512,-0.006883,-0.005489,0.000171,0.000803,-0.000669,0.000282,-0.000050,2.169423,-0.015949,-0.012790,0.000308,0.001847,-0.001532,0.000645,-0.000115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.350423,-0.133245,-0.030981,0.004520,-0.000527,0.000367,-0.000143,0.000025,0.164676,-0.016140,-0.003673,0.000575,-0.000069,0.000045,-0.000017,0.000003,2.141837,-0.037809,-0.008832,0.001270,-0.000146,0.000104,-0.000041,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.190439,-0.182606,-0.018539,0.004091,0.000033,-0.000020,0.000007,-0.000001,0.145399,-0.021899,-0.002115,0.000503,-0.000000,-0.000002,0.000001,-0.000000,2.096390,-0.051924,-0.005322,0.001159,0.000012,-0.000006,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.006596,-0.207343,-0.006183,0.004126,-0.000002,0.000001,-0.000000,0.000000,0.121887,-0.024628,-0.000621,0.000491,-0.000004,0.000000,-0.000000,-0.000000,2.040311,-0.059063,-0.001809,0.001176,0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.215996,-0.207333,0.006193,0.004126,0.000002,-0.000003,0.000002,-0.000001,0.097124,-0.024413,0.000830,0.000477,-0.000003,-0.000000,0.000000,-0.000000,1.980616,-0.059149,0.001726,0.001181,0.000002,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.413011,-0.182577,0.018547,0.004089,-0.000034,0.000032,-0.000032,0.000025,0.074015,-0.021334,0.002243,0.000463,-0.000006,0.000004,-0.000004,0.000003,1.924375,-0.052153,0.005267,0.001174,-0.000009,0.000009,-0.000009,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.572959,-0.133204,0.030982,0.004515,0.000526,-0.000241,0.000557,-0.000406,0.055384,-0.015464,0.003642,0.000506,0.000059,-0.000027,0.000064,-0.000047,1.878662,-0.038090,0.008841,0.001298,0.000151,-0.000069,0.000160,-0.000116,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.670230,-0.056297,0.045101,0.001142,-0.006527,0.000040,-0.008168,0.005369,0.044118,-0.006503,0.005223,0.000116,-0.000751,0.000004,-0.000938,0.000617,1.850836,-0.016112,0.012902,0.000334,-0.001869,0.000012,-0.002340,0.001538,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/6/3.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.973840,0.000000,0.000000,0.000000,-0.013673,0.014761,-0.006807,0.001243,-0.169930,0.000000,0.000000,0.000000,0.004443,-0.004797,0.002212,-0.000404,1.761000,0.000000,0.000000,0.000000,-0.013244,0.014299,-0.006594,0.001204,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.969364,-0.013030,-0.010440,0.000263,0.001511,-0.001254,0.000528,-0.000094,-0.168475,0.004234,0.003393,-0.000085,-0.000491,0.000407,-0.000172,0.000031,1.756665,-0.012620,-0.010111,0.000256,0.001463,-0.001215,0.000512,-0.000091,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.946847,-0.030838,-0.007177,0.001044,-0.000121,0.000085,-0.000033,0.000006,-0.161158,0.010022,0.002333,-0.000339,0.000039,-0.000028,0.000011,-0.000002,1.734859,-0.029861,-0.006946,0.001012,-0.000118,0.000082,-0.000032,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.909813,-0.042278,-0.004299,0.000947,0.000008,-0.000005,0.000002,-0.000000,-0.149122,0.013741,0.001398,-0.000307,-0.000003,0.000001,-0.000001,0.000000,1.699001,-0.040931,-0.004159,0.000917,0.000008,-0.000004,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.864186,-0.048020,-0.001439,0.000956,-0.000000,0.000000,-0.000000,0.000000,-0.134292,0.015610,0.000469,-0.000310,-0.000000,-0.000000,0.000000,0.000000,1.654833,-0.046482,-0.001389,0.000925,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.815683,-0.048030,0.001429,0.000957,0.000001,-0.000001,0.000000,-0.000000,-0.118524,0.015617,-0.000463,-0.000311,-0.000000,0.000000,-0.000000,0.000000,1.607887,-0.046485,0.001386,0.000925,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.770038,-0.042304,0.004294,0.000948,-0.000008,0.000007,-0.000007,0.000006,-0.103681,0.013758,-0.001395,-0.000309,0.000003,-0.000002,0.000002,-0.000002,1.563714,-0.040938,0.004157,0.000917,-0.000008,0.000007,-0.000007,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.732975,-0.030868,0.007178,0.001047,0.000122,-0.000056,0.000129,-0.000094,-0.091627,0.010040,-0.002334,-0.000341,-0.000040,0.000018,-0.000042,0.000031,1.527849,-0.029869,0.006947,0.001013,0.000118,-0.000054,0.000125,-0.000091,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.710432,-0.013048,0.010452,0.000265,-0.001513,0.000009,-0.001893,0.001245,-0.084294,0.004244,-0.003400,-0.000087,0.000492,-0.000003,0.000616,-0.000405,1.506037,-0.012624,0.010113,0.000256,-0.001464,0.000009,-0.001832,0.001204,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/6/8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.235860,0.000000,0.000000,0.000000,-0.014315,0.015456,-0.007128,0.001301,-0.614500,0.000000,0.000000,0.000000,0.031736,-0.034266,0.015802,-0.002884,1.486300,0.000000,0.000000,-0.000000,-0.015541,0.016780,-0.007738,0.001412,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.231174,-0.013640,-0.010928,0.000277,0.001582,-0.001313,0.000553,-0.000098,-0.604111,0.030241,0.024226,-0.000614,-0.003506,0.002911,-0.001226,0.000218,1.481213,-0.014808,-0.011863,0.000301,0.001717,-0.001426,0.000600,-0.000107,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.207606,-0.032274,-0.007507,0.001094,-0.000127,0.000089,-0.000035,0.000006,-0.551862,0.071551,0.016641,-0.002426,0.000282,-0.000197,0.000077,-0.000013,1.455627,-0.035037,-0.008149,0.001188,-0.000138,0.000096,-0.000038,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.168852,-0.044237,-0.004493,0.000991,0.000008,-0.000005,0.000002,-0.000000,-0.465946,0.098069,0.009961,-0.002197,-0.000018,0.000011,-0.000004,0.000001,1.413555,-0.048022,-0.004878,0.001076,0.000009,-0.000005,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.121117,-0.050234,-0.001500,0.001000,-0.000000,0.000000,-0.000000,0.000000,-0.360124,0.111362,0.003324,-0.002216,0.000001,-0.000001,0.000000,-0.000000,1.361736,-0.054532,-0.001628,0.001085,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.070384,-0.050234,0.001499,0.001000,0.000000,-0.000001,0.000000,-0.000000,-0.247653,0.111363,-0.003323,-0.002217,-0.000001,0.000002,-0.000001,0.000001,1.306662,-0.054531,0.001628,0.001085,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.022648,-0.044239,0.004493,0.000991,-0.000008,0.000008,-0.000008,0.000006,-0.141829,0.098071,-0.009961,-0.002197,0.000018,-0.000017,0.000017,-0.000013,1.254844,-0.048022,0.004878,0.001076,-0.000009,0.000008,-0.000008,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.016109,-0.032277,0.007507,0.001094,0.000127,-0.000058,0.000135,-0.000098,-0.055911,0.071553,-0.016642,-0.002426,-0.000282,0.000130,-0.000299,0.000218,1.212773,-0.035037,0.008149,0.001188,0.000138,-0.000063,0.000146,-0.000107,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.039680,-0.013642,0.010929,0.000277,-0.001582,0.000010,-0.001979,0.001301,-0.003659,0.030242,-0.024227,-0.000614,0.003506,-0.000022,0.004388,-0.002884,1.187187,-0.014808,0.011863,0.000301,-0.001717,0.000011,-0.002149,0.001412,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/6/9.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.044366,0.000000,0.000000,-0.000000,0.022992,-0.024826,0.011450,-0.002090,0.006730,0.000000,0.000000,0.000000,-0.017883,0.019313,-0.008908,0.001626,1.182100,0.000000,0.000000,-0.000000,0.009131,-0.009858,0.004546,-0.000830,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.036840,0.021906,0.017548,-0.000447,-0.002540,0.002109,-0.000888,0.000158,0.000878,-0.017031,-0.013637,0.000354,0.001976,-0.001641,0.000691,-0.000123,1.185089,0.008701,0.006971,-0.000176,-0.001009,0.000837,-0.000353,0.000063,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.001005,0.051820,0.012046,-0.001759,0.000205,-0.000143,0.000056,-0.000010,-0.028533,-0.040257,-0.009340,0.001374,-0.000160,0.000111,-0.000043,0.000008,1.200125,0.020592,0.004791,-0.000697,0.000081,-0.000057,0.000022,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.063220,0.071007,0.007203,-0.001591,-0.000013,0.000008,-0.000003,0.000001,-0.076842,-0.055110,-0.005562,0.001241,0.000009,-0.000006,0.000002,-0.000000,1.224853,0.028230,0.002871,-0.000632,-0.000005,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.139831,0.080614,0.002399,-0.001604,0.000001,-0.000000,0.000000,-0.000000,-0.136269,-0.062495,-0.001822,0.001246,-0.000002,0.000000,-0.000000,0.000000,1.255319,0.032065,0.000962,-0.000638,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.221241,0.080604,-0.002409,-0.001603,-0.000001,0.000001,-0.000001,0.000000,-0.199342,-0.062408,0.001906,0.001240,-0.000001,-0.000001,0.000001,-0.000000,1.287707,0.032074,-0.000953,-0.000638,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.297833,0.070979,-0.007210,-0.001589,0.000013,-0.000012,0.000012,-0.000010,-0.258605,-0.054884,0.005611,0.001223,-0.000011,0.000010,-0.000009,0.000007,1.318190,0.028254,-0.002865,-0.000634,0.000005,-0.000005,0.000005,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.360015,0.051785,-0.012044,-0.001756,-0.000204,0.000094,-0.000216,0.000158,-0.306658,-0.039993,0.009325,0.001347,0.000157,-0.000072,0.000167,-0.000122,1.342946,0.020620,-0.004793,-0.000700,-0.000081,0.000037,-0.000086,0.000063,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.397831,0.021887,-0.017534,-0.000444,0.002538,-0.000016,0.003176,-0.002088,-0.335850,-0.016886,0.013535,0.000334,-0.001956,0.000012,-0.002447,0.001609,1.358005,0.008717,-0.006983,-0.000178,0.001011,-0.000006,0.001265,-0.000832,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/7/3.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.748540,0.000000,0.000000,0.000000,-0.001490,0.001608,-0.000742,0.000135,-0.755560,0.000000,0.000000,-0.000000,0.000147,-0.000163,0.000076,-0.000014,2.391500,0.000000,0.000000,0.000000,-0.015270,0.016487,-0.007603,0.001388,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.748052,-0.001420,-0.001137,0.000029,0.000165,-0.000137,0.000058,-0.000010,-0.755514,0.000131,0.000098,-0.000011,-0.000017,0.000014,-0.000006,0.000001,2.386502,-0.014550,-0.011657,0.000295,0.001687,-0.001401,0.000590,-0.000105,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.745599,-0.003359,-0.000781,0.000114,-0.000013,0.000009,-0.000004,0.000001,-0.755303,0.000271,0.000041,-0.000018,0.000002,-0.000001,0.000000,-0.000000,2.361361,-0.034427,-0.008007,0.001167,-0.000136,0.000095,-0.000037,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.741566,-0.004605,-0.000468,0.000103,0.000001,-0.000000,0.000000,-0.000000,-0.755007,0.000305,-0.000004,-0.000013,0.000001,0.000000,-0.000000,0.000000,2.320023,-0.047186,-0.004793,0.001057,0.000009,-0.000005,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.736597,-0.005229,-0.000156,0.000104,-0.000000,0.000000,-0.000000,0.000000,-0.754717,0.000263,-0.000035,-0.000008,0.000001,0.000000,0.000000,-0.000000,2.269107,-0.053582,-0.001599,0.001066,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.731316,-0.005229,0.000156,0.000104,0.000000,-0.000000,0.000000,-0.000000,-0.754496,0.000175,-0.000050,-0.000002,0.000002,0.000000,-0.000000,-0.000000,2.214993,-0.053581,0.001599,0.001066,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.726346,-0.004605,0.000468,0.000103,-0.000001,0.000001,-0.000001,0.000001,-0.754371,0.000075,-0.000047,0.000004,0.000002,-0.000000,-0.000000,0.000000,2.164077,-0.047186,0.004793,0.001057,-0.000009,0.000008,-0.000008,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.722312,-0.003360,0.000781,0.000114,0.000013,-0.000006,0.000014,-0.000010,-0.754337,-0.000000,-0.000026,0.000010,0.000001,-0.000000,0.000000,-0.000000,2.122739,-0.034427,0.008007,0.001167,0.000136,-0.000062,0.000144,-0.000105,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.719858,-0.001420,0.001138,0.000029,-0.000165,0.000001,-0.000206,0.000135,-0.754352,-0.000019,0.000008,0.000010,-0.000004,0.000000,-0.000006,0.000004,2.097599,-0.014550,0.011657,0.000295,-0.001687,0.000010,-0.002111,0.001388,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/3.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.098826,0.000000,0.000000,0.000000,0.006737,-0.007274,0.003355,-0.000612,-0.710310,0.000000,0.000000,0.000000,0.014754,-0.015930,0.007347,-0.001341,2.131000,0.000000,0.000000,0.000000,-0.025908,0.027973,-0.012901,0.002355,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.096621,0.006419,0.005142,-0.000131,-0.000744,0.000618,-0.000260,0.000046,-0.705480,0.014059,0.011264,-0.000285,-0.001630,0.001353,-0.000570,0.000101,2.122519,-0.024688,-0.019779,0.000500,0.002862,-0.002376,0.001001,-0.000178,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.085532,0.015184,0.003530,-0.000516,0.000060,-0.000042,0.000016,-0.000003,-0.681188,0.033267,0.007738,-0.001127,0.000131,-0.000091,0.000036,-0.000006,2.079861,-0.058417,-0.013589,0.001980,-0.000230,0.000161,-0.000063,0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.067302,0.020806,0.002110,-0.000467,-0.000004,0.000002,-0.000001,0.000000,-0.641241,0.045599,0.004633,-0.001021,-0.000008,0.000005,-0.000002,0.000000,2.009713,-0.080074,-0.008137,0.001793,0.000015,-0.000009,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.044855,0.023617,0.000700,-0.000470,0.000001,-0.000000,0.000000,-0.000000,-0.592035,0.051784,0.001548,-0.001031,0.000000,-0.000000,0.000000,-0.000000,1.923304,-0.090938,-0.002719,0.001810,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.021008,0.023608,-0.000708,-0.000469,-0.000000,0.000000,-0.000000,0.000000,-0.539734,0.051788,-0.001544,-0.001031,-0.000000,0.000001,-0.000000,0.000000,1.831457,-0.090946,0.002711,0.001811,0.000001,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,0.001423,0.020785,-0.002113,-0.000465,0.000004,-0.000004,0.000004,-0.000003,-0.490521,0.045609,-0.004631,-0.001022,0.000008,-0.000008,0.000008,-0.000006,1.745032,-0.080095,0.008133,0.001795,-0.000015,0.000014,-0.000014,0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,0.019631,0.015163,-0.003528,-0.000514,-0.000060,0.000027,-0.000063,0.000046,-0.450563,0.033278,-0.007739,-0.001128,-0.000131,0.000060,-0.000139,0.000101,1.674862,-0.058440,0.013591,0.001982,0.000231,-0.000106,0.000244,-0.000178,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,0.030703,0.006408,-0.005134,-0.000130,0.000743,-0.000005,0.000930,-0.000611,-0.426262,0.014065,-0.011268,-0.000286,0.001631,-0.000010,0.002041,-0.001342,1.632185,-0.024700,0.019788,0.000502,-0.002864,0.000018,-0.003584,0.002356,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/9.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.485560,0.000000,0.000000,-0.000000,0.008760,-0.009458,0.004362,-0.000796,0.453090,0.000000,0.000000,-0.000000,-0.034750,0.037520,-0.017304,0.003158,1.686600,0.000000,0.000000,0.000000,-0.017359,0.018743,-0.008644,0.001578,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.482693,0.008347,0.006687,-0.000169,-0.000968,0.000804,-0.000338,0.000060,0.441715,-0.033113,-0.026528,0.000672,0.003839,-0.003188,0.001343,-0.000239,1.680918,-0.016541,-0.013252,0.000336,0.001918,-0.001592,0.000671,-0.000119,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.468271,0.019749,0.004593,-0.000670,0.000078,-0.000054,0.000021,-0.000004,0.384502,-0.078347,-0.018222,0.002656,-0.000309,0.000216,-0.000084,0.000015,1.652337,-0.039137,-0.009103,0.001327,-0.000154,0.000108,-0.000042,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.444556,0.027069,0.002749,-0.000606,-0.000005,0.000003,-0.000001,0.000000,0.290427,-0.107383,-0.010907,0.002406,0.000020,-0.000012,0.000004,-0.000001,1.605343,-0.053642,-0.005448,0.001202,0.000010,-0.000006,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.415347,0.030738,0.000917,-0.000612,0.000000,-0.000000,0.000000,-0.000000,0.174555,-0.121938,-0.003639,0.002427,-0.000001,0.000001,-0.000000,0.000000,1.547460,-0.060913,-0.001818,0.001212,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.384303,0.030738,-0.000917,-0.000612,-0.000000,0.000000,-0.000000,0.000000,0.051404,-0.121938,0.003640,0.002427,0.000001,-0.000002,0.000001,-0.000001,1.485940,-0.060913,0.001818,0.001212,0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.355094,0.027069,-0.002749,-0.000606,0.000005,-0.000005,0.000005,-0.000004,-0.064468,-0.107382,0.010907,0.002406,-0.000020,0.000019,-0.000019,0.000015,1.428057,-0.053642,0.005448,0.001202,-0.000010,0.000009,-0.000009,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.331380,0.019750,-0.004593,-0.000670,-0.000078,0.000036,-0.000083,0.000060,-0.158543,-0.078346,0.018222,0.002656,0.000309,-0.000142,0.000327,-0.000239,1.381063,-0.039138,0.009103,0.001327,0.000154,-0.000071,0.000164,-0.000119,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.316958,0.008347,-0.006687,-0.000169,0.000968,-0.000006,0.001211,-0.000796,-0.215755,-0.033113,0.026527,0.000672,-0.003839,0.000024,-0.004805,0.003158,1.352482,-0.016541,0.013252,0.000336,-0.001918,0.000012,-0.002400,0.001578,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/2/3.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.043176,0.000000,0.000000,0.000000,-0.021155,0.022842,-0.010534,0.001923,-0.777380,0.000000,0.000000,0.000000,0.049209,-0.053132,0.024503,-0.004472,1.248000,0.000000,0.000000,0.000000,0.002910,-0.003141,0.001448,-0.000264,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.050100,-0.020156,-0.016146,0.000411,0.002337,-0.001941,0.000817,-0.000145,-0.761272,0.046890,0.037564,-0.000952,-0.005437,0.004514,-0.001901,0.000338,1.248953,0.002776,0.002226,-0.000054,-0.000321,0.000267,-0.000112,0.000020,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.084922,-0.047681,-0.011085,0.001619,-0.000188,0.000131,-0.000051,0.000009,-0.680257,0.110940,0.025800,-0.003762,0.000438,-0.000305,0.000119,-0.000021,1.253755,0.006582,0.001538,-0.000220,0.000026,-0.000018,0.000007,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.142169,-0.065337,-0.006628,0.001465,0.000011,-0.000007,0.000003,-0.000000,-0.547047,0.152049,0.015440,-0.003407,-0.000028,0.000016,-0.000006,0.000001,1.261668,0.009043,0.000930,-0.000202,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.212662,-0.074175,-0.002206,0.001476,-0.000001,0.000000,-0.000000,0.000000,-0.382981,0.172651,0.005149,-0.003436,0.000002,-0.000001,0.000000,-0.000000,1.271438,0.010291,0.000316,-0.000206,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.287568,-0.074162,0.002219,0.001475,0.000001,-0.000001,0.000001,-0.000000,-0.208617,0.172644,-0.005156,-0.003436,-0.000001,0.000003,-0.000001,0.000001,1.281838,0.010305,-0.000302,-0.000206,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.358037,-0.065302,0.006636,0.001462,-0.000012,0.000011,-0.000011,0.000009,-0.044563,0.152032,-0.015443,-0.003405,0.000028,-0.000027,0.000026,-0.000021,1.291635,0.009082,-0.000919,-0.000204,0.000002,-0.000002,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.415244,-0.047641,0.011082,0.001615,0.000188,-0.000086,0.000199,-0.000145,0.088626,0.110920,-0.025799,-0.003760,-0.000438,0.000201,-0.000464,0.000338,1.299594,0.006630,-0.001540,-0.000226,-0.000026,0.000012,-0.000028,0.000020,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.450033,-0.020134,0.016130,0.000408,-0.002334,0.000014,-0.002921,0.001920,0.169626,0.046880,-0.037556,-0.000951,0.005435,-0.000034,0.006802,-0.004471,1.304437,0.002804,-0.002246,-0.000057,0.000325,-0.000002,0.000407,-0.000268,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/2/8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.326590,0.000000,0.000000,-0.000000,-0.012302,0.011952,-0.005241,0.000925,0.079709,0.000000,0.000000,0.000000,0.092652,-0.102569,0.047819,-0.008789,1.111500,0.000000,0.000000,-0.000000,-0.008252,0.009144,-0.004265,0.000784,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.321924,-0.014421,-0.013486,-0.002139,0.001211,-0.000925,0.000370,-0.000064,0.108822,0.083154,0.062939,-0.006314,-0.010519,0.008889,-0.003785,0.000677,1.108911,-0.007390,-0.005581,0.000577,0.000938,-0.000793,0.000338,-0.000060,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.292469,-0.045819,-0.017679,-0.001382,-0.000102,0.000093,-0.000091,0.000028,0.243862,0.174487,0.027215,-0.011505,0.000845,-0.000563,0.000142,-0.000008,1.096939,-0.015431,-0.002356,0.001039,-0.000075,0.000050,-0.000013,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.227517,-0.085619,-0.022292,-0.001711,-0.000033,-0.000162,0.000478,-0.000099,0.434475,0.195766,-0.005894,-0.011194,-0.000124,-0.000184,0.000664,-0.000133,1.080154,-0.017148,0.000634,0.001009,0.000009,0.000015,-0.000058,0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.118077,-0.134108,-0.024161,0.002622,0.002855,-0.000617,-0.000271,0.000110,0.613377,0.152040,-0.034875,-0.004879,0.004283,-0.001022,-0.000135,0.000109,1.064626,-0.013010,0.003236,0.000439,-0.000379,0.000092,0.000008,-0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.035493,-0.167089,-0.007101,0.006287,-0.000458,-0.000032,0.000132,-0.000040,0.728899,0.079632,-0.033759,0.003165,0.000980,-0.000420,0.000078,-0.000003,1.055004,-0.006288,0.003138,-0.000302,-0.000106,0.000041,-0.000006,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.203794,-0.163913,0.009826,0.005368,-0.000044,0.000078,-0.000066,0.000034,0.778572,0.023877,-0.021472,0.004349,-0.000046,0.000006,-0.000003,0.000006,1.051481,-0.001172,0.001919,-0.000436,0.000004,0.000000,-0.000000,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.352510,-0.128097,0.026179,0.005855,0.000557,-0.000270,0.000606,-0.000432,0.785289,-0.006146,-0.008552,0.004383,0.000154,-0.000092,0.000207,-0.000130,1.051796,0.001371,0.000624,-0.000441,-0.000018,0.000010,-0.000023,0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.448111,-0.056679,0.044415,0.002398,-0.006810,0.000086,-0.008667,0.005638,0.775112,-0.009618,0.004962,0.003647,-0.001772,0.000130,-0.002619,0.001558,1.053335,0.001243,-0.000736,-0.000351,0.000206,-0.000013,0.000296,-0.000179,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/6/10.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,0.405350,0.000000,0.000000,0.000000,-0.031862,0.034402,-0.015865,0.002896,-0.341650,0.000000,0.000000,0.000000,0.005951,-0.006425,0.002963,-0.000541,1.361000,0.000000,0.000000,0.000000,-0.005814,0.006278,-0.002895,0.000528,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,0.394920,-0.030361,-0.024323,0.000616,0.003520,-0.002923,0.001231,-0.000219,-0.339702,0.005671,0.004543,-0.000115,-0.000657,0.000546,-0.000230,0.000041,1.359097,-0.005540,-0.004438,0.000113,0.000642,-0.000533,0.000225,-0.000040,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,0.342462,-0.071836,-0.016708,0.002435,-0.000283,0.000198,-0.000077,0.000013,-0.329903,0.013419,0.003122,-0.000455,0.000053,-0.000037,0.000014,-0.000003,1.349524,-0.013109,-0.003049,0.000444,-0.000052,0.000036,-0.000014,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,0.256203,-0.098461,-0.010001,0.002206,0.000018,-0.000011,0.000004,-0.000001,-0.313789,0.018395,0.001870,-0.000412,-0.000003,0.000002,-0.000001,0.000000,1.333784,-0.017966,-0.001824,0.000403,0.000003,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,0.149958,-0.111808,-0.003338,0.002225,-0.000001,0.000001,-0.000000,0.000000,-0.293938,0.020892,0.000626,-0.000416,0.000000,-0.000000,0.000000,-0.000000,1.314398,-0.020401,-0.000608,0.000406,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,0.037036,-0.111810,0.003336,0.002225,0.000001,-0.000002,0.000001,-0.000001,-0.272836,0.020897,-0.000621,-0.000416,-0.000000,0.000000,-0.000000,0.000000,1.293795,-0.020400,0.000609,0.000406,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.069213,-0.098465,0.010000,0.002206,-0.000018,0.000017,-0.000017,0.000013,-0.252977,0.018406,-0.001868,-0.000413,0.000003,-0.000003,0.000003,-0.000003,1.274410,-0.017964,0.001825,0.000402,-0.000003,0.000003,-0.000003,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.155476,-0.071842,0.016708,0.002436,0.000283,-0.000130,0.000300,-0.000219,-0.236850,0.013432,-0.003123,-0.000456,-0.000053,0.000024,-0.000056,0.000041,1.258673,-0.013106,0.003048,0.000444,0.000052,-0.000024,0.000055,-0.000040,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.207939,-0.030364,0.024325,0.000616,-0.003521,0.000022,-0.004406,0.002896,-0.227041,0.005678,-0.004548,-0.000116,0.000658,-0.000004,0.000824,-0.000542,1.249103,-0.005539,0.004437,0.000112,-0.000642,0.000004,-0.000804,0.000528,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/7/10.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.062437,0.000000,0.000000,0.000000,-0.025299,0.027317,-0.012598,0.002299,-1.072600,0.000000,0.000000,0.000000,0.003419,-0.003695,0.001705,-0.000311,1.158000,0.000000,0.000000,0.000000,-0.009136,0.009864,-0.004549,0.000830,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.070718,-0.024106,-0.019311,0.000490,0.002795,-0.002321,0.000978,-0.000174,-1.071482,0.003253,0.002602,-0.000070,-0.000377,0.000315,-0.000133,0.000024,1.155009,-0.008706,-0.006975,0.000176,0.001009,-0.000838,0.000353,-0.000063,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.112366,-0.057030,-0.013261,0.001934,-0.000225,0.000157,-0.000061,0.000011,-1.065869,0.007682,0.001783,-0.000259,0.000032,-0.000022,0.000008,-0.000001,1.139967,-0.020600,-0.004792,0.000698,-0.000081,0.000057,-0.000022,0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.180842,-0.078159,-0.007935,0.001751,0.000014,-0.000008,0.000003,-0.000001,-1.056646,0.010529,0.001074,-0.000233,-0.000002,0.000001,-0.000000,0.000000,1.115230,-0.028237,-0.002869,0.000632,0.000005,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.265177,-0.088747,-0.002646,0.001766,-0.000001,0.000000,-0.000000,0.000000,-1.045278,0.011971,0.000367,-0.000237,-0.000000,-0.000000,-0.000000,-0.000000,1.084760,-0.032067,-0.000958,0.000638,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.354804,-0.088742,0.002651,0.001766,0.000001,-0.000001,0.000001,-0.000001,-1.033177,0.011994,-0.000346,-0.000239,-0.000001,0.000000,-0.000000,0.000000,1.052372,-0.032070,0.000955,0.000638,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.439131,-0.078147,0.007938,0.001750,-0.000014,0.000014,-0.000014,0.000011,-1.021768,0.010585,-0.001063,-0.000239,0.000002,-0.000002,0.000002,-0.000001,1.021896,-0.028246,0.002866,0.000633,-0.000005,0.000005,-0.000005,0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.507594,-0.057017,0.013260,0.001933,0.000225,-0.000103,0.000238,-0.000174,-1.012485,0.007741,-0.001791,-0.000265,-0.000031,0.000014,-0.000033,0.000024,0.997148,-0.020613,0.004791,0.000700,0.000082,-0.000037,0.000086,-0.000063,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.549231,-0.024099,0.019306,0.000490,-0.002794,0.000017,-0.003497,0.002299,-1.006827,0.003279,-0.002624,-0.000071,0.000381,-0.000002,0.000477,-0.000314,0.982094,-0.008714,0.006980,0.000178,-0.001011,0.000006,-0.001265,0.000831,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/7/8.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.088519,0.000000,0.000000,0.000000,-0.025303,0.027320,-0.012599,0.002300,-0.793990,0.000000,0.000000,0.000000,0.018926,-0.020435,0.009424,-0.001720,2.278900,0.000000,0.000000,0.000000,-0.027153,0.029317,-0.013520,0.002468,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.096802,-0.024111,-0.019315,0.000489,0.002795,-0.002321,0.000978,-0.000174,-0.787795,0.018032,0.014445,-0.000367,-0.002091,0.001736,-0.000731,0.000130,2.270012,-0.025873,-0.020728,0.000525,0.003000,-0.002491,0.001049,-0.000186,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.138460,-0.057047,-0.013268,0.001934,-0.000225,0.000157,-0.000061,0.000011,-0.756641,0.042660,0.009919,-0.001447,0.000169,-0.000117,0.000046,-0.000008,2.225308,-0.061217,-0.014238,0.002075,-0.000242,0.000168,-0.000066,0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.206959,-0.078189,-0.007942,0.001752,0.000014,-0.000008,0.000003,-0.000001,-0.705420,0.058465,0.005936,-0.001310,-0.000011,0.000006,-0.000002,0.000000,2.151801,-0.083905,-0.008522,0.001880,0.000016,-0.000009,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.291329,-0.088787,-0.002650,0.001767,-0.000001,0.000000,-0.000000,0.000000,-0.642335,0.066386,0.001980,-0.001321,0.000001,-0.000000,0.000000,-0.000000,2.061263,-0.095278,-0.002844,0.001896,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.380999,-0.088787,0.002650,0.001767,0.000001,-0.000001,0.000001,-0.000001,-0.575290,0.066383,-0.001982,-0.001321,-0.000001,0.000001,-0.000000,0.000000,1.965037,-0.095278,0.002844,0.001896,0.000001,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.465370,-0.078189,0.007942,0.001752,-0.000014,0.000014,-0.000014,0.000011,-0.512210,0.058458,-0.005938,-0.001309,0.000011,-0.000010,0.000010,-0.000008,1.874499,-0.083905,0.008522,0.001880,-0.000016,0.000015,-0.000015,0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.533869,-0.057047,0.013268,0.001934,0.000225,-0.000103,0.000238,-0.000174,-0.460997,0.042650,-0.009920,-0.001446,-0.000168,0.000077,-0.000178,0.000130,1.800992,-0.061217,0.014238,0.002075,0.000242,-0.000111,0.000256,-0.000186,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.575527,-0.024111,0.019316,0.000489,-0.002795,0.000017,-0.003498,0.002300,-0.429852,0.018026,-0.014441,-0.000366,0.002090,-0.000013,0.002615,-0.001719,1.756288,-0.025873,0.020728,0.000525,-0.003000,0.000019,-0.003754,0.002468,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/7/9.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.583810,0.000000,0.000000,0.000000,0.026636,-0.028759,0.013263,-0.002421,-0.423660,0.000000,0.000000,0.000000,-0.033152,0.035795,-0.016508,0.003013,1.747400,0.000000,0.000000,-0.000000,-0.030111,0.032511,-0.014993,0.002737,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.575091,0.025380,0.020333,-0.000515,-0.002943,0.002443,-0.001029,0.000183,-0.434512,-0.031590,-0.025307,0.000641,0.003663,-0.003041,0.001281,-0.000228,1.737544,-0.028692,-0.022986,0.000582,0.003327,-0.002762,0.001163,-0.000207,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.531239,0.060051,0.013966,-0.002036,0.000237,-0.000165,0.000064,-0.000011,-0.489093,-0.074743,-0.017384,0.002534,-0.000295,0.000206,-0.000080,0.000014,1.687970,-0.067886,-0.015789,0.002302,-0.000268,0.000187,-0.000073,0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.459132,0.082306,0.008360,-0.001844,-0.000015,0.000009,-0.000003,0.000001,-0.578842,-0.102444,-0.010405,0.002295,0.000019,-0.000011,0.000004,-0.000001,1.606455,-0.093045,-0.009451,0.002084,0.000017,-0.000010,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.370319,0.093463,0.002790,-0.001860,0.000001,-0.000000,0.000000,-0.000000,-0.689385,-0.116330,-0.003472,0.002315,-0.000001,0.000001,-0.000000,0.000000,1.506054,-0.105657,-0.003154,0.002103,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.275927,0.093462,-0.002790,-0.001860,-0.000001,0.000001,-0.000001,0.000001,-0.806873,-0.116330,0.003472,0.002315,0.000001,-0.000002,0.000001,-0.000001,1.399346,-0.105657,0.003154,0.002103,0.000001,-0.000002,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.187114,0.082306,-0.008360,-0.001844,0.000015,-0.000014,0.000014,-0.000011,-0.917417,-0.102445,0.010405,0.002295,-0.000019,0.000018,-0.000018,0.000014,1.298945,-0.093045,0.009451,0.002084,-0.000017,0.000016,-0.000016,0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.115008,0.060050,-0.013967,-0.002036,-0.000237,0.000109,-0.000251,0.000183,-1.007166,-0.074744,0.017384,0.002534,0.000295,-0.000135,0.000312,-0.000228,1.217430,-0.067886,0.015789,0.002302,0.000268,-0.000123,0.000284,-0.000207,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.071156,0.025380,-0.020333,-0.000515,0.002943,-0.000018,0.003683,-0.002421,-1.061748,-0.031590,0.025308,0.000641,-0.003663,0.000023,-0.004584,0.003013,1.167856,-0.028692,0.022986,0.000582,-0.003327,0.000021,-0.004163,0.002737,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/sequence_trajectories/1/10.csv: -------------------------------------------------------------------------------- 1 | duration,x^0,x^1,x^2,x^3,x^4,x^5,x^6,x^7,y^0,y^1,y^2,y^3,y^4,y^5,y^6,y^7,z^0,z^1,z^2,z^3,z^4,z^5,z^6,z^7,yaw^0,yaw^1,yaw^2,yaw^3,yaw^4,yaw^5,yaw^6,yaw^7, 2 | 1.000000,-0.314090,0.000000,0.000000,0.000000,-0.031461,0.033971,-0.015667,0.002860,-0.227130,0.000000,0.000000,0.000000,-0.001420,0.001536,-0.000709,0.000130,1.346800,0.000000,0.000000,0.000000,-0.005097,0.005504,-0.002538,0.000463,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 3 | 1.000000,-0.324387,-0.029974,-0.024009,0.000613,0.003476,-0.002886,0.001216,-0.000216,-0.227593,-0.001347,-0.001074,0.000033,0.000157,-0.000131,0.000055,-0.000010,1.345132,-0.004856,-0.003890,0.000099,0.000563,-0.000468,0.000197,-0.000035,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 4 | 1.000000,-0.376168,-0.070898,-0.016477,0.002409,-0.000280,0.000195,-0.000076,0.000013,-0.229909,-0.003158,-0.000718,0.000114,-0.000013,0.000009,-0.000003,0.000001,1.336742,-0.011487,-0.002670,0.000390,-0.000045,0.000032,-0.000012,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 5 | 1.000000,-0.461283,-0.097136,-0.009845,0.002180,0.000017,-0.000011,0.000004,-0.000001,-0.233678,-0.004277,-0.000406,0.000101,0.000000,-0.000001,0.000000,-0.000000,1.322952,-0.015738,-0.001595,0.000353,0.000003,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 6 | 1.000000,-0.566074,-0.110252,-0.003264,0.002196,-0.000002,0.000000,-0.000000,0.000000,-0.238260,-0.004787,-0.000105,0.000098,-0.000001,-0.000000,0.000000,-0.000000,1.305973,-0.017863,-0.000528,0.000356,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 7 | 1.000000,-0.677395,-0.110196,0.003318,0.002193,-0.000000,-0.000002,0.000001,-0.000001,-0.243055,-0.004707,0.000183,0.000093,-0.000001,-0.000000,0.000000,-0.000000,1.287938,-0.017852,0.000539,0.000355,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 8 | 1.000000,-0.782081,-0.096990,0.009878,0.002169,-0.000019,0.000017,-0.000017,0.000013,-0.247487,-0.004068,0.000453,0.000086,-0.000003,0.000000,-0.000000,0.000000,1.270979,-0.015710,0.001602,0.000351,-0.000003,0.000003,-0.000003,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 9 | 1.000000,-0.867030,-0.070726,0.016468,0.002391,0.000278,-0.000128,0.000295,-0.000215,-0.251018,-0.002910,0.000705,0.000089,0.000010,-0.000005,0.000012,-0.000009,1.257222,-0.011452,0.002668,0.000387,0.000045,-0.000021,0.000048,-0.000035,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 10 | 1.000000,-0.918667,-0.029878,0.023942,0.000600,-0.003463,0.000021,-0.004333,0.002849,-0.253126,-0.001209,0.000977,0.000014,-0.000138,0.000001,-0.000172,0.000113,1.248861,-0.004837,0.003876,0.000096,-0.000560,0.000003,-0.000701,0.000461,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000, 11 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/test_videoOutput.py: -------------------------------------------------------------------------------- 1 | """Test video output in simulation. 2 | 3 | This test is a bit strange: due to the following conditions 4 | 5 | - Video capture runs until the script process stops, 6 | - Video capture uses a child ffmpeg process to do compression, 7 | - We register an `atexit` function to ensure that the pipe is flushed and 8 | ffmpeg process is closed cleanly, 9 | 10 | we must run the video-generating script in a separate process. 11 | 12 | Using `multiprocessing` would be easier, but `multiprocessing` processes don't 13 | run `atexit` handlers when they exit. This design is controversial [1, 2]. 14 | Therefore, we must do a full `system()`-style process spawn with `subprocess` 15 | instead. To avoid adding another script just to support this test, this script 16 | will behave as the video generator process when run as `__main__()`, and behave 17 | as the test process when run via pytest. 18 | 19 | [1] https://stackoverflow.com/questions/2546276/python-process-wont-call-atexit 20 | [2] https://stackoverflow.com/questions/34506638/how-to-register-atexit-function-in-pythons-multiprocessing-subprocess 21 | """ 22 | 23 | import os 24 | import subprocess 25 | import sys 26 | 27 | import numpy as np 28 | import pytest 29 | 30 | from pycrazyswarm import * 31 | 32 | crazyflies_yaml = """ 33 | crazyflies: 34 | - id: 1 35 | channel: 110 36 | initialPosition: [0.0, 0.0, 0.0] 37 | """ 38 | 39 | Z = 1.0 40 | FPS = 12 41 | DT = 1.0 / FPS 42 | TOTAL_TIME = 4.0 43 | 44 | 45 | try: 46 | import vispy 47 | import ffmpeg 48 | HAS_DEPENDENCIES = True 49 | # For some reason the vispy import fails in Github CI for MacOS Py2.7 with a 50 | # ValueError, even though we install vispy. Strange but doesn't matter. 51 | except (ImportError, ValueError): 52 | HAS_DEPENDENCIES = False 53 | 54 | 55 | def videoWriterProcess(path): 56 | args = "--sim --vis vispy --dt {} --video {}".format(DT, path) 57 | swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args=args) 58 | timeHelper = swarm.timeHelper 59 | cf = swarm.allcfs.crazyflies[0] 60 | 61 | cf.takeoff(targetHeight=Z, duration=TOTAL_TIME / 2) 62 | timeHelper.sleep(TOTAL_TIME / 2) 63 | 64 | cf.goTo(cf.initialPosition + np.array([0.0, 1.0, Z]), yaw=0.0, duration=TOTAL_TIME / 2) 65 | timeHelper.sleep(TOTAL_TIME / 2) 66 | 67 | 68 | @pytest.mark.skipif(not HAS_DEPENDENCIES, reason="vispy and ffmpeg required for video.") 69 | def test_videoOutput(tmp_path): 70 | # tmp_path is supplied by pytest - a directory where we can write that will 71 | # eventually be deleted. 72 | path = str(tmp_path / "crazyswarm_test_video.mp4") 73 | subprocess.call([os.environ["CSW_PYTHON"], __file__, path]) 74 | 75 | import ffmpeg 76 | properties = ffmpeg.probe(path) 77 | stream = properties["streams"][0] 78 | file_duration = float(stream["duration"]) 79 | file_fps = int(stream["avg_frame_rate"].split("/")[0]) 80 | assert file_fps == FPS 81 | assert abs(file_duration - TOTAL_TIME) <= DT 82 | 83 | 84 | if __name__ == "__main__": 85 | videoWriterProcess(sys.argv[1]) 86 | -------------------------------------------------------------------------------- /ros_ws/src/crazyswarm/scripts/pycrazyswarm/visualizer/visMatplotlib.py: -------------------------------------------------------------------------------- 1 | import warnings 2 | 3 | from mpl_toolkits.mplot3d import Axes3D 4 | from mpl_toolkits.mplot3d.art3d import Line3DCollection 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | 8 | 9 | class VisMatplotlib: 10 | def __init__(self): 11 | self.fig = plt.figure() 12 | self.ax = self.fig.add_subplot(111, projection='3d') 13 | self.ax.set_xlim([-5, 5]) 14 | self.ax.set_ylim([-5, 5]) 15 | self.ax.set_zlim([0, 3]) 16 | self.ax.set_xlabel("X") 17 | self.ax.set_ylabel("Y") 18 | self.ax.set_zlabel("Z") 19 | self.plot = None 20 | self.timeAnnotation = self.ax.annotate("Time", xy=(0, 0), xycoords='axes fraction', fontsize=12, ha='right', va='bottom') 21 | 22 | self.line_color = 0.3 * np.ones(3) 23 | 24 | # Lazy-constructed data for connectivity graph gfx. 25 | self.graph_edges = None 26 | self.graph_lines = None 27 | self.graph = None 28 | 29 | def setGraph(self, edges): 30 | """Set edges of graph visualization - sequence of (i,j) tuples.""" 31 | 32 | # Only allocate new memory if we need to. 33 | n_edges = len(edges) 34 | if self.graph_edges is None or n_edges != len(self.graph_edges): 35 | self.graph_lines = np.zeros((n_edges, 2, 3)) 36 | self.graph_edges = edges 37 | 38 | # Lazily construct Matplotlib object for graph. 39 | if self.graph is None: 40 | self.graph = Line3DCollection(self.graph_lines, edgecolor=self.line_color) 41 | self.ax.add_collection(self.graph) 42 | 43 | def showEllipsoids(self, radii): 44 | warnings.warn("showEllipsoids not implemented in Matplotlib visualizer.") 45 | 46 | def update(self, t, crazyflies): 47 | xs = [] 48 | ys = [] 49 | zs = [] 50 | cs = [] 51 | for cf in crazyflies: 52 | x, y, z = cf.position() 53 | color = cf.ledRGB 54 | xs.append(x) 55 | ys.append(y) 56 | zs.append(z) 57 | cs.append(color) 58 | 59 | if self.plot is None: 60 | self.plot = self.ax.scatter(xs, ys, zs, c=cs) 61 | else: 62 | # TODO: Don't use protected members. 63 | self.plot._offsets3d = (xs, ys, zs) 64 | self.plot.set_facecolors(cs) 65 | self.plot.set_edgecolors(cs) 66 | self.plot._facecolor3d = self.plot.get_facecolor() 67 | self.plot._edgecolor3d = self.plot.get_edgecolor() 68 | 69 | if self.graph is not None: 70 | # Update graph line segments to match new Crazyflie positions. 71 | for k, (i, j) in enumerate(self.graph_edges): 72 | self.graph_lines[k, 0, :] = xs[i], ys[i], zs[i] 73 | self.graph_lines[k, 1, :] = xs[j], ys[j], zs[j] 74 | self.graph.set_segments(self.graph_lines) 75 | 76 | self.timeAnnotation.set_text("{} s".format(t)) 77 | plt.pause(0.0001) 78 | 79 | def render(self): 80 | warnings.warn("Rendering video not supported in VisMatplotlib yet.") 81 | return None 82 | --------------------------------------------------------------------------------