diff --git a/controller_test_drone_modeling/build/CMakeCache.txt b/controller_test_drone_modeling/build/CMakeCache.txt
deleted file mode 100644
index 95087ad58e83e9670d694f9532ab417fc54cc2ed..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeCache.txt
+++ /dev/null
@@ -1,394 +0,0 @@
-# This is the CMakeCache file.
-# For build in directory: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-# It was generated by CMake: /usr/bin/cmake
-# You can edit this file to change values found and used by cmake.
-# If you do not want to change any of the values, simply exit the editor.
-# If you do want to change a value, simply edit, save, and exit the editor.
-# The syntax for the file is as follows:
-# KEY:TYPE=VALUE
-# KEY is the name of a variable in the cache.
-# TYPE is a hint to GUIs for the type of VALUE, DO NOT EDIT TYPE!.
-# VALUE is the current value for the KEY.
-
-########################
-# EXTERNAL cache entries
-########################
-
-//The directory containing a CMake configuration file for Boost.
-Boost_DIR:PATH=Boost_DIR-NOTFOUND
-
-//Path to a file.
-Boost_INCLUDE_DIR:PATH=/usr/include
-
-//Boost library directory DEBUG
-Boost_LIBRARY_DIR_DEBUG:PATH=/usr/lib/x86_64-linux-gnu
-
-//Boost library directory RELEASE
-Boost_LIBRARY_DIR_RELEASE:PATH=/usr/lib/x86_64-linux-gnu
-
-//Boost program_options library (debug)
-Boost_PROGRAM_OPTIONS_LIBRARY_DEBUG:FILEPATH=/usr/lib/x86_64-linux-gnu/libboost_program_options.so
-
-//Boost program_options library (release)
-Boost_PROGRAM_OPTIONS_LIBRARY_RELEASE:FILEPATH=/usr/lib/x86_64-linux-gnu/libboost_program_options.so
-
-//Path to a program.
-CMAKE_AR:FILEPATH=/usr/bin/ar
-
-//Choose the type of build, options are: None(CMAKE_CXX_FLAGS or
-// CMAKE_C_FLAGS used) Debug Release RelWithDebInfo MinSizeRel.
-CMAKE_BUILD_TYPE:STRING=
-
-//Enable/Disable color output during build.
-CMAKE_COLOR_MAKEFILE:BOOL=ON
-
-//CXX compiler
-CMAKE_CXX_COMPILER:FILEPATH=/usr/bin/c++
-
-//A wrapper around 'ar' adding the appropriate '--plugin' option
-// for the GCC compiler
-CMAKE_CXX_COMPILER_AR:FILEPATH=/usr/bin/gcc-ar-7
-
-//A wrapper around 'ranlib' adding the appropriate '--plugin' option
-// for the GCC compiler
-CMAKE_CXX_COMPILER_RANLIB:FILEPATH=/usr/bin/gcc-ranlib-7
-
-//Flags used by the compiler during all build types.
-CMAKE_CXX_FLAGS:STRING=
-
-//Flags used by the compiler during debug builds.
-CMAKE_CXX_FLAGS_DEBUG:STRING=-g
-
-//Flags used by the compiler during release builds for minimum
-// size.
-CMAKE_CXX_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG
-
-//Flags used by the compiler during release builds.
-CMAKE_CXX_FLAGS_RELEASE:STRING=-O3 -DNDEBUG
-
-//Flags used by the compiler during release builds with debug info.
-CMAKE_CXX_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG
-
-//C compiler
-CMAKE_C_COMPILER:FILEPATH=/usr/bin/cc
-
-//A wrapper around 'ar' adding the appropriate '--plugin' option
-// for the GCC compiler
-CMAKE_C_COMPILER_AR:FILEPATH=/usr/bin/gcc-ar-7
-
-//A wrapper around 'ranlib' adding the appropriate '--plugin' option
-// for the GCC compiler
-CMAKE_C_COMPILER_RANLIB:FILEPATH=/usr/bin/gcc-ranlib-7
-
-//Flags used by the compiler during all build types.
-CMAKE_C_FLAGS:STRING=
-
-//Flags used by the compiler during debug builds.
-CMAKE_C_FLAGS_DEBUG:STRING=-g
-
-//Flags used by the compiler during release builds for minimum
-// size.
-CMAKE_C_FLAGS_MINSIZEREL:STRING=-Os -DNDEBUG
-
-//Flags used by the compiler during release builds.
-CMAKE_C_FLAGS_RELEASE:STRING=-O3 -DNDEBUG
-
-//Flags used by the compiler during release builds with debug info.
-CMAKE_C_FLAGS_RELWITHDEBINFO:STRING=-O2 -g -DNDEBUG
-
-//Flags used by the linker.
-CMAKE_EXE_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during debug builds.
-CMAKE_EXE_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during release minsize builds.
-CMAKE_EXE_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during release builds.
-CMAKE_EXE_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during Release with Debug Info builds.
-CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Enable/Disable output of compile commands during generation.
-CMAKE_EXPORT_COMPILE_COMMANDS:BOOL=OFF
-
-//Install path prefix, prepended onto install directories.
-CMAKE_INSTALL_PREFIX:PATH=/usr/local
-
-//Path to a program.
-CMAKE_LINKER:FILEPATH=/usr/bin/ld
-
-//Path to a program.
-CMAKE_MAKE_PROGRAM:FILEPATH=/usr/bin/make
-
-//Flags used by the linker during the creation of modules.
-CMAKE_MODULE_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during debug builds.
-CMAKE_MODULE_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during release minsize builds.
-CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during release builds.
-CMAKE_MODULE_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during Release with Debug Info builds.
-CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Path to a program.
-CMAKE_NM:FILEPATH=/usr/bin/nm
-
-//Path to a program.
-CMAKE_OBJCOPY:FILEPATH=/usr/bin/objcopy
-
-//Path to a program.
-CMAKE_OBJDUMP:FILEPATH=/usr/bin/objdump
-
-//Value Computed by CMake
-CMAKE_PROJECT_NAME:STATIC=controller_test
-
-//Path to a program.
-CMAKE_RANLIB:FILEPATH=/usr/bin/ranlib
-
-//Flags used by the linker during the creation of dll's.
-CMAKE_SHARED_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during debug builds.
-CMAKE_SHARED_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during release minsize builds.
-CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during release builds.
-CMAKE_SHARED_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during Release with Debug Info builds.
-CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//If set, runtime paths are not added when installing shared libraries,
-// but are added when building.
-CMAKE_SKIP_INSTALL_RPATH:BOOL=NO
-
-//If set, runtime paths are not added when using shared libraries.
-CMAKE_SKIP_RPATH:BOOL=NO
-
-//Flags used by the linker during the creation of static libraries.
-CMAKE_STATIC_LINKER_FLAGS:STRING=
-
-//Flags used by the linker during debug builds.
-CMAKE_STATIC_LINKER_FLAGS_DEBUG:STRING=
-
-//Flags used by the linker during release minsize builds.
-CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL:STRING=
-
-//Flags used by the linker during release builds.
-CMAKE_STATIC_LINKER_FLAGS_RELEASE:STRING=
-
-//Flags used by the linker during Release with Debug Info builds.
-CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO:STRING=
-
-//Path to a program.
-CMAKE_STRIP:FILEPATH=/usr/bin/strip
-
-//If this value is on, makefiles will be generated without the
-// .SILENT directive, and all commands will be echoed to the console
-// during the make. This is useful for debugging only. With Visual
-// Studio IDE projects all commands are done without /nologo.
-CMAKE_VERBOSE_MAKEFILE:BOOL=FALSE
-
-//Path to a library.
-USB_LIB:FILEPATH=/usr/lib/x86_64-linux-gnu/libusb-1.0.so
-
-//Value Computed by CMake
-controller_test_BINARY_DIR:STATIC=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-
-//Value Computed by CMake
-controller_test_SOURCE_DIR:STATIC=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-
-//Value Computed by CMake
-crazyflie_cpp_BINARY_DIR:STATIC=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp
-
-//Dependencies for the target
-crazyflie_cpp_LIB_DEPENDS:STATIC=general;/usr/lib/x86_64-linux-gnu/libusb-1.0.so;
-
-//Value Computed by CMake
-crazyflie_cpp_SOURCE_DIR:STATIC=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp
-
-
-########################
-# INTERNAL cache entries
-########################
-
-//ADVANCED property for variable: Boost_DIR
-Boost_DIR-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: Boost_INCLUDE_DIR
-Boost_INCLUDE_DIR-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: Boost_LIBRARY_DIR_DEBUG
-Boost_LIBRARY_DIR_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: Boost_LIBRARY_DIR_RELEASE
-Boost_LIBRARY_DIR_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: Boost_PROGRAM_OPTIONS_LIBRARY_DEBUG
-Boost_PROGRAM_OPTIONS_LIBRARY_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: Boost_PROGRAM_OPTIONS_LIBRARY_RELEASE
-Boost_PROGRAM_OPTIONS_LIBRARY_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_AR
-CMAKE_AR-ADVANCED:INTERNAL=1
-//This is the directory where this CMakeCache.txt was created
-CMAKE_CACHEFILE_DIR:INTERNAL=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-//Major version of cmake used to create the current loaded cache
-CMAKE_CACHE_MAJOR_VERSION:INTERNAL=3
-//Minor version of cmake used to create the current loaded cache
-CMAKE_CACHE_MINOR_VERSION:INTERNAL=10
-//Patch version of cmake used to create the current loaded cache
-CMAKE_CACHE_PATCH_VERSION:INTERNAL=2
-//ADVANCED property for variable: CMAKE_COLOR_MAKEFILE
-CMAKE_COLOR_MAKEFILE-ADVANCED:INTERNAL=1
-//Path to CMake executable.
-CMAKE_COMMAND:INTERNAL=/usr/bin/cmake
-//Path to cpack program executable.
-CMAKE_CPACK_COMMAND:INTERNAL=/usr/bin/cpack
-//Path to ctest program executable.
-CMAKE_CTEST_COMMAND:INTERNAL=/usr/bin/ctest
-//ADVANCED property for variable: CMAKE_CXX_COMPILER
-CMAKE_CXX_COMPILER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_COMPILER_AR
-CMAKE_CXX_COMPILER_AR-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_COMPILER_RANLIB
-CMAKE_CXX_COMPILER_RANLIB-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS
-CMAKE_CXX_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_DEBUG
-CMAKE_CXX_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_MINSIZEREL
-CMAKE_CXX_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELEASE
-CMAKE_CXX_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_CXX_FLAGS_RELWITHDEBINFO
-CMAKE_CXX_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_COMPILER
-CMAKE_C_COMPILER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_COMPILER_AR
-CMAKE_C_COMPILER_AR-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_COMPILER_RANLIB
-CMAKE_C_COMPILER_RANLIB-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS
-CMAKE_C_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_DEBUG
-CMAKE_C_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_MINSIZEREL
-CMAKE_C_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_RELEASE
-CMAKE_C_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_C_FLAGS_RELWITHDEBINFO
-CMAKE_C_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//Executable file format
-CMAKE_EXECUTABLE_FORMAT:INTERNAL=ELF
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS
-CMAKE_EXE_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_DEBUG
-CMAKE_EXE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_MINSIZEREL
-CMAKE_EXE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELEASE
-CMAKE_EXE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_EXE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_EXPORT_COMPILE_COMMANDS
-CMAKE_EXPORT_COMPILE_COMMANDS-ADVANCED:INTERNAL=1
-//Name of external makefile project generator.
-CMAKE_EXTRA_GENERATOR:INTERNAL=
-//Name of generator.
-CMAKE_GENERATOR:INTERNAL=Unix Makefiles
-//Name of generator platform.
-CMAKE_GENERATOR_PLATFORM:INTERNAL=
-//Name of generator toolset.
-CMAKE_GENERATOR_TOOLSET:INTERNAL=
-//Have symbol pthread_create
-CMAKE_HAVE_LIBC_CREATE:INTERNAL=
-//Have library pthreads
-CMAKE_HAVE_PTHREADS_CREATE:INTERNAL=
-//Have library pthread
-CMAKE_HAVE_PTHREAD_CREATE:INTERNAL=1
-//Have include pthread.h
-CMAKE_HAVE_PTHREAD_H:INTERNAL=1
-//Source directory with the top level CMakeLists.txt file for this
-// project
-CMAKE_HOME_DIRECTORY:INTERNAL=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-//Install .so files without execute permission.
-CMAKE_INSTALL_SO_NO_EXE:INTERNAL=1
-//ADVANCED property for variable: CMAKE_LINKER
-CMAKE_LINKER-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MAKE_PROGRAM
-CMAKE_MAKE_PROGRAM-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS
-CMAKE_MODULE_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_DEBUG
-CMAKE_MODULE_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL
-CMAKE_MODULE_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELEASE
-CMAKE_MODULE_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_MODULE_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_NM
-CMAKE_NM-ADVANCED:INTERNAL=1
-//number of local generators
-CMAKE_NUMBER_OF_MAKEFILES:INTERNAL=2
-//ADVANCED property for variable: CMAKE_OBJCOPY
-CMAKE_OBJCOPY-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_OBJDUMP
-CMAKE_OBJDUMP-ADVANCED:INTERNAL=1
-//Platform information initialized
-CMAKE_PLATFORM_INFO_INITIALIZED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_RANLIB
-CMAKE_RANLIB-ADVANCED:INTERNAL=1
-//Path to CMake installation.
-CMAKE_ROOT:INTERNAL=/usr/share/cmake-3.10
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS
-CMAKE_SHARED_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_DEBUG
-CMAKE_SHARED_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL
-CMAKE_SHARED_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELEASE
-CMAKE_SHARED_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_SHARED_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SKIP_INSTALL_RPATH
-CMAKE_SKIP_INSTALL_RPATH-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_SKIP_RPATH
-CMAKE_SKIP_RPATH-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS
-CMAKE_STATIC_LINKER_FLAGS-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_DEBUG
-CMAKE_STATIC_LINKER_FLAGS_DEBUG-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL
-CMAKE_STATIC_LINKER_FLAGS_MINSIZEREL-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELEASE
-CMAKE_STATIC_LINKER_FLAGS_RELEASE-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO
-CMAKE_STATIC_LINKER_FLAGS_RELWITHDEBINFO-ADVANCED:INTERNAL=1
-//ADVANCED property for variable: CMAKE_STRIP
-CMAKE_STRIP-ADVANCED:INTERNAL=1
-//uname command
-CMAKE_UNAME:INTERNAL=/bin/uname
-//ADVANCED property for variable: CMAKE_VERBOSE_MAKEFILE
-CMAKE_VERBOSE_MAKEFILE-ADVANCED:INTERNAL=1
-//Details about finding Threads
-FIND_PACKAGE_MESSAGE_DETAILS_Threads:INTERNAL=[TRUE][v()]
-//Components requested for this build tree.
-_Boost_COMPONENTS_SEARCHED:INTERNAL=program_options
-//Last used Boost_INCLUDE_DIR value.
-_Boost_INCLUDE_DIR_LAST:INTERNAL=/usr/include
-//Last used Boost_LIBRARY_DIR_DEBUG value.
-_Boost_LIBRARY_DIR_DEBUG_LAST:INTERNAL=/usr/lib/x86_64-linux-gnu
-//Last used Boost_LIBRARY_DIR_RELEASE value.
-_Boost_LIBRARY_DIR_RELEASE_LAST:INTERNAL=/usr/lib/x86_64-linux-gnu
-//Last used Boost_NAMESPACE value.
-_Boost_NAMESPACE_LAST:INTERNAL=boost
-//Last used Boost_USE_MULTITHREADED value.
-_Boost_USE_MULTITHREADED_LAST:INTERNAL=TRUE
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeCCompiler.cmake b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeCCompiler.cmake
deleted file mode 100644
index 9e0e71d8c6b896ec560f07199f9cc974e84817a5..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeCCompiler.cmake
+++ /dev/null
@@ -1,73 +0,0 @@
-set(CMAKE_C_COMPILER "/usr/bin/cc")
-set(CMAKE_C_COMPILER_ARG1 "")
-set(CMAKE_C_COMPILER_ID "GNU")
-set(CMAKE_C_COMPILER_VERSION "7.5.0")
-set(CMAKE_C_COMPILER_VERSION_INTERNAL "")
-set(CMAKE_C_COMPILER_WRAPPER "")
-set(CMAKE_C_STANDARD_COMPUTED_DEFAULT "11")
-set(CMAKE_C_COMPILE_FEATURES "c_std_90;c_function_prototypes;c_std_99;c_restrict;c_variadic_macros;c_std_11;c_static_assert")
-set(CMAKE_C90_COMPILE_FEATURES "c_std_90;c_function_prototypes")
-set(CMAKE_C99_COMPILE_FEATURES "c_std_99;c_restrict;c_variadic_macros")
-set(CMAKE_C11_COMPILE_FEATURES "c_std_11;c_static_assert")
-
-set(CMAKE_C_PLATFORM_ID "Linux")
-set(CMAKE_C_SIMULATE_ID "")
-set(CMAKE_C_SIMULATE_VERSION "")
-
-
-
-set(CMAKE_AR "/usr/bin/ar")
-set(CMAKE_C_COMPILER_AR "/usr/bin/gcc-ar-7")
-set(CMAKE_RANLIB "/usr/bin/ranlib")
-set(CMAKE_C_COMPILER_RANLIB "/usr/bin/gcc-ranlib-7")
-set(CMAKE_LINKER "/usr/bin/ld")
-set(CMAKE_COMPILER_IS_GNUCC 1)
-set(CMAKE_C_COMPILER_LOADED 1)
-set(CMAKE_C_COMPILER_WORKS TRUE)
-set(CMAKE_C_ABI_COMPILED TRUE)
-set(CMAKE_COMPILER_IS_MINGW )
-set(CMAKE_COMPILER_IS_CYGWIN )
-if(CMAKE_COMPILER_IS_CYGWIN)
- set(CYGWIN 1)
- set(UNIX 1)
-endif()
-
-set(CMAKE_C_COMPILER_ENV_VAR "CC")
-
-if(CMAKE_COMPILER_IS_MINGW)
- set(MINGW 1)
-endif()
-set(CMAKE_C_COMPILER_ID_RUN 1)
-set(CMAKE_C_SOURCE_FILE_EXTENSIONS c;m)
-set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC)
-set(CMAKE_C_LINKER_PREFERENCE 10)
-
-# Save compiler ABI information.
-set(CMAKE_C_SIZEOF_DATA_PTR "8")
-set(CMAKE_C_COMPILER_ABI "ELF")
-set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
-
-if(CMAKE_C_SIZEOF_DATA_PTR)
- set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}")
-endif()
-
-if(CMAKE_C_COMPILER_ABI)
- set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}")
-endif()
-
-if(CMAKE_C_LIBRARY_ARCHITECTURE)
- set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
-endif()
-
-set(CMAKE_C_CL_SHOWINCLUDES_PREFIX "")
-if(CMAKE_C_CL_SHOWINCLUDES_PREFIX)
- set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_C_CL_SHOWINCLUDES_PREFIX}")
-endif()
-
-
-
-
-
-set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "gcc;gcc_s;c;gcc;gcc_s")
-set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib")
-set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "")
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeCXXCompiler.cmake b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeCXXCompiler.cmake
deleted file mode 100644
index 85984d753b2f39384841d4e81fd0f2225061bc74..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeCXXCompiler.cmake
+++ /dev/null
@@ -1,75 +0,0 @@
-set(CMAKE_CXX_COMPILER "/usr/bin/c++")
-set(CMAKE_CXX_COMPILER_ARG1 "")
-set(CMAKE_CXX_COMPILER_ID "GNU")
-set(CMAKE_CXX_COMPILER_VERSION "7.5.0")
-set(CMAKE_CXX_COMPILER_VERSION_INTERNAL "")
-set(CMAKE_CXX_COMPILER_WRAPPER "")
-set(CMAKE_CXX_STANDARD_COMPUTED_DEFAULT "14")
-set(CMAKE_CXX_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters;cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates;cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates;cxx_std_17")
-set(CMAKE_CXX98_COMPILE_FEATURES "cxx_std_98;cxx_template_template_parameters")
-set(CMAKE_CXX11_COMPILE_FEATURES "cxx_std_11;cxx_alias_templates;cxx_alignas;cxx_alignof;cxx_attributes;cxx_auto_type;cxx_constexpr;cxx_decltype;cxx_decltype_incomplete_return_types;cxx_default_function_template_args;cxx_defaulted_functions;cxx_defaulted_move_initializers;cxx_delegating_constructors;cxx_deleted_functions;cxx_enum_forward_declarations;cxx_explicit_conversions;cxx_extended_friend_declarations;cxx_extern_templates;cxx_final;cxx_func_identifier;cxx_generalized_initializers;cxx_inheriting_constructors;cxx_inline_namespaces;cxx_lambdas;cxx_local_type_template_args;cxx_long_long_type;cxx_noexcept;cxx_nonstatic_member_init;cxx_nullptr;cxx_override;cxx_range_for;cxx_raw_string_literals;cxx_reference_qualified_functions;cxx_right_angle_brackets;cxx_rvalue_references;cxx_sizeof_member;cxx_static_assert;cxx_strong_enums;cxx_thread_local;cxx_trailing_return_types;cxx_unicode_literals;cxx_uniform_initialization;cxx_unrestricted_unions;cxx_user_literals;cxx_variadic_macros;cxx_variadic_templates")
-set(CMAKE_CXX14_COMPILE_FEATURES "cxx_std_14;cxx_aggregate_default_initializers;cxx_attribute_deprecated;cxx_binary_literals;cxx_contextual_conversions;cxx_decltype_auto;cxx_digit_separators;cxx_generic_lambdas;cxx_lambda_init_captures;cxx_relaxed_constexpr;cxx_return_type_deduction;cxx_variable_templates")
-set(CMAKE_CXX17_COMPILE_FEATURES "cxx_std_17")
-
-set(CMAKE_CXX_PLATFORM_ID "Linux")
-set(CMAKE_CXX_SIMULATE_ID "")
-set(CMAKE_CXX_SIMULATE_VERSION "")
-
-
-
-set(CMAKE_AR "/usr/bin/ar")
-set(CMAKE_CXX_COMPILER_AR "/usr/bin/gcc-ar-7")
-set(CMAKE_RANLIB "/usr/bin/ranlib")
-set(CMAKE_CXX_COMPILER_RANLIB "/usr/bin/gcc-ranlib-7")
-set(CMAKE_LINKER "/usr/bin/ld")
-set(CMAKE_COMPILER_IS_GNUCXX 1)
-set(CMAKE_CXX_COMPILER_LOADED 1)
-set(CMAKE_CXX_COMPILER_WORKS TRUE)
-set(CMAKE_CXX_ABI_COMPILED TRUE)
-set(CMAKE_COMPILER_IS_MINGW )
-set(CMAKE_COMPILER_IS_CYGWIN )
-if(CMAKE_COMPILER_IS_CYGWIN)
- set(CYGWIN 1)
- set(UNIX 1)
-endif()
-
-set(CMAKE_CXX_COMPILER_ENV_VAR "CXX")
-
-if(CMAKE_COMPILER_IS_MINGW)
- set(MINGW 1)
-endif()
-set(CMAKE_CXX_COMPILER_ID_RUN 1)
-set(CMAKE_CXX_IGNORE_EXTENSIONS inl;h;hpp;HPP;H;o;O;obj;OBJ;def;DEF;rc;RC)
-set(CMAKE_CXX_SOURCE_FILE_EXTENSIONS C;M;c++;cc;cpp;cxx;mm;CPP)
-set(CMAKE_CXX_LINKER_PREFERENCE 30)
-set(CMAKE_CXX_LINKER_PREFERENCE_PROPAGATES 1)
-
-# Save compiler ABI information.
-set(CMAKE_CXX_SIZEOF_DATA_PTR "8")
-set(CMAKE_CXX_COMPILER_ABI "ELF")
-set(CMAKE_CXX_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
-
-if(CMAKE_CXX_SIZEOF_DATA_PTR)
- set(CMAKE_SIZEOF_VOID_P "${CMAKE_CXX_SIZEOF_DATA_PTR}")
-endif()
-
-if(CMAKE_CXX_COMPILER_ABI)
- set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_CXX_COMPILER_ABI}")
-endif()
-
-if(CMAKE_CXX_LIBRARY_ARCHITECTURE)
- set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu")
-endif()
-
-set(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX "")
-if(CMAKE_CXX_CL_SHOWINCLUDES_PREFIX)
- set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_CXX_CL_SHOWINCLUDES_PREFIX}")
-endif()
-
-
-
-
-
-set(CMAKE_CXX_IMPLICIT_LINK_LIBRARIES "stdc++;m;gcc_s;gcc;c;gcc_s;gcc")
-set(CMAKE_CXX_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib")
-set(CMAKE_CXX_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "")
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_C.bin b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_C.bin
deleted file mode 100755
index b1860a3dd40b90b19fab68ce7a4bf1a8fee6135d..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_C.bin and /dev/null differ
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_CXX.bin b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_CXX.bin
deleted file mode 100755
index 19a9ccae8956d20ea78fcbaa929a195ae4089300..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_CXX.bin and /dev/null differ
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeSystem.cmake b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeSystem.cmake
deleted file mode 100644
index 7962f62c0b4af7ac10a6ee39988ef0ca224dbc1d..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CMakeSystem.cmake
+++ /dev/null
@@ -1,15 +0,0 @@
-set(CMAKE_HOST_SYSTEM "Linux-5.4.0-105-generic")
-set(CMAKE_HOST_SYSTEM_NAME "Linux")
-set(CMAKE_HOST_SYSTEM_VERSION "5.4.0-105-generic")
-set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64")
-
-
-
-set(CMAKE_SYSTEM "Linux-5.4.0-105-generic")
-set(CMAKE_SYSTEM_NAME "Linux")
-set(CMAKE_SYSTEM_VERSION "5.4.0-105-generic")
-set(CMAKE_SYSTEM_PROCESSOR "x86_64")
-
-set(CMAKE_CROSSCOMPILING "FALSE")
-
-set(CMAKE_SYSTEM_LOADED 1)
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdC/CMakeCCompilerId.c b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdC/CMakeCCompilerId.c
deleted file mode 100644
index 722faa803f6df8628261ccd5da97b74fa64c0114..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdC/CMakeCCompilerId.c
+++ /dev/null
@@ -1,598 +0,0 @@
-#ifdef __cplusplus
-# error "A C++ compiler has been selected for C."
-#endif
-
-#if defined(__18CXX)
-# define ID_VOID_MAIN
-#endif
-#if defined(__CLASSIC_C__)
-/* cv-qualifiers did not exist in K&R C */
-# define const
-# define volatile
-#endif
-
-
-/* Version number components: V=Version, R=Revision, P=Patch
- Version date components: YYYY=Year, MM=Month, DD=Day */
-
-#if defined(__INTEL_COMPILER) || defined(__ICC)
-# define COMPILER_ID "Intel"
-# if defined(_MSC_VER)
-# define SIMULATE_ID "MSVC"
-# endif
- /* __INTEL_COMPILER = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100)
-# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10)
-# if defined(__INTEL_COMPILER_UPDATE)
-# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER_UPDATE)
-# else
-# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10)
-# endif
-# if defined(__INTEL_COMPILER_BUILD_DATE)
- /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */
-# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE)
-# endif
-# if defined(_MSC_VER)
- /* _MSC_VER = VVRR */
-# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
-# endif
-
-#elif defined(__PATHCC__)
-# define COMPILER_ID "PathScale"
-# define COMPILER_VERSION_MAJOR DEC(__PATHCC__)
-# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__)
-# if defined(__PATHCC_PATCHLEVEL__)
-# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__)
-# endif
-
-#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__)
-# define COMPILER_ID "Embarcadero"
-# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF)
-# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF)
-# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF)
-
-#elif defined(__BORLANDC__)
-# define COMPILER_ID "Borland"
- /* __BORLANDC__ = 0xVRR */
-# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8)
-# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF)
-
-#elif defined(__WATCOMC__) && __WATCOMC__ < 1200
-# define COMPILER_ID "Watcom"
- /* __WATCOMC__ = VVRR */
-# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100)
-# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10)
-# if (__WATCOMC__ % 10) > 0
-# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10)
-# endif
-
-#elif defined(__WATCOMC__)
-# define COMPILER_ID "OpenWatcom"
- /* __WATCOMC__ = VVRP + 1100 */
-# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100)
-# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10)
-# if (__WATCOMC__ % 10) > 0
-# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10)
-# endif
-
-#elif defined(__SUNPRO_C)
-# define COMPILER_ID "SunPro"
-# if __SUNPRO_C >= 0x5100
- /* __SUNPRO_C = 0xVRRP */
-# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>12)
-# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xFF)
-# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF)
-# else
- /* __SUNPRO_CC = 0xVRP */
-# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_C>>8)
-# define COMPILER_VERSION_MINOR HEX(__SUNPRO_C>>4 & 0xF)
-# define COMPILER_VERSION_PATCH HEX(__SUNPRO_C & 0xF)
-# endif
-
-#elif defined(__HP_cc)
-# define COMPILER_ID "HP"
- /* __HP_cc = VVRRPP */
-# define COMPILER_VERSION_MAJOR DEC(__HP_cc/10000)
-# define COMPILER_VERSION_MINOR DEC(__HP_cc/100 % 100)
-# define COMPILER_VERSION_PATCH DEC(__HP_cc % 100)
-
-#elif defined(__DECC)
-# define COMPILER_ID "Compaq"
- /* __DECC_VER = VVRRTPPPP */
-# define COMPILER_VERSION_MAJOR DEC(__DECC_VER/10000000)
-# define COMPILER_VERSION_MINOR DEC(__DECC_VER/100000 % 100)
-# define COMPILER_VERSION_PATCH DEC(__DECC_VER % 10000)
-
-#elif defined(__IBMC__) && defined(__COMPILER_VER__)
-# define COMPILER_ID "zOS"
- /* __IBMC__ = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100)
-# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10)
-
-#elif defined(__IBMC__) && !defined(__COMPILER_VER__) && __IBMC__ >= 800
-# define COMPILER_ID "XL"
- /* __IBMC__ = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100)
-# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10)
-
-#elif defined(__IBMC__) && !defined(__COMPILER_VER__) && __IBMC__ < 800
-# define COMPILER_ID "VisualAge"
- /* __IBMC__ = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__IBMC__/100)
-# define COMPILER_VERSION_MINOR DEC(__IBMC__/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(__IBMC__ % 10)
-
-#elif defined(__PGI)
-# define COMPILER_ID "PGI"
-# define COMPILER_VERSION_MAJOR DEC(__PGIC__)
-# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__)
-# if defined(__PGIC_PATCHLEVEL__)
-# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__)
-# endif
-
-#elif defined(_CRAYC)
-# define COMPILER_ID "Cray"
-# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR)
-# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR)
-
-#elif defined(__TI_COMPILER_VERSION__)
-# define COMPILER_ID "TI"
- /* __TI_COMPILER_VERSION__ = VVVRRRPPP */
-# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000)
-# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000)
-# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000)
-
-#elif defined(__FUJITSU) || defined(__FCC_VERSION) || defined(__fcc_version)
-# define COMPILER_ID "Fujitsu"
-
-#elif defined(__TINYC__)
-# define COMPILER_ID "TinyCC"
-
-#elif defined(__BCC__)
-# define COMPILER_ID "Bruce"
-
-#elif defined(__SCO_VERSION__)
-# define COMPILER_ID "SCO"
-
-#elif defined(__clang__) && defined(__apple_build_version__)
-# define COMPILER_ID "AppleClang"
-# if defined(_MSC_VER)
-# define SIMULATE_ID "MSVC"
-# endif
-# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
-# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
-# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
-# if defined(_MSC_VER)
- /* _MSC_VER = VVRR */
-# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
-# endif
-# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__)
-
-#elif defined(__clang__)
-# define COMPILER_ID "Clang"
-# if defined(_MSC_VER)
-# define SIMULATE_ID "MSVC"
-# endif
-# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
-# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
-# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
-# if defined(_MSC_VER)
- /* _MSC_VER = VVRR */
-# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
-# endif
-
-#elif defined(__GNUC__)
-# define COMPILER_ID "GNU"
-# define COMPILER_VERSION_MAJOR DEC(__GNUC__)
-# if defined(__GNUC_MINOR__)
-# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__)
-# endif
-# if defined(__GNUC_PATCHLEVEL__)
-# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__)
-# endif
-
-#elif defined(_MSC_VER)
-# define COMPILER_ID "MSVC"
- /* _MSC_VER = VVRR */
-# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100)
-# if defined(_MSC_FULL_VER)
-# if _MSC_VER >= 1400
- /* _MSC_FULL_VER = VVRRPPPPP */
-# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000)
-# else
- /* _MSC_FULL_VER = VVRRPPPP */
-# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000)
-# endif
-# endif
-# if defined(_MSC_BUILD)
-# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD)
-# endif
-
-#elif defined(__VISUALDSPVERSION__) || defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__)
-# define COMPILER_ID "ADSP"
-#if defined(__VISUALDSPVERSION__)
- /* __VISUALDSPVERSION__ = 0xVVRRPP00 */
-# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24)
-# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF)
-# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF)
-#endif
-
-#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC)
-# define COMPILER_ID "IAR"
-# if defined(__VER__)
-# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000)
-# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000)
-# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000)
-# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__)
-# endif
-
-#elif defined(__ARMCC_VERSION)
-# define COMPILER_ID "ARMCC"
-#if __ARMCC_VERSION >= 1000000
- /* __ARMCC_VERSION = VRRPPPP */
- # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000)
- # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100)
- # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000)
-#else
- /* __ARMCC_VERSION = VRPPPP */
- # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000)
- # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10)
- # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000)
-#endif
-
-
-#elif defined(__SDCC_VERSION_MAJOR) || defined(SDCC)
-# define COMPILER_ID "SDCC"
-# if defined(__SDCC_VERSION_MAJOR)
-# define COMPILER_VERSION_MAJOR DEC(__SDCC_VERSION_MAJOR)
-# define COMPILER_VERSION_MINOR DEC(__SDCC_VERSION_MINOR)
-# define COMPILER_VERSION_PATCH DEC(__SDCC_VERSION_PATCH)
-# else
- /* SDCC = VRP */
-# define COMPILER_VERSION_MAJOR DEC(SDCC/100)
-# define COMPILER_VERSION_MINOR DEC(SDCC/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(SDCC % 10)
-# endif
-
-#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION)
-# define COMPILER_ID "MIPSpro"
-# if defined(_SGI_COMPILER_VERSION)
- /* _SGI_COMPILER_VERSION = VRP */
-# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100)
-# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10)
-# else
- /* _COMPILER_VERSION = VRP */
-# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100)
-# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10)
-# endif
-
-
-/* These compilers are either not known or too old to define an
- identification macro. Try to identify the platform and guess that
- it is the native compiler. */
-#elif defined(__sgi)
-# define COMPILER_ID "MIPSpro"
-
-#elif defined(__hpux) || defined(__hpua)
-# define COMPILER_ID "HP"
-
-#else /* unknown compiler */
-# define COMPILER_ID ""
-#endif
-
-/* Construct the string literal in pieces to prevent the source from
- getting matched. Store it in a pointer rather than an array
- because some compilers will just produce instructions to fill the
- array rather than assigning a pointer to a static array. */
-char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]";
-#ifdef SIMULATE_ID
-char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]";
-#endif
-
-#ifdef __QNXNTO__
-char const* qnxnto = "INFO" ":" "qnxnto[]";
-#endif
-
-#if defined(__CRAYXE) || defined(__CRAYXC)
-char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]";
-#endif
-
-#define STRINGIFY_HELPER(X) #X
-#define STRINGIFY(X) STRINGIFY_HELPER(X)
-
-/* Identify known platforms by name. */
-#if defined(__linux) || defined(__linux__) || defined(linux)
-# define PLATFORM_ID "Linux"
-
-#elif defined(__CYGWIN__)
-# define PLATFORM_ID "Cygwin"
-
-#elif defined(__MINGW32__)
-# define PLATFORM_ID "MinGW"
-
-#elif defined(__APPLE__)
-# define PLATFORM_ID "Darwin"
-
-#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32)
-# define PLATFORM_ID "Windows"
-
-#elif defined(__FreeBSD__) || defined(__FreeBSD)
-# define PLATFORM_ID "FreeBSD"
-
-#elif defined(__NetBSD__) || defined(__NetBSD)
-# define PLATFORM_ID "NetBSD"
-
-#elif defined(__OpenBSD__) || defined(__OPENBSD)
-# define PLATFORM_ID "OpenBSD"
-
-#elif defined(__sun) || defined(sun)
-# define PLATFORM_ID "SunOS"
-
-#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__)
-# define PLATFORM_ID "AIX"
-
-#elif defined(__sgi) || defined(__sgi__) || defined(_SGI)
-# define PLATFORM_ID "IRIX"
-
-#elif defined(__hpux) || defined(__hpux__)
-# define PLATFORM_ID "HP-UX"
-
-#elif defined(__HAIKU__)
-# define PLATFORM_ID "Haiku"
-
-#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS)
-# define PLATFORM_ID "BeOS"
-
-#elif defined(__QNX__) || defined(__QNXNTO__)
-# define PLATFORM_ID "QNX"
-
-#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__)
-# define PLATFORM_ID "Tru64"
-
-#elif defined(__riscos) || defined(__riscos__)
-# define PLATFORM_ID "RISCos"
-
-#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__)
-# define PLATFORM_ID "SINIX"
-
-#elif defined(__UNIX_SV__)
-# define PLATFORM_ID "UNIX_SV"
-
-#elif defined(__bsdos__)
-# define PLATFORM_ID "BSDOS"
-
-#elif defined(_MPRAS) || defined(MPRAS)
-# define PLATFORM_ID "MP-RAS"
-
-#elif defined(__osf) || defined(__osf__)
-# define PLATFORM_ID "OSF1"
-
-#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv)
-# define PLATFORM_ID "SCO_SV"
-
-#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX)
-# define PLATFORM_ID "ULTRIX"
-
-#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX)
-# define PLATFORM_ID "Xenix"
-
-#elif defined(__WATCOMC__)
-# if defined(__LINUX__)
-# define PLATFORM_ID "Linux"
-
-# elif defined(__DOS__)
-# define PLATFORM_ID "DOS"
-
-# elif defined(__OS2__)
-# define PLATFORM_ID "OS2"
-
-# elif defined(__WINDOWS__)
-# define PLATFORM_ID "Windows3x"
-
-# else /* unknown platform */
-# define PLATFORM_ID
-# endif
-
-#else /* unknown platform */
-# define PLATFORM_ID
-
-#endif
-
-/* For windows compilers MSVC and Intel we can determine
- the architecture of the compiler being used. This is because
- the compilers do not have flags that can change the architecture,
- but rather depend on which compiler is being used
-*/
-#if defined(_WIN32) && defined(_MSC_VER)
-# if defined(_M_IA64)
-# define ARCHITECTURE_ID "IA64"
-
-# elif defined(_M_X64) || defined(_M_AMD64)
-# define ARCHITECTURE_ID "x64"
-
-# elif defined(_M_IX86)
-# define ARCHITECTURE_ID "X86"
-
-# elif defined(_M_ARM64)
-# define ARCHITECTURE_ID "ARM64"
-
-# elif defined(_M_ARM)
-# if _M_ARM == 4
-# define ARCHITECTURE_ID "ARMV4I"
-# elif _M_ARM == 5
-# define ARCHITECTURE_ID "ARMV5I"
-# else
-# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM)
-# endif
-
-# elif defined(_M_MIPS)
-# define ARCHITECTURE_ID "MIPS"
-
-# elif defined(_M_SH)
-# define ARCHITECTURE_ID "SHx"
-
-# else /* unknown architecture */
-# define ARCHITECTURE_ID ""
-# endif
-
-#elif defined(__WATCOMC__)
-# if defined(_M_I86)
-# define ARCHITECTURE_ID "I86"
-
-# elif defined(_M_IX86)
-# define ARCHITECTURE_ID "X86"
-
-# else /* unknown architecture */
-# define ARCHITECTURE_ID ""
-# endif
-
-#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC)
-# if defined(__ICCARM__)
-# define ARCHITECTURE_ID "ARM"
-
-# elif defined(__ICCAVR__)
-# define ARCHITECTURE_ID "AVR"
-
-# else /* unknown architecture */
-# define ARCHITECTURE_ID ""
-# endif
-#else
-# define ARCHITECTURE_ID
-#endif
-
-/* Convert integer to decimal digit literals. */
-#define DEC(n) \
- ('0' + (((n) / 10000000)%10)), \
- ('0' + (((n) / 1000000)%10)), \
- ('0' + (((n) / 100000)%10)), \
- ('0' + (((n) / 10000)%10)), \
- ('0' + (((n) / 1000)%10)), \
- ('0' + (((n) / 100)%10)), \
- ('0' + (((n) / 10)%10)), \
- ('0' + ((n) % 10))
-
-/* Convert integer to hex digit literals. */
-#define HEX(n) \
- ('0' + ((n)>>28 & 0xF)), \
- ('0' + ((n)>>24 & 0xF)), \
- ('0' + ((n)>>20 & 0xF)), \
- ('0' + ((n)>>16 & 0xF)), \
- ('0' + ((n)>>12 & 0xF)), \
- ('0' + ((n)>>8 & 0xF)), \
- ('0' + ((n)>>4 & 0xF)), \
- ('0' + ((n) & 0xF))
-
-/* Construct a string literal encoding the version number components. */
-#ifdef COMPILER_VERSION_MAJOR
-char const info_version[] = {
- 'I', 'N', 'F', 'O', ':',
- 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[',
- COMPILER_VERSION_MAJOR,
-# ifdef COMPILER_VERSION_MINOR
- '.', COMPILER_VERSION_MINOR,
-# ifdef COMPILER_VERSION_PATCH
- '.', COMPILER_VERSION_PATCH,
-# ifdef COMPILER_VERSION_TWEAK
- '.', COMPILER_VERSION_TWEAK,
-# endif
-# endif
-# endif
- ']','\0'};
-#endif
-
-/* Construct a string literal encoding the internal version number. */
-#ifdef COMPILER_VERSION_INTERNAL
-char const info_version_internal[] = {
- 'I', 'N', 'F', 'O', ':',
- 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_',
- 'i','n','t','e','r','n','a','l','[',
- COMPILER_VERSION_INTERNAL,']','\0'};
-#endif
-
-/* Construct a string literal encoding the version number components. */
-#ifdef SIMULATE_VERSION_MAJOR
-char const info_simulate_version[] = {
- 'I', 'N', 'F', 'O', ':',
- 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[',
- SIMULATE_VERSION_MAJOR,
-# ifdef SIMULATE_VERSION_MINOR
- '.', SIMULATE_VERSION_MINOR,
-# ifdef SIMULATE_VERSION_PATCH
- '.', SIMULATE_VERSION_PATCH,
-# ifdef SIMULATE_VERSION_TWEAK
- '.', SIMULATE_VERSION_TWEAK,
-# endif
-# endif
-# endif
- ']','\0'};
-#endif
-
-/* Construct the string literal in pieces to prevent the source from
- getting matched. Store it in a pointer rather than an array
- because some compilers will just produce instructions to fill the
- array rather than assigning a pointer to a static array. */
-char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]";
-char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]";
-
-
-
-
-#if !defined(__STDC__)
-# if defined(_MSC_VER) && !defined(__clang__)
-# define C_DIALECT "90"
-# else
-# define C_DIALECT
-# endif
-#elif __STDC_VERSION__ >= 201000L
-# define C_DIALECT "11"
-#elif __STDC_VERSION__ >= 199901L
-# define C_DIALECT "99"
-#else
-# define C_DIALECT "90"
-#endif
-const char* info_language_dialect_default =
- "INFO" ":" "dialect_default[" C_DIALECT "]";
-
-/*--------------------------------------------------------------------------*/
-
-#ifdef ID_VOID_MAIN
-void main() {}
-#else
-# if defined(__CLASSIC_C__)
-int main(argc, argv) int argc; char *argv[];
-# else
-int main(int argc, char* argv[])
-# endif
-{
- int require = 0;
- require += info_compiler[argc];
- require += info_platform[argc];
- require += info_arch[argc];
-#ifdef COMPILER_VERSION_MAJOR
- require += info_version[argc];
-#endif
-#ifdef COMPILER_VERSION_INTERNAL
- require += info_version_internal[argc];
-#endif
-#ifdef SIMULATE_ID
- require += info_simulate[argc];
-#endif
-#ifdef SIMULATE_VERSION_MAJOR
- require += info_simulate_version[argc];
-#endif
-#if defined(__CRAYXE) || defined(__CRAYXC)
- require += info_cray[argc];
-#endif
- require += info_language_dialect_default[argc];
- (void)argv;
- return require;
-}
-#endif
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdC/a.out b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdC/a.out
deleted file mode 100755
index 11b7df452ad29dfd7bf1d9b188856c9e33182403..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdC/a.out and /dev/null differ
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdCXX/CMakeCXXCompilerId.cpp b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
deleted file mode 100644
index 2d66298588989dc5d404dae0025b8bf4e952498e..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
+++ /dev/null
@@ -1,576 +0,0 @@
-/* This source file must have a .cpp extension so that all C++ compilers
- recognize the extension without flags. Borland does not know .cxx for
- example. */
-#ifndef __cplusplus
-# error "A C compiler has been selected for C++."
-#endif
-
-
-/* Version number components: V=Version, R=Revision, P=Patch
- Version date components: YYYY=Year, MM=Month, DD=Day */
-
-#if defined(__COMO__)
-# define COMPILER_ID "Comeau"
- /* __COMO_VERSION__ = VRR */
-# define COMPILER_VERSION_MAJOR DEC(__COMO_VERSION__ / 100)
-# define COMPILER_VERSION_MINOR DEC(__COMO_VERSION__ % 100)
-
-#elif defined(__INTEL_COMPILER) || defined(__ICC)
-# define COMPILER_ID "Intel"
-# if defined(_MSC_VER)
-# define SIMULATE_ID "MSVC"
-# endif
- /* __INTEL_COMPILER = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__INTEL_COMPILER/100)
-# define COMPILER_VERSION_MINOR DEC(__INTEL_COMPILER/10 % 10)
-# if defined(__INTEL_COMPILER_UPDATE)
-# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER_UPDATE)
-# else
-# define COMPILER_VERSION_PATCH DEC(__INTEL_COMPILER % 10)
-# endif
-# if defined(__INTEL_COMPILER_BUILD_DATE)
- /* __INTEL_COMPILER_BUILD_DATE = YYYYMMDD */
-# define COMPILER_VERSION_TWEAK DEC(__INTEL_COMPILER_BUILD_DATE)
-# endif
-# if defined(_MSC_VER)
- /* _MSC_VER = VVRR */
-# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
-# endif
-
-#elif defined(__PATHCC__)
-# define COMPILER_ID "PathScale"
-# define COMPILER_VERSION_MAJOR DEC(__PATHCC__)
-# define COMPILER_VERSION_MINOR DEC(__PATHCC_MINOR__)
-# if defined(__PATHCC_PATCHLEVEL__)
-# define COMPILER_VERSION_PATCH DEC(__PATHCC_PATCHLEVEL__)
-# endif
-
-#elif defined(__BORLANDC__) && defined(__CODEGEARC_VERSION__)
-# define COMPILER_ID "Embarcadero"
-# define COMPILER_VERSION_MAJOR HEX(__CODEGEARC_VERSION__>>24 & 0x00FF)
-# define COMPILER_VERSION_MINOR HEX(__CODEGEARC_VERSION__>>16 & 0x00FF)
-# define COMPILER_VERSION_PATCH DEC(__CODEGEARC_VERSION__ & 0xFFFF)
-
-#elif defined(__BORLANDC__)
-# define COMPILER_ID "Borland"
- /* __BORLANDC__ = 0xVRR */
-# define COMPILER_VERSION_MAJOR HEX(__BORLANDC__>>8)
-# define COMPILER_VERSION_MINOR HEX(__BORLANDC__ & 0xFF)
-
-#elif defined(__WATCOMC__) && __WATCOMC__ < 1200
-# define COMPILER_ID "Watcom"
- /* __WATCOMC__ = VVRR */
-# define COMPILER_VERSION_MAJOR DEC(__WATCOMC__ / 100)
-# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10)
-# if (__WATCOMC__ % 10) > 0
-# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10)
-# endif
-
-#elif defined(__WATCOMC__)
-# define COMPILER_ID "OpenWatcom"
- /* __WATCOMC__ = VVRP + 1100 */
-# define COMPILER_VERSION_MAJOR DEC((__WATCOMC__ - 1100) / 100)
-# define COMPILER_VERSION_MINOR DEC((__WATCOMC__ / 10) % 10)
-# if (__WATCOMC__ % 10) > 0
-# define COMPILER_VERSION_PATCH DEC(__WATCOMC__ % 10)
-# endif
-
-#elif defined(__SUNPRO_CC)
-# define COMPILER_ID "SunPro"
-# if __SUNPRO_CC >= 0x5100
- /* __SUNPRO_CC = 0xVRRP */
-# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>12)
-# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xFF)
-# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF)
-# else
- /* __SUNPRO_CC = 0xVRP */
-# define COMPILER_VERSION_MAJOR HEX(__SUNPRO_CC>>8)
-# define COMPILER_VERSION_MINOR HEX(__SUNPRO_CC>>4 & 0xF)
-# define COMPILER_VERSION_PATCH HEX(__SUNPRO_CC & 0xF)
-# endif
-
-#elif defined(__HP_aCC)
-# define COMPILER_ID "HP"
- /* __HP_aCC = VVRRPP */
-# define COMPILER_VERSION_MAJOR DEC(__HP_aCC/10000)
-# define COMPILER_VERSION_MINOR DEC(__HP_aCC/100 % 100)
-# define COMPILER_VERSION_PATCH DEC(__HP_aCC % 100)
-
-#elif defined(__DECCXX)
-# define COMPILER_ID "Compaq"
- /* __DECCXX_VER = VVRRTPPPP */
-# define COMPILER_VERSION_MAJOR DEC(__DECCXX_VER/10000000)
-# define COMPILER_VERSION_MINOR DEC(__DECCXX_VER/100000 % 100)
-# define COMPILER_VERSION_PATCH DEC(__DECCXX_VER % 10000)
-
-#elif defined(__IBMCPP__) && defined(__COMPILER_VER__)
-# define COMPILER_ID "zOS"
- /* __IBMCPP__ = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100)
-# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10)
-
-#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ >= 800
-# define COMPILER_ID "XL"
- /* __IBMCPP__ = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100)
-# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10)
-
-#elif defined(__IBMCPP__) && !defined(__COMPILER_VER__) && __IBMCPP__ < 800
-# define COMPILER_ID "VisualAge"
- /* __IBMCPP__ = VRP */
-# define COMPILER_VERSION_MAJOR DEC(__IBMCPP__/100)
-# define COMPILER_VERSION_MINOR DEC(__IBMCPP__/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(__IBMCPP__ % 10)
-
-#elif defined(__PGI)
-# define COMPILER_ID "PGI"
-# define COMPILER_VERSION_MAJOR DEC(__PGIC__)
-# define COMPILER_VERSION_MINOR DEC(__PGIC_MINOR__)
-# if defined(__PGIC_PATCHLEVEL__)
-# define COMPILER_VERSION_PATCH DEC(__PGIC_PATCHLEVEL__)
-# endif
-
-#elif defined(_CRAYC)
-# define COMPILER_ID "Cray"
-# define COMPILER_VERSION_MAJOR DEC(_RELEASE_MAJOR)
-# define COMPILER_VERSION_MINOR DEC(_RELEASE_MINOR)
-
-#elif defined(__TI_COMPILER_VERSION__)
-# define COMPILER_ID "TI"
- /* __TI_COMPILER_VERSION__ = VVVRRRPPP */
-# define COMPILER_VERSION_MAJOR DEC(__TI_COMPILER_VERSION__/1000000)
-# define COMPILER_VERSION_MINOR DEC(__TI_COMPILER_VERSION__/1000 % 1000)
-# define COMPILER_VERSION_PATCH DEC(__TI_COMPILER_VERSION__ % 1000)
-
-#elif defined(__FUJITSU) || defined(__FCC_VERSION) || defined(__fcc_version)
-# define COMPILER_ID "Fujitsu"
-
-#elif defined(__SCO_VERSION__)
-# define COMPILER_ID "SCO"
-
-#elif defined(__clang__) && defined(__apple_build_version__)
-# define COMPILER_ID "AppleClang"
-# if defined(_MSC_VER)
-# define SIMULATE_ID "MSVC"
-# endif
-# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
-# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
-# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
-# if defined(_MSC_VER)
- /* _MSC_VER = VVRR */
-# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
-# endif
-# define COMPILER_VERSION_TWEAK DEC(__apple_build_version__)
-
-#elif defined(__clang__)
-# define COMPILER_ID "Clang"
-# if defined(_MSC_VER)
-# define SIMULATE_ID "MSVC"
-# endif
-# define COMPILER_VERSION_MAJOR DEC(__clang_major__)
-# define COMPILER_VERSION_MINOR DEC(__clang_minor__)
-# define COMPILER_VERSION_PATCH DEC(__clang_patchlevel__)
-# if defined(_MSC_VER)
- /* _MSC_VER = VVRR */
-# define SIMULATE_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define SIMULATE_VERSION_MINOR DEC(_MSC_VER % 100)
-# endif
-
-#elif defined(__GNUC__) || defined(__GNUG__)
-# define COMPILER_ID "GNU"
-# if defined(__GNUC__)
-# define COMPILER_VERSION_MAJOR DEC(__GNUC__)
-# else
-# define COMPILER_VERSION_MAJOR DEC(__GNUG__)
-# endif
-# if defined(__GNUC_MINOR__)
-# define COMPILER_VERSION_MINOR DEC(__GNUC_MINOR__)
-# endif
-# if defined(__GNUC_PATCHLEVEL__)
-# define COMPILER_VERSION_PATCH DEC(__GNUC_PATCHLEVEL__)
-# endif
-
-#elif defined(_MSC_VER)
-# define COMPILER_ID "MSVC"
- /* _MSC_VER = VVRR */
-# define COMPILER_VERSION_MAJOR DEC(_MSC_VER / 100)
-# define COMPILER_VERSION_MINOR DEC(_MSC_VER % 100)
-# if defined(_MSC_FULL_VER)
-# if _MSC_VER >= 1400
- /* _MSC_FULL_VER = VVRRPPPPP */
-# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 100000)
-# else
- /* _MSC_FULL_VER = VVRRPPPP */
-# define COMPILER_VERSION_PATCH DEC(_MSC_FULL_VER % 10000)
-# endif
-# endif
-# if defined(_MSC_BUILD)
-# define COMPILER_VERSION_TWEAK DEC(_MSC_BUILD)
-# endif
-
-#elif defined(__VISUALDSPVERSION__) || defined(__ADSPBLACKFIN__) || defined(__ADSPTS__) || defined(__ADSP21000__)
-# define COMPILER_ID "ADSP"
-#if defined(__VISUALDSPVERSION__)
- /* __VISUALDSPVERSION__ = 0xVVRRPP00 */
-# define COMPILER_VERSION_MAJOR HEX(__VISUALDSPVERSION__>>24)
-# define COMPILER_VERSION_MINOR HEX(__VISUALDSPVERSION__>>16 & 0xFF)
-# define COMPILER_VERSION_PATCH HEX(__VISUALDSPVERSION__>>8 & 0xFF)
-#endif
-
-#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC)
-# define COMPILER_ID "IAR"
-# if defined(__VER__)
-# define COMPILER_VERSION_MAJOR DEC((__VER__) / 1000000)
-# define COMPILER_VERSION_MINOR DEC(((__VER__) / 1000) % 1000)
-# define COMPILER_VERSION_PATCH DEC((__VER__) % 1000)
-# define COMPILER_VERSION_INTERNAL DEC(__IAR_SYSTEMS_ICC__)
-# endif
-
-#elif defined(__ARMCC_VERSION)
-# define COMPILER_ID "ARMCC"
-#if __ARMCC_VERSION >= 1000000
- /* __ARMCC_VERSION = VRRPPPP */
- # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/1000000)
- # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 100)
- # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000)
-#else
- /* __ARMCC_VERSION = VRPPPP */
- # define COMPILER_VERSION_MAJOR DEC(__ARMCC_VERSION/100000)
- # define COMPILER_VERSION_MINOR DEC(__ARMCC_VERSION/10000 % 10)
- # define COMPILER_VERSION_PATCH DEC(__ARMCC_VERSION % 10000)
-#endif
-
-
-#elif defined(_SGI_COMPILER_VERSION) || defined(_COMPILER_VERSION)
-# define COMPILER_ID "MIPSpro"
-# if defined(_SGI_COMPILER_VERSION)
- /* _SGI_COMPILER_VERSION = VRP */
-# define COMPILER_VERSION_MAJOR DEC(_SGI_COMPILER_VERSION/100)
-# define COMPILER_VERSION_MINOR DEC(_SGI_COMPILER_VERSION/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(_SGI_COMPILER_VERSION % 10)
-# else
- /* _COMPILER_VERSION = VRP */
-# define COMPILER_VERSION_MAJOR DEC(_COMPILER_VERSION/100)
-# define COMPILER_VERSION_MINOR DEC(_COMPILER_VERSION/10 % 10)
-# define COMPILER_VERSION_PATCH DEC(_COMPILER_VERSION % 10)
-# endif
-
-
-/* These compilers are either not known or too old to define an
- identification macro. Try to identify the platform and guess that
- it is the native compiler. */
-#elif defined(__sgi)
-# define COMPILER_ID "MIPSpro"
-
-#elif defined(__hpux) || defined(__hpua)
-# define COMPILER_ID "HP"
-
-#else /* unknown compiler */
-# define COMPILER_ID ""
-#endif
-
-/* Construct the string literal in pieces to prevent the source from
- getting matched. Store it in a pointer rather than an array
- because some compilers will just produce instructions to fill the
- array rather than assigning a pointer to a static array. */
-char const* info_compiler = "INFO" ":" "compiler[" COMPILER_ID "]";
-#ifdef SIMULATE_ID
-char const* info_simulate = "INFO" ":" "simulate[" SIMULATE_ID "]";
-#endif
-
-#ifdef __QNXNTO__
-char const* qnxnto = "INFO" ":" "qnxnto[]";
-#endif
-
-#if defined(__CRAYXE) || defined(__CRAYXC)
-char const *info_cray = "INFO" ":" "compiler_wrapper[CrayPrgEnv]";
-#endif
-
-#define STRINGIFY_HELPER(X) #X
-#define STRINGIFY(X) STRINGIFY_HELPER(X)
-
-/* Identify known platforms by name. */
-#if defined(__linux) || defined(__linux__) || defined(linux)
-# define PLATFORM_ID "Linux"
-
-#elif defined(__CYGWIN__)
-# define PLATFORM_ID "Cygwin"
-
-#elif defined(__MINGW32__)
-# define PLATFORM_ID "MinGW"
-
-#elif defined(__APPLE__)
-# define PLATFORM_ID "Darwin"
-
-#elif defined(_WIN32) || defined(__WIN32__) || defined(WIN32)
-# define PLATFORM_ID "Windows"
-
-#elif defined(__FreeBSD__) || defined(__FreeBSD)
-# define PLATFORM_ID "FreeBSD"
-
-#elif defined(__NetBSD__) || defined(__NetBSD)
-# define PLATFORM_ID "NetBSD"
-
-#elif defined(__OpenBSD__) || defined(__OPENBSD)
-# define PLATFORM_ID "OpenBSD"
-
-#elif defined(__sun) || defined(sun)
-# define PLATFORM_ID "SunOS"
-
-#elif defined(_AIX) || defined(__AIX) || defined(__AIX__) || defined(__aix) || defined(__aix__)
-# define PLATFORM_ID "AIX"
-
-#elif defined(__sgi) || defined(__sgi__) || defined(_SGI)
-# define PLATFORM_ID "IRIX"
-
-#elif defined(__hpux) || defined(__hpux__)
-# define PLATFORM_ID "HP-UX"
-
-#elif defined(__HAIKU__)
-# define PLATFORM_ID "Haiku"
-
-#elif defined(__BeOS) || defined(__BEOS__) || defined(_BEOS)
-# define PLATFORM_ID "BeOS"
-
-#elif defined(__QNX__) || defined(__QNXNTO__)
-# define PLATFORM_ID "QNX"
-
-#elif defined(__tru64) || defined(_tru64) || defined(__TRU64__)
-# define PLATFORM_ID "Tru64"
-
-#elif defined(__riscos) || defined(__riscos__)
-# define PLATFORM_ID "RISCos"
-
-#elif defined(__sinix) || defined(__sinix__) || defined(__SINIX__)
-# define PLATFORM_ID "SINIX"
-
-#elif defined(__UNIX_SV__)
-# define PLATFORM_ID "UNIX_SV"
-
-#elif defined(__bsdos__)
-# define PLATFORM_ID "BSDOS"
-
-#elif defined(_MPRAS) || defined(MPRAS)
-# define PLATFORM_ID "MP-RAS"
-
-#elif defined(__osf) || defined(__osf__)
-# define PLATFORM_ID "OSF1"
-
-#elif defined(_SCO_SV) || defined(SCO_SV) || defined(sco_sv)
-# define PLATFORM_ID "SCO_SV"
-
-#elif defined(__ultrix) || defined(__ultrix__) || defined(_ULTRIX)
-# define PLATFORM_ID "ULTRIX"
-
-#elif defined(__XENIX__) || defined(_XENIX) || defined(XENIX)
-# define PLATFORM_ID "Xenix"
-
-#elif defined(__WATCOMC__)
-# if defined(__LINUX__)
-# define PLATFORM_ID "Linux"
-
-# elif defined(__DOS__)
-# define PLATFORM_ID "DOS"
-
-# elif defined(__OS2__)
-# define PLATFORM_ID "OS2"
-
-# elif defined(__WINDOWS__)
-# define PLATFORM_ID "Windows3x"
-
-# else /* unknown platform */
-# define PLATFORM_ID
-# endif
-
-#else /* unknown platform */
-# define PLATFORM_ID
-
-#endif
-
-/* For windows compilers MSVC and Intel we can determine
- the architecture of the compiler being used. This is because
- the compilers do not have flags that can change the architecture,
- but rather depend on which compiler is being used
-*/
-#if defined(_WIN32) && defined(_MSC_VER)
-# if defined(_M_IA64)
-# define ARCHITECTURE_ID "IA64"
-
-# elif defined(_M_X64) || defined(_M_AMD64)
-# define ARCHITECTURE_ID "x64"
-
-# elif defined(_M_IX86)
-# define ARCHITECTURE_ID "X86"
-
-# elif defined(_M_ARM64)
-# define ARCHITECTURE_ID "ARM64"
-
-# elif defined(_M_ARM)
-# if _M_ARM == 4
-# define ARCHITECTURE_ID "ARMV4I"
-# elif _M_ARM == 5
-# define ARCHITECTURE_ID "ARMV5I"
-# else
-# define ARCHITECTURE_ID "ARMV" STRINGIFY(_M_ARM)
-# endif
-
-# elif defined(_M_MIPS)
-# define ARCHITECTURE_ID "MIPS"
-
-# elif defined(_M_SH)
-# define ARCHITECTURE_ID "SHx"
-
-# else /* unknown architecture */
-# define ARCHITECTURE_ID ""
-# endif
-
-#elif defined(__WATCOMC__)
-# if defined(_M_I86)
-# define ARCHITECTURE_ID "I86"
-
-# elif defined(_M_IX86)
-# define ARCHITECTURE_ID "X86"
-
-# else /* unknown architecture */
-# define ARCHITECTURE_ID ""
-# endif
-
-#elif defined(__IAR_SYSTEMS_ICC__) || defined(__IAR_SYSTEMS_ICC)
-# if defined(__ICCARM__)
-# define ARCHITECTURE_ID "ARM"
-
-# elif defined(__ICCAVR__)
-# define ARCHITECTURE_ID "AVR"
-
-# else /* unknown architecture */
-# define ARCHITECTURE_ID ""
-# endif
-#else
-# define ARCHITECTURE_ID
-#endif
-
-/* Convert integer to decimal digit literals. */
-#define DEC(n) \
- ('0' + (((n) / 10000000)%10)), \
- ('0' + (((n) / 1000000)%10)), \
- ('0' + (((n) / 100000)%10)), \
- ('0' + (((n) / 10000)%10)), \
- ('0' + (((n) / 1000)%10)), \
- ('0' + (((n) / 100)%10)), \
- ('0' + (((n) / 10)%10)), \
- ('0' + ((n) % 10))
-
-/* Convert integer to hex digit literals. */
-#define HEX(n) \
- ('0' + ((n)>>28 & 0xF)), \
- ('0' + ((n)>>24 & 0xF)), \
- ('0' + ((n)>>20 & 0xF)), \
- ('0' + ((n)>>16 & 0xF)), \
- ('0' + ((n)>>12 & 0xF)), \
- ('0' + ((n)>>8 & 0xF)), \
- ('0' + ((n)>>4 & 0xF)), \
- ('0' + ((n) & 0xF))
-
-/* Construct a string literal encoding the version number components. */
-#ifdef COMPILER_VERSION_MAJOR
-char const info_version[] = {
- 'I', 'N', 'F', 'O', ':',
- 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','[',
- COMPILER_VERSION_MAJOR,
-# ifdef COMPILER_VERSION_MINOR
- '.', COMPILER_VERSION_MINOR,
-# ifdef COMPILER_VERSION_PATCH
- '.', COMPILER_VERSION_PATCH,
-# ifdef COMPILER_VERSION_TWEAK
- '.', COMPILER_VERSION_TWEAK,
-# endif
-# endif
-# endif
- ']','\0'};
-#endif
-
-/* Construct a string literal encoding the internal version number. */
-#ifdef COMPILER_VERSION_INTERNAL
-char const info_version_internal[] = {
- 'I', 'N', 'F', 'O', ':',
- 'c','o','m','p','i','l','e','r','_','v','e','r','s','i','o','n','_',
- 'i','n','t','e','r','n','a','l','[',
- COMPILER_VERSION_INTERNAL,']','\0'};
-#endif
-
-/* Construct a string literal encoding the version number components. */
-#ifdef SIMULATE_VERSION_MAJOR
-char const info_simulate_version[] = {
- 'I', 'N', 'F', 'O', ':',
- 's','i','m','u','l','a','t','e','_','v','e','r','s','i','o','n','[',
- SIMULATE_VERSION_MAJOR,
-# ifdef SIMULATE_VERSION_MINOR
- '.', SIMULATE_VERSION_MINOR,
-# ifdef SIMULATE_VERSION_PATCH
- '.', SIMULATE_VERSION_PATCH,
-# ifdef SIMULATE_VERSION_TWEAK
- '.', SIMULATE_VERSION_TWEAK,
-# endif
-# endif
-# endif
- ']','\0'};
-#endif
-
-/* Construct the string literal in pieces to prevent the source from
- getting matched. Store it in a pointer rather than an array
- because some compilers will just produce instructions to fill the
- array rather than assigning a pointer to a static array. */
-char const* info_platform = "INFO" ":" "platform[" PLATFORM_ID "]";
-char const* info_arch = "INFO" ":" "arch[" ARCHITECTURE_ID "]";
-
-
-
-
-#if defined(_MSC_VER) && defined(_MSVC_LANG)
-#define CXX_STD _MSVC_LANG
-#else
-#define CXX_STD __cplusplus
-#endif
-
-const char* info_language_dialect_default = "INFO" ":" "dialect_default["
-#if CXX_STD > 201402L
- "17"
-#elif CXX_STD >= 201402L
- "14"
-#elif CXX_STD >= 201103L
- "11"
-#else
- "98"
-#endif
-"]";
-
-/*--------------------------------------------------------------------------*/
-
-int main(int argc, char* argv[])
-{
- int require = 0;
- require += info_compiler[argc];
- require += info_platform[argc];
-#ifdef COMPILER_VERSION_MAJOR
- require += info_version[argc];
-#endif
-#ifdef COMPILER_VERSION_INTERNAL
- require += info_version_internal[argc];
-#endif
-#ifdef SIMULATE_ID
- require += info_simulate[argc];
-#endif
-#ifdef SIMULATE_VERSION_MAJOR
- require += info_simulate_version[argc];
-#endif
-#if defined(__CRAYXE) || defined(__CRAYXC)
- require += info_cray[argc];
-#endif
- require += info_language_dialect_default[argc];
- (void)argv;
- return require;
-}
diff --git a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdCXX/a.out b/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdCXX/a.out
deleted file mode 100755
index 71c2ceda86a2fae89d661d131b89af3990e37838..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdCXX/a.out and /dev/null differ
diff --git a/controller_test_drone_modeling/build/CMakeFiles/CMakeDirectoryInformation.cmake b/controller_test_drone_modeling/build/CMakeFiles/CMakeDirectoryInformation.cmake
deleted file mode 100644
index 70cde40ccb0dbe1225c7de672ef9f5d5a8dc14aa..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/CMakeDirectoryInformation.cmake
+++ /dev/null
@@ -1,16 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# Relative path conversion top directories.
-set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling")
-set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build")
-
-# Force unix paths in dependencies.
-set(CMAKE_FORCE_UNIX_PATHS 1)
-
-
-# The C and CXX include file regular expressions for this directory.
-set(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
-set(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
-set(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
-set(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})
diff --git a/controller_test_drone_modeling/build/CMakeFiles/CMakeError.log b/controller_test_drone_modeling/build/CMakeFiles/CMakeError.log
deleted file mode 100644
index a0749bdfed19ffda4f749ada754f01ac3e2b0da8..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/CMakeError.log
+++ /dev/null
@@ -1,55 +0,0 @@
-Determining if the pthread_create exist failed with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_35e00/fast"
-/usr/bin/make -f CMakeFiles/cmTC_35e00.dir/build.make CMakeFiles/cmTC_35e00.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_35e00.dir/CheckSymbolExists.c.o
-/usr/bin/cc -o CMakeFiles/cmTC_35e00.dir/CheckSymbolExists.c.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c
-Linking C executable cmTC_35e00
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_35e00.dir/link.txt --verbose=1
-/usr/bin/cc -rdynamic CMakeFiles/cmTC_35e00.dir/CheckSymbolExists.c.o -o cmTC_35e00
-CMakeFiles/cmTC_35e00.dir/CheckSymbolExists.c.o: In function `main':
-CheckSymbolExists.c:(.text+0x1b): undefined reference to `pthread_create'
-collect2: error: ld returned 1 exit status
-CMakeFiles/cmTC_35e00.dir/build.make:97: recipe for target 'cmTC_35e00' failed
-make[1]: *** [cmTC_35e00] Error 1
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Makefile:126: recipe for target 'cmTC_35e00/fast' failed
-make: *** [cmTC_35e00/fast] Error 2
-
-File /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp/CheckSymbolExists.c:
-/* */
-#include <pthread.h>
-
-int main(int argc, char** argv)
-{
- (void)argv;
-#ifndef pthread_create
- return ((int*)(&pthread_create))[argc];
-#else
- (void)argc;
- return 0;
-#endif
-}
-
-Determining if the function pthread_create exists in the pthreads failed with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_62770/fast"
-/usr/bin/make -f CMakeFiles/cmTC_62770.dir/build.make CMakeFiles/cmTC_62770.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_62770.dir/CheckFunctionExists.c.o
-/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTC_62770.dir/CheckFunctionExists.c.o -c /usr/share/cmake-3.10/Modules/CheckFunctionExists.c
-Linking C executable cmTC_62770
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_62770.dir/link.txt --verbose=1
-/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -rdynamic CMakeFiles/cmTC_62770.dir/CheckFunctionExists.c.o -o cmTC_62770 -lpthreads
-/usr/bin/ld: cannot find -lpthreads
-collect2: error: ld returned 1 exit status
-CMakeFiles/cmTC_62770.dir/build.make:97: recipe for target 'cmTC_62770' failed
-make[1]: *** [cmTC_62770] Error 1
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Makefile:126: recipe for target 'cmTC_62770/fast' failed
-make: *** [cmTC_62770/fast] Error 2
-
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/CMakeOutput.log b/controller_test_drone_modeling/build/CMakeFiles/CMakeOutput.log
deleted file mode 100644
index 270746ec6c12bf43292060970eceeca9b0b4ebc4..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/CMakeOutput.log
+++ /dev/null
@@ -1,661 +0,0 @@
-The system is: Linux - 5.4.0-105-generic - x86_64
-Compiling the C compiler identification source file "CMakeCCompilerId.c" succeeded.
-Compiler: /usr/bin/cc
-Build flags:
-Id flags:
-
-The output was:
-0
-
-
-Compilation of the C compiler identification source "CMakeCCompilerId.c" produced "a.out"
-
-The C compiler identification is GNU, found in "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdC/a.out"
-
-Compiling the CXX compiler identification source file "CMakeCXXCompilerId.cpp" succeeded.
-Compiler: /usr/bin/c++
-Build flags:
-Id flags:
-
-The output was:
-0
-
-
-Compilation of the CXX compiler identification source "CMakeCXXCompilerId.cpp" produced "a.out"
-
-The CXX compiler identification is GNU, found in "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/3.10.2/CompilerIdCXX/a.out"
-
-Determining if the C compiler works passed with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_8b82e/fast"
-/usr/bin/make -f CMakeFiles/cmTC_8b82e.dir/build.make CMakeFiles/cmTC_8b82e.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_8b82e.dir/testCCompiler.c.o
-/usr/bin/cc -o CMakeFiles/cmTC_8b82e.dir/testCCompiler.c.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp/testCCompiler.c
-Linking C executable cmTC_8b82e
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_8b82e.dir/link.txt --verbose=1
-/usr/bin/cc -rdynamic CMakeFiles/cmTC_8b82e.dir/testCCompiler.c.o -o cmTC_8b82e
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
-Detecting C compiler ABI info compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_c55ce/fast"
-/usr/bin/make -f CMakeFiles/cmTC_c55ce.dir/build.make CMakeFiles/cmTC_c55ce.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o
-/usr/bin/cc -o CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-3.10/Modules/CMakeCCompilerABI.c
-Linking C executable cmTC_c55ce
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_c55ce.dir/link.txt --verbose=1
-/usr/bin/cc -v -rdynamic CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o -o cmTC_c55ce
-Using built-in specs.
-COLLECT_GCC=/usr/bin/cc
-COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper
-OFFLOAD_TARGET_NAMES=nvptx-none
-OFFLOAD_TARGET_DEFAULT=1
-Target: x86_64-linux-gnu
-Configured with: ../src/configure -v --with-pkgversion='Ubuntu 7.5.0-3ubuntu1~18.04' --with-bugurl=file:///usr/share/doc/gcc-7/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++ --prefix=/usr --with-gcc-major-version-only --program-suffix=-7 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-bootstrap --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-libmpx --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu
-Thread model: posix
-gcc version 7.5.0 (Ubuntu 7.5.0-3ubuntu1~18.04)
-COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/
-LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../:/lib/:/usr/lib/
-COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_c55ce' '-mtune=generic' '-march=x86-64'
- /usr/lib/gcc/x86_64-linux-gnu/7/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/7/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper -plugin-opt=-fresolution=/tmp/ccW9fIMm.res -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_c55ce /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/7/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/7 -L/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/7/../../.. CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o -lgcc --push-state --as-needed -lgcc_s --pop-state -lc -lgcc --push-state --as-needed -lgcc_s --pop-state /usr/lib/gcc/x86_64-linux-gnu/7/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crtn.o
-COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_c55ce' '-mtune=generic' '-march=x86-64'
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
-Parsed C implicit link information from above output:
- link line regex: [^( *|.*[/\])(ld|CMAKE_LINK_STARTFILE-NOTFOUND|([^/\]+-)?ld|collect2)[^/\]*( |$)]
- ignore line: [Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp]
- ignore line: []
- ignore line: [Run Build Command:"/usr/bin/make" "cmTC_c55ce/fast"]
- ignore line: [/usr/bin/make -f CMakeFiles/cmTC_c55ce.dir/build.make CMakeFiles/cmTC_c55ce.dir/build]
- ignore line: [make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp']
- ignore line: [Building C object CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o]
- ignore line: [/usr/bin/cc -o CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o -c /usr/share/cmake-3.10/Modules/CMakeCCompilerABI.c]
- ignore line: [Linking C executable cmTC_c55ce]
- ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_c55ce.dir/link.txt --verbose=1]
- ignore line: [/usr/bin/cc -v -rdynamic CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o -o cmTC_c55ce ]
- ignore line: [Using built-in specs.]
- ignore line: [COLLECT_GCC=/usr/bin/cc]
- ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper]
- ignore line: [OFFLOAD_TARGET_NAMES=nvptx-none]
- ignore line: [OFFLOAD_TARGET_DEFAULT=1]
- ignore line: [Target: x86_64-linux-gnu]
- ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 7.5.0-3ubuntu1~18.04' --with-bugurl=file:///usr/share/doc/gcc-7/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++ --prefix=/usr --with-gcc-major-version-only --program-suffix=-7 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-bootstrap --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-libmpx --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu]
- ignore line: [Thread model: posix]
- ignore line: [gcc version 7.5.0 (Ubuntu 7.5.0-3ubuntu1~18.04) ]
- ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/]
- ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../:/lib/:/usr/lib/]
- ignore line: [COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_c55ce' '-mtune=generic' '-march=x86-64']
- link line: [ /usr/lib/gcc/x86_64-linux-gnu/7/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/7/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper -plugin-opt=-fresolution=/tmp/ccW9fIMm.res -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lgcc_s --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_c55ce /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/7/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/7 -L/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/7/../../.. CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o -lgcc --push-state --as-needed -lgcc_s --pop-state -lc -lgcc --push-state --as-needed -lgcc_s --pop-state /usr/lib/gcc/x86_64-linux-gnu/7/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crtn.o]
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/collect2] ==> ignore
- arg [-plugin] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/liblto_plugin.so] ==> ignore
- arg [-plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper] ==> ignore
- arg [-plugin-opt=-fresolution=/tmp/ccW9fIMm.res] ==> ignore
- arg [-plugin-opt=-pass-through=-lgcc] ==> ignore
- arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore
- arg [-plugin-opt=-pass-through=-lc] ==> ignore
- arg [-plugin-opt=-pass-through=-lgcc] ==> ignore
- arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore
- arg [--build-id] ==> ignore
- arg [--eh-frame-hdr] ==> ignore
- arg [-m] ==> ignore
- arg [elf_x86_64] ==> ignore
- arg [--hash-style=gnu] ==> ignore
- arg [--as-needed] ==> ignore
- arg [-export-dynamic] ==> ignore
- arg [-dynamic-linker] ==> ignore
- arg [/lib64/ld-linux-x86-64.so.2] ==> ignore
- arg [-pie] ==> ignore
- arg [-znow] ==> ignore
- arg [-zrelro] ==> ignore
- arg [-o] ==> ignore
- arg [cmTC_c55ce] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/Scrt1.o] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crti.o] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/crtbeginS.o] ==> ignore
- arg [-L/usr/lib/gcc/x86_64-linux-gnu/7] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/7]
- arg [-L/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu]
- arg [-L/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib]
- arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu]
- arg [-L/lib/../lib] ==> dir [/lib/../lib]
- arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu]
- arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib]
- arg [-L/usr/lib/gcc/x86_64-linux-gnu/7/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../..]
- arg [CMakeFiles/cmTC_c55ce.dir/CMakeCCompilerABI.c.o] ==> ignore
- arg [-lgcc] ==> lib [gcc]
- arg [--push-state] ==> ignore
- arg [--as-needed] ==> ignore
- arg [-lgcc_s] ==> lib [gcc_s]
- arg [--pop-state] ==> ignore
- arg [-lc] ==> lib [c]
- arg [-lgcc] ==> lib [gcc]
- arg [--push-state] ==> ignore
- arg [--as-needed] ==> ignore
- arg [-lgcc_s] ==> lib [gcc_s]
- arg [--pop-state] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/crtendS.o] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crtn.o] ==> ignore
- collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/7] ==> [/usr/lib/gcc/x86_64-linux-gnu/7]
- collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
- collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib] ==> [/usr/lib]
- collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu]
- collapse library dir [/lib/../lib] ==> [/lib]
- collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
- collapse library dir [/usr/lib/../lib] ==> [/usr/lib]
- collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../..] ==> [/usr/lib]
- implicit libs: [gcc;gcc_s;c;gcc;gcc_s]
- implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib]
- implicit fwks: []
-
-
-
-
-Detecting C [-std=c11] compiler features compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_09416/fast"
-/usr/bin/make -f CMakeFiles/cmTC_09416.dir/build.make CMakeFiles/cmTC_09416.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_09416.dir/feature_tests.c.o
-/usr/bin/cc -std=c11 -o CMakeFiles/cmTC_09416.dir/feature_tests.c.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/feature_tests.c
-Linking C executable cmTC_09416
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_09416.dir/link.txt --verbose=1
-/usr/bin/cc -rdynamic CMakeFiles/cmTC_09416.dir/feature_tests.c.o -o cmTC_09416
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
- Feature record: C_FEATURE:1c_function_prototypes
- Feature record: C_FEATURE:1c_restrict
- Feature record: C_FEATURE:1c_static_assert
- Feature record: C_FEATURE:1c_variadic_macros
-
-
-Detecting C [-std=c99] compiler features compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_9dc82/fast"
-/usr/bin/make -f CMakeFiles/cmTC_9dc82.dir/build.make CMakeFiles/cmTC_9dc82.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_9dc82.dir/feature_tests.c.o
-/usr/bin/cc -std=c99 -o CMakeFiles/cmTC_9dc82.dir/feature_tests.c.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/feature_tests.c
-Linking C executable cmTC_9dc82
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_9dc82.dir/link.txt --verbose=1
-/usr/bin/cc -rdynamic CMakeFiles/cmTC_9dc82.dir/feature_tests.c.o -o cmTC_9dc82
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
- Feature record: C_FEATURE:1c_function_prototypes
- Feature record: C_FEATURE:1c_restrict
- Feature record: C_FEATURE:0c_static_assert
- Feature record: C_FEATURE:1c_variadic_macros
-
-
-Detecting C [-std=c90] compiler features compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_0606c/fast"
-/usr/bin/make -f CMakeFiles/cmTC_0606c.dir/build.make CMakeFiles/cmTC_0606c.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_0606c.dir/feature_tests.c.o
-/usr/bin/cc -std=c90 -o CMakeFiles/cmTC_0606c.dir/feature_tests.c.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/feature_tests.c
-Linking C executable cmTC_0606c
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_0606c.dir/link.txt --verbose=1
-/usr/bin/cc -rdynamic CMakeFiles/cmTC_0606c.dir/feature_tests.c.o -o cmTC_0606c
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
- Feature record: C_FEATURE:1c_function_prototypes
- Feature record: C_FEATURE:0c_restrict
- Feature record: C_FEATURE:0c_static_assert
- Feature record: C_FEATURE:0c_variadic_macros
-Determining if the CXX compiler works passed with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_c77b2/fast"
-/usr/bin/make -f CMakeFiles/cmTC_c77b2.dir/build.make CMakeFiles/cmTC_c77b2.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building CXX object CMakeFiles/cmTC_c77b2.dir/testCXXCompiler.cxx.o
-/usr/bin/c++ -o CMakeFiles/cmTC_c77b2.dir/testCXXCompiler.cxx.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp/testCXXCompiler.cxx
-Linking CXX executable cmTC_c77b2
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_c77b2.dir/link.txt --verbose=1
-/usr/bin/c++ -rdynamic CMakeFiles/cmTC_c77b2.dir/testCXXCompiler.cxx.o -o cmTC_c77b2
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
-Detecting CXX compiler ABI info compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_e5562/fast"
-/usr/bin/make -f CMakeFiles/cmTC_e5562.dir/build.make CMakeFiles/cmTC_e5562.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building CXX object CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o
-/usr/bin/c++ -o CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-3.10/Modules/CMakeCXXCompilerABI.cpp
-Linking CXX executable cmTC_e5562
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_e5562.dir/link.txt --verbose=1
-/usr/bin/c++ -v -rdynamic CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o -o cmTC_e5562
-Using built-in specs.
-COLLECT_GCC=/usr/bin/c++
-COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper
-OFFLOAD_TARGET_NAMES=nvptx-none
-OFFLOAD_TARGET_DEFAULT=1
-Target: x86_64-linux-gnu
-Configured with: ../src/configure -v --with-pkgversion='Ubuntu 7.5.0-3ubuntu1~18.04' --with-bugurl=file:///usr/share/doc/gcc-7/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++ --prefix=/usr --with-gcc-major-version-only --program-suffix=-7 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-bootstrap --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-libmpx --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu
-Thread model: posix
-gcc version 7.5.0 (Ubuntu 7.5.0-3ubuntu1~18.04)
-COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/
-LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../:/lib/:/usr/lib/
-COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_e5562' '-shared-libgcc' '-mtune=generic' '-march=x86-64'
- /usr/lib/gcc/x86_64-linux-gnu/7/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/7/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper -plugin-opt=-fresolution=/tmp/ccqOaE5G.res -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_e5562 /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/7/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/7 -L/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/7/../../.. CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/7/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crtn.o
-COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_e5562' '-shared-libgcc' '-mtune=generic' '-march=x86-64'
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
-Parsed CXX implicit link information from above output:
- link line regex: [^( *|.*[/\])(ld|CMAKE_LINK_STARTFILE-NOTFOUND|([^/\]+-)?ld|collect2)[^/\]*( |$)]
- ignore line: [Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp]
- ignore line: []
- ignore line: [Run Build Command:"/usr/bin/make" "cmTC_e5562/fast"]
- ignore line: [/usr/bin/make -f CMakeFiles/cmTC_e5562.dir/build.make CMakeFiles/cmTC_e5562.dir/build]
- ignore line: [make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp']
- ignore line: [Building CXX object CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o]
- ignore line: [/usr/bin/c++ -o CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o -c /usr/share/cmake-3.10/Modules/CMakeCXXCompilerABI.cpp]
- ignore line: [Linking CXX executable cmTC_e5562]
- ignore line: [/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_e5562.dir/link.txt --verbose=1]
- ignore line: [/usr/bin/c++ -v -rdynamic CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o -o cmTC_e5562 ]
- ignore line: [Using built-in specs.]
- ignore line: [COLLECT_GCC=/usr/bin/c++]
- ignore line: [COLLECT_LTO_WRAPPER=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper]
- ignore line: [OFFLOAD_TARGET_NAMES=nvptx-none]
- ignore line: [OFFLOAD_TARGET_DEFAULT=1]
- ignore line: [Target: x86_64-linux-gnu]
- ignore line: [Configured with: ../src/configure -v --with-pkgversion='Ubuntu 7.5.0-3ubuntu1~18.04' --with-bugurl=file:///usr/share/doc/gcc-7/README.Bugs --enable-languages=c,ada,c++,go,brig,d,fortran,objc,obj-c++ --prefix=/usr --with-gcc-major-version-only --program-suffix=-7 --program-prefix=x86_64-linux-gnu- --enable-shared --enable-linker-build-id --libexecdir=/usr/lib --without-included-gettext --enable-threads=posix --libdir=/usr/lib --enable-nls --enable-bootstrap --enable-clocale=gnu --enable-libstdcxx-debug --enable-libstdcxx-time=yes --with-default-libstdcxx-abi=new --enable-gnu-unique-object --disable-vtable-verify --enable-libmpx --enable-plugin --enable-default-pie --with-system-zlib --with-target-system-zlib --enable-objc-gc=auto --enable-multiarch --disable-werror --with-arch-32=i686 --with-abi=m64 --with-multilib-list=m32,m64,mx32 --enable-multilib --with-tune=generic --enable-offload-targets=nvptx-none --without-cuda-driver --enable-checking=release --build=x86_64-linux-gnu --host=x86_64-linux-gnu --target=x86_64-linux-gnu]
- ignore line: [Thread model: posix]
- ignore line: [gcc version 7.5.0 (Ubuntu 7.5.0-3ubuntu1~18.04) ]
- ignore line: [COMPILER_PATH=/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/]
- ignore line: [LIBRARY_PATH=/usr/lib/gcc/x86_64-linux-gnu/7/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib/:/lib/x86_64-linux-gnu/:/lib/../lib/:/usr/lib/x86_64-linux-gnu/:/usr/lib/../lib/:/usr/lib/gcc/x86_64-linux-gnu/7/../../../:/lib/:/usr/lib/]
- ignore line: [COLLECT_GCC_OPTIONS='-v' '-rdynamic' '-o' 'cmTC_e5562' '-shared-libgcc' '-mtune=generic' '-march=x86-64']
- link line: [ /usr/lib/gcc/x86_64-linux-gnu/7/collect2 -plugin /usr/lib/gcc/x86_64-linux-gnu/7/liblto_plugin.so -plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper -plugin-opt=-fresolution=/tmp/ccqOaE5G.res -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc -plugin-opt=-pass-through=-lc -plugin-opt=-pass-through=-lgcc_s -plugin-opt=-pass-through=-lgcc --build-id --eh-frame-hdr -m elf_x86_64 --hash-style=gnu --as-needed -export-dynamic -dynamic-linker /lib64/ld-linux-x86-64.so.2 -pie -z now -z relro -o cmTC_e5562 /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/Scrt1.o /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crti.o /usr/lib/gcc/x86_64-linux-gnu/7/crtbeginS.o -L/usr/lib/gcc/x86_64-linux-gnu/7 -L/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu -L/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib -L/lib/x86_64-linux-gnu -L/lib/../lib -L/usr/lib/x86_64-linux-gnu -L/usr/lib/../lib -L/usr/lib/gcc/x86_64-linux-gnu/7/../../.. CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o -lstdc++ -lm -lgcc_s -lgcc -lc -lgcc_s -lgcc /usr/lib/gcc/x86_64-linux-gnu/7/crtendS.o /usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crtn.o]
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/collect2] ==> ignore
- arg [-plugin] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/liblto_plugin.so] ==> ignore
- arg [-plugin-opt=/usr/lib/gcc/x86_64-linux-gnu/7/lto-wrapper] ==> ignore
- arg [-plugin-opt=-fresolution=/tmp/ccqOaE5G.res] ==> ignore
- arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore
- arg [-plugin-opt=-pass-through=-lgcc] ==> ignore
- arg [-plugin-opt=-pass-through=-lc] ==> ignore
- arg [-plugin-opt=-pass-through=-lgcc_s] ==> ignore
- arg [-plugin-opt=-pass-through=-lgcc] ==> ignore
- arg [--build-id] ==> ignore
- arg [--eh-frame-hdr] ==> ignore
- arg [-m] ==> ignore
- arg [elf_x86_64] ==> ignore
- arg [--hash-style=gnu] ==> ignore
- arg [--as-needed] ==> ignore
- arg [-export-dynamic] ==> ignore
- arg [-dynamic-linker] ==> ignore
- arg [/lib64/ld-linux-x86-64.so.2] ==> ignore
- arg [-pie] ==> ignore
- arg [-znow] ==> ignore
- arg [-zrelro] ==> ignore
- arg [-o] ==> ignore
- arg [cmTC_e5562] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/Scrt1.o] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crti.o] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/crtbeginS.o] ==> ignore
- arg [-L/usr/lib/gcc/x86_64-linux-gnu/7] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/7]
- arg [-L/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu]
- arg [-L/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib]
- arg [-L/lib/x86_64-linux-gnu] ==> dir [/lib/x86_64-linux-gnu]
- arg [-L/lib/../lib] ==> dir [/lib/../lib]
- arg [-L/usr/lib/x86_64-linux-gnu] ==> dir [/usr/lib/x86_64-linux-gnu]
- arg [-L/usr/lib/../lib] ==> dir [/usr/lib/../lib]
- arg [-L/usr/lib/gcc/x86_64-linux-gnu/7/../../..] ==> dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../..]
- arg [CMakeFiles/cmTC_e5562.dir/CMakeCXXCompilerABI.cpp.o] ==> ignore
- arg [-lstdc++] ==> lib [stdc++]
- arg [-lm] ==> lib [m]
- arg [-lgcc_s] ==> lib [gcc_s]
- arg [-lgcc] ==> lib [gcc]
- arg [-lc] ==> lib [c]
- arg [-lgcc_s] ==> lib [gcc_s]
- arg [-lgcc] ==> lib [gcc]
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/crtendS.o] ==> ignore
- arg [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu/crtn.o] ==> ignore
- collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/7] ==> [/usr/lib/gcc/x86_64-linux-gnu/7]
- collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../../x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
- collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../../../lib] ==> [/usr/lib]
- collapse library dir [/lib/x86_64-linux-gnu] ==> [/lib/x86_64-linux-gnu]
- collapse library dir [/lib/../lib] ==> [/lib]
- collapse library dir [/usr/lib/x86_64-linux-gnu] ==> [/usr/lib/x86_64-linux-gnu]
- collapse library dir [/usr/lib/../lib] ==> [/usr/lib]
- collapse library dir [/usr/lib/gcc/x86_64-linux-gnu/7/../../..] ==> [/usr/lib]
- implicit libs: [stdc++;m;gcc_s;gcc;c;gcc_s;gcc]
- implicit dirs: [/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib]
- implicit fwks: []
-
-
-
-
-Detecting CXX [-std=c++1z] compiler features compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_a67a4/fast"
-/usr/bin/make -f CMakeFiles/cmTC_a67a4.dir/build.make CMakeFiles/cmTC_a67a4.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building CXX object CMakeFiles/cmTC_a67a4.dir/feature_tests.cxx.o
-/usr/bin/c++ -std=c++1z -o CMakeFiles/cmTC_a67a4.dir/feature_tests.cxx.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/feature_tests.cxx
-Linking CXX executable cmTC_a67a4
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_a67a4.dir/link.txt --verbose=1
-/usr/bin/c++ -rdynamic CMakeFiles/cmTC_a67a4.dir/feature_tests.cxx.o -o cmTC_a67a4
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
- Feature record: CXX_FEATURE:1cxx_aggregate_default_initializers
- Feature record: CXX_FEATURE:1cxx_alias_templates
- Feature record: CXX_FEATURE:1cxx_alignas
- Feature record: CXX_FEATURE:1cxx_alignof
- Feature record: CXX_FEATURE:1cxx_attributes
- Feature record: CXX_FEATURE:1cxx_attribute_deprecated
- Feature record: CXX_FEATURE:1cxx_auto_type
- Feature record: CXX_FEATURE:1cxx_binary_literals
- Feature record: CXX_FEATURE:1cxx_constexpr
- Feature record: CXX_FEATURE:1cxx_contextual_conversions
- Feature record: CXX_FEATURE:1cxx_decltype
- Feature record: CXX_FEATURE:1cxx_decltype_auto
- Feature record: CXX_FEATURE:1cxx_decltype_incomplete_return_types
- Feature record: CXX_FEATURE:1cxx_default_function_template_args
- Feature record: CXX_FEATURE:1cxx_defaulted_functions
- Feature record: CXX_FEATURE:1cxx_defaulted_move_initializers
- Feature record: CXX_FEATURE:1cxx_delegating_constructors
- Feature record: CXX_FEATURE:1cxx_deleted_functions
- Feature record: CXX_FEATURE:1cxx_digit_separators
- Feature record: CXX_FEATURE:1cxx_enum_forward_declarations
- Feature record: CXX_FEATURE:1cxx_explicit_conversions
- Feature record: CXX_FEATURE:1cxx_extended_friend_declarations
- Feature record: CXX_FEATURE:1cxx_extern_templates
- Feature record: CXX_FEATURE:1cxx_final
- Feature record: CXX_FEATURE:1cxx_func_identifier
- Feature record: CXX_FEATURE:1cxx_generalized_initializers
- Feature record: CXX_FEATURE:1cxx_generic_lambdas
- Feature record: CXX_FEATURE:1cxx_inheriting_constructors
- Feature record: CXX_FEATURE:1cxx_inline_namespaces
- Feature record: CXX_FEATURE:1cxx_lambdas
- Feature record: CXX_FEATURE:1cxx_lambda_init_captures
- Feature record: CXX_FEATURE:1cxx_local_type_template_args
- Feature record: CXX_FEATURE:1cxx_long_long_type
- Feature record: CXX_FEATURE:1cxx_noexcept
- Feature record: CXX_FEATURE:1cxx_nonstatic_member_init
- Feature record: CXX_FEATURE:1cxx_nullptr
- Feature record: CXX_FEATURE:1cxx_override
- Feature record: CXX_FEATURE:1cxx_range_for
- Feature record: CXX_FEATURE:1cxx_raw_string_literals
- Feature record: CXX_FEATURE:1cxx_reference_qualified_functions
- Feature record: CXX_FEATURE:1cxx_relaxed_constexpr
- Feature record: CXX_FEATURE:1cxx_return_type_deduction
- Feature record: CXX_FEATURE:1cxx_right_angle_brackets
- Feature record: CXX_FEATURE:1cxx_rvalue_references
- Feature record: CXX_FEATURE:1cxx_sizeof_member
- Feature record: CXX_FEATURE:1cxx_static_assert
- Feature record: CXX_FEATURE:1cxx_strong_enums
- Feature record: CXX_FEATURE:1cxx_template_template_parameters
- Feature record: CXX_FEATURE:1cxx_thread_local
- Feature record: CXX_FEATURE:1cxx_trailing_return_types
- Feature record: CXX_FEATURE:1cxx_unicode_literals
- Feature record: CXX_FEATURE:1cxx_uniform_initialization
- Feature record: CXX_FEATURE:1cxx_unrestricted_unions
- Feature record: CXX_FEATURE:1cxx_user_literals
- Feature record: CXX_FEATURE:1cxx_variable_templates
- Feature record: CXX_FEATURE:1cxx_variadic_macros
- Feature record: CXX_FEATURE:1cxx_variadic_templates
-
-
-Detecting CXX [-std=c++14] compiler features compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_98c3b/fast"
-/usr/bin/make -f CMakeFiles/cmTC_98c3b.dir/build.make CMakeFiles/cmTC_98c3b.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building CXX object CMakeFiles/cmTC_98c3b.dir/feature_tests.cxx.o
-/usr/bin/c++ -std=c++14 -o CMakeFiles/cmTC_98c3b.dir/feature_tests.cxx.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/feature_tests.cxx
-Linking CXX executable cmTC_98c3b
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_98c3b.dir/link.txt --verbose=1
-/usr/bin/c++ -rdynamic CMakeFiles/cmTC_98c3b.dir/feature_tests.cxx.o -o cmTC_98c3b
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
- Feature record: CXX_FEATURE:1cxx_aggregate_default_initializers
- Feature record: CXX_FEATURE:1cxx_alias_templates
- Feature record: CXX_FEATURE:1cxx_alignas
- Feature record: CXX_FEATURE:1cxx_alignof
- Feature record: CXX_FEATURE:1cxx_attributes
- Feature record: CXX_FEATURE:1cxx_attribute_deprecated
- Feature record: CXX_FEATURE:1cxx_auto_type
- Feature record: CXX_FEATURE:1cxx_binary_literals
- Feature record: CXX_FEATURE:1cxx_constexpr
- Feature record: CXX_FEATURE:1cxx_contextual_conversions
- Feature record: CXX_FEATURE:1cxx_decltype
- Feature record: CXX_FEATURE:1cxx_decltype_auto
- Feature record: CXX_FEATURE:1cxx_decltype_incomplete_return_types
- Feature record: CXX_FEATURE:1cxx_default_function_template_args
- Feature record: CXX_FEATURE:1cxx_defaulted_functions
- Feature record: CXX_FEATURE:1cxx_defaulted_move_initializers
- Feature record: CXX_FEATURE:1cxx_delegating_constructors
- Feature record: CXX_FEATURE:1cxx_deleted_functions
- Feature record: CXX_FEATURE:1cxx_digit_separators
- Feature record: CXX_FEATURE:1cxx_enum_forward_declarations
- Feature record: CXX_FEATURE:1cxx_explicit_conversions
- Feature record: CXX_FEATURE:1cxx_extended_friend_declarations
- Feature record: CXX_FEATURE:1cxx_extern_templates
- Feature record: CXX_FEATURE:1cxx_final
- Feature record: CXX_FEATURE:1cxx_func_identifier
- Feature record: CXX_FEATURE:1cxx_generalized_initializers
- Feature record: CXX_FEATURE:1cxx_generic_lambdas
- Feature record: CXX_FEATURE:1cxx_inheriting_constructors
- Feature record: CXX_FEATURE:1cxx_inline_namespaces
- Feature record: CXX_FEATURE:1cxx_lambdas
- Feature record: CXX_FEATURE:1cxx_lambda_init_captures
- Feature record: CXX_FEATURE:1cxx_local_type_template_args
- Feature record: CXX_FEATURE:1cxx_long_long_type
- Feature record: CXX_FEATURE:1cxx_noexcept
- Feature record: CXX_FEATURE:1cxx_nonstatic_member_init
- Feature record: CXX_FEATURE:1cxx_nullptr
- Feature record: CXX_FEATURE:1cxx_override
- Feature record: CXX_FEATURE:1cxx_range_for
- Feature record: CXX_FEATURE:1cxx_raw_string_literals
- Feature record: CXX_FEATURE:1cxx_reference_qualified_functions
- Feature record: CXX_FEATURE:1cxx_relaxed_constexpr
- Feature record: CXX_FEATURE:1cxx_return_type_deduction
- Feature record: CXX_FEATURE:1cxx_right_angle_brackets
- Feature record: CXX_FEATURE:1cxx_rvalue_references
- Feature record: CXX_FEATURE:1cxx_sizeof_member
- Feature record: CXX_FEATURE:1cxx_static_assert
- Feature record: CXX_FEATURE:1cxx_strong_enums
- Feature record: CXX_FEATURE:1cxx_template_template_parameters
- Feature record: CXX_FEATURE:1cxx_thread_local
- Feature record: CXX_FEATURE:1cxx_trailing_return_types
- Feature record: CXX_FEATURE:1cxx_unicode_literals
- Feature record: CXX_FEATURE:1cxx_uniform_initialization
- Feature record: CXX_FEATURE:1cxx_unrestricted_unions
- Feature record: CXX_FEATURE:1cxx_user_literals
- Feature record: CXX_FEATURE:1cxx_variable_templates
- Feature record: CXX_FEATURE:1cxx_variadic_macros
- Feature record: CXX_FEATURE:1cxx_variadic_templates
-
-
-Detecting CXX [-std=c++11] compiler features compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_3fab2/fast"
-/usr/bin/make -f CMakeFiles/cmTC_3fab2.dir/build.make CMakeFiles/cmTC_3fab2.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building CXX object CMakeFiles/cmTC_3fab2.dir/feature_tests.cxx.o
-/usr/bin/c++ -std=c++11 -o CMakeFiles/cmTC_3fab2.dir/feature_tests.cxx.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/feature_tests.cxx
-Linking CXX executable cmTC_3fab2
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_3fab2.dir/link.txt --verbose=1
-/usr/bin/c++ -rdynamic CMakeFiles/cmTC_3fab2.dir/feature_tests.cxx.o -o cmTC_3fab2
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
- Feature record: CXX_FEATURE:0cxx_aggregate_default_initializers
- Feature record: CXX_FEATURE:1cxx_alias_templates
- Feature record: CXX_FEATURE:1cxx_alignas
- Feature record: CXX_FEATURE:1cxx_alignof
- Feature record: CXX_FEATURE:1cxx_attributes
- Feature record: CXX_FEATURE:0cxx_attribute_deprecated
- Feature record: CXX_FEATURE:1cxx_auto_type
- Feature record: CXX_FEATURE:0cxx_binary_literals
- Feature record: CXX_FEATURE:1cxx_constexpr
- Feature record: CXX_FEATURE:0cxx_contextual_conversions
- Feature record: CXX_FEATURE:1cxx_decltype
- Feature record: CXX_FEATURE:0cxx_decltype_auto
- Feature record: CXX_FEATURE:1cxx_decltype_incomplete_return_types
- Feature record: CXX_FEATURE:1cxx_default_function_template_args
- Feature record: CXX_FEATURE:1cxx_defaulted_functions
- Feature record: CXX_FEATURE:1cxx_defaulted_move_initializers
- Feature record: CXX_FEATURE:1cxx_delegating_constructors
- Feature record: CXX_FEATURE:1cxx_deleted_functions
- Feature record: CXX_FEATURE:0cxx_digit_separators
- Feature record: CXX_FEATURE:1cxx_enum_forward_declarations
- Feature record: CXX_FEATURE:1cxx_explicit_conversions
- Feature record: CXX_FEATURE:1cxx_extended_friend_declarations
- Feature record: CXX_FEATURE:1cxx_extern_templates
- Feature record: CXX_FEATURE:1cxx_final
- Feature record: CXX_FEATURE:1cxx_func_identifier
- Feature record: CXX_FEATURE:1cxx_generalized_initializers
- Feature record: CXX_FEATURE:0cxx_generic_lambdas
- Feature record: CXX_FEATURE:1cxx_inheriting_constructors
- Feature record: CXX_FEATURE:1cxx_inline_namespaces
- Feature record: CXX_FEATURE:1cxx_lambdas
- Feature record: CXX_FEATURE:0cxx_lambda_init_captures
- Feature record: CXX_FEATURE:1cxx_local_type_template_args
- Feature record: CXX_FEATURE:1cxx_long_long_type
- Feature record: CXX_FEATURE:1cxx_noexcept
- Feature record: CXX_FEATURE:1cxx_nonstatic_member_init
- Feature record: CXX_FEATURE:1cxx_nullptr
- Feature record: CXX_FEATURE:1cxx_override
- Feature record: CXX_FEATURE:1cxx_range_for
- Feature record: CXX_FEATURE:1cxx_raw_string_literals
- Feature record: CXX_FEATURE:1cxx_reference_qualified_functions
- Feature record: CXX_FEATURE:0cxx_relaxed_constexpr
- Feature record: CXX_FEATURE:0cxx_return_type_deduction
- Feature record: CXX_FEATURE:1cxx_right_angle_brackets
- Feature record: CXX_FEATURE:1cxx_rvalue_references
- Feature record: CXX_FEATURE:1cxx_sizeof_member
- Feature record: CXX_FEATURE:1cxx_static_assert
- Feature record: CXX_FEATURE:1cxx_strong_enums
- Feature record: CXX_FEATURE:1cxx_template_template_parameters
- Feature record: CXX_FEATURE:1cxx_thread_local
- Feature record: CXX_FEATURE:1cxx_trailing_return_types
- Feature record: CXX_FEATURE:1cxx_unicode_literals
- Feature record: CXX_FEATURE:1cxx_uniform_initialization
- Feature record: CXX_FEATURE:1cxx_unrestricted_unions
- Feature record: CXX_FEATURE:1cxx_user_literals
- Feature record: CXX_FEATURE:0cxx_variable_templates
- Feature record: CXX_FEATURE:1cxx_variadic_macros
- Feature record: CXX_FEATURE:1cxx_variadic_templates
-
-
-Detecting CXX [-std=c++98] compiler features compiled with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_1f049/fast"
-/usr/bin/make -f CMakeFiles/cmTC_1f049.dir/build.make CMakeFiles/cmTC_1f049.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building CXX object CMakeFiles/cmTC_1f049.dir/feature_tests.cxx.o
-/usr/bin/c++ -std=c++98 -o CMakeFiles/cmTC_1f049.dir/feature_tests.cxx.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/feature_tests.cxx
-Linking CXX executable cmTC_1f049
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_1f049.dir/link.txt --verbose=1
-/usr/bin/c++ -rdynamic CMakeFiles/cmTC_1f049.dir/feature_tests.cxx.o -o cmTC_1f049
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
- Feature record: CXX_FEATURE:0cxx_aggregate_default_initializers
- Feature record: CXX_FEATURE:0cxx_alias_templates
- Feature record: CXX_FEATURE:0cxx_alignas
- Feature record: CXX_FEATURE:0cxx_alignof
- Feature record: CXX_FEATURE:0cxx_attributes
- Feature record: CXX_FEATURE:0cxx_attribute_deprecated
- Feature record: CXX_FEATURE:0cxx_auto_type
- Feature record: CXX_FEATURE:0cxx_binary_literals
- Feature record: CXX_FEATURE:0cxx_constexpr
- Feature record: CXX_FEATURE:0cxx_contextual_conversions
- Feature record: CXX_FEATURE:0cxx_decltype
- Feature record: CXX_FEATURE:0cxx_decltype_auto
- Feature record: CXX_FEATURE:0cxx_decltype_incomplete_return_types
- Feature record: CXX_FEATURE:0cxx_default_function_template_args
- Feature record: CXX_FEATURE:0cxx_defaulted_functions
- Feature record: CXX_FEATURE:0cxx_defaulted_move_initializers
- Feature record: CXX_FEATURE:0cxx_delegating_constructors
- Feature record: CXX_FEATURE:0cxx_deleted_functions
- Feature record: CXX_FEATURE:0cxx_digit_separators
- Feature record: CXX_FEATURE:0cxx_enum_forward_declarations
- Feature record: CXX_FEATURE:0cxx_explicit_conversions
- Feature record: CXX_FEATURE:0cxx_extended_friend_declarations
- Feature record: CXX_FEATURE:0cxx_extern_templates
- Feature record: CXX_FEATURE:0cxx_final
- Feature record: CXX_FEATURE:0cxx_func_identifier
- Feature record: CXX_FEATURE:0cxx_generalized_initializers
- Feature record: CXX_FEATURE:0cxx_generic_lambdas
- Feature record: CXX_FEATURE:0cxx_inheriting_constructors
- Feature record: CXX_FEATURE:0cxx_inline_namespaces
- Feature record: CXX_FEATURE:0cxx_lambdas
- Feature record: CXX_FEATURE:0cxx_lambda_init_captures
- Feature record: CXX_FEATURE:0cxx_local_type_template_args
- Feature record: CXX_FEATURE:0cxx_long_long_type
- Feature record: CXX_FEATURE:0cxx_noexcept
- Feature record: CXX_FEATURE:0cxx_nonstatic_member_init
- Feature record: CXX_FEATURE:0cxx_nullptr
- Feature record: CXX_FEATURE:0cxx_override
- Feature record: CXX_FEATURE:0cxx_range_for
- Feature record: CXX_FEATURE:0cxx_raw_string_literals
- Feature record: CXX_FEATURE:0cxx_reference_qualified_functions
- Feature record: CXX_FEATURE:0cxx_relaxed_constexpr
- Feature record: CXX_FEATURE:0cxx_return_type_deduction
- Feature record: CXX_FEATURE:0cxx_right_angle_brackets
- Feature record: CXX_FEATURE:0cxx_rvalue_references
- Feature record: CXX_FEATURE:0cxx_sizeof_member
- Feature record: CXX_FEATURE:0cxx_static_assert
- Feature record: CXX_FEATURE:0cxx_strong_enums
- Feature record: CXX_FEATURE:1cxx_template_template_parameters
- Feature record: CXX_FEATURE:0cxx_thread_local
- Feature record: CXX_FEATURE:0cxx_trailing_return_types
- Feature record: CXX_FEATURE:0cxx_unicode_literals
- Feature record: CXX_FEATURE:0cxx_uniform_initialization
- Feature record: CXX_FEATURE:0cxx_unrestricted_unions
- Feature record: CXX_FEATURE:0cxx_user_literals
- Feature record: CXX_FEATURE:0cxx_variable_templates
- Feature record: CXX_FEATURE:0cxx_variadic_macros
- Feature record: CXX_FEATURE:0cxx_variadic_templates
-Determining if the include file pthread.h exists passed with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_9d083/fast"
-/usr/bin/make -f CMakeFiles/cmTC_9d083.dir/build.make CMakeFiles/cmTC_9d083.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_9d083.dir/CheckIncludeFile.c.o
-/usr/bin/cc -o CMakeFiles/cmTC_9d083.dir/CheckIncludeFile.c.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp/CheckIncludeFile.c
-Linking C executable cmTC_9d083
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_9d083.dir/link.txt --verbose=1
-/usr/bin/cc -rdynamic CMakeFiles/cmTC_9d083.dir/CheckIncludeFile.c.o -o cmTC_9d083
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
-Determining if the function pthread_create exists in the pthread passed with the following output:
-Change Dir: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp
-
-Run Build Command:"/usr/bin/make" "cmTC_5316e/fast"
-/usr/bin/make -f CMakeFiles/cmTC_5316e.dir/build.make CMakeFiles/cmTC_5316e.dir/build
-make[1]: Entering directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-Building C object CMakeFiles/cmTC_5316e.dir/CheckFunctionExists.c.o
-/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -o CMakeFiles/cmTC_5316e.dir/CheckFunctionExists.c.o -c /usr/share/cmake-3.10/Modules/CheckFunctionExists.c
-Linking C executable cmTC_5316e
-/usr/bin/cmake -E cmake_link_script CMakeFiles/cmTC_5316e.dir/link.txt --verbose=1
-/usr/bin/cc -DCHECK_FUNCTION_EXISTS=pthread_create -rdynamic CMakeFiles/cmTC_5316e.dir/CheckFunctionExists.c.o -o cmTC_5316e -lpthread
-make[1]: Leaving directory '/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/CMakeTmp'
-
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/Makefile.cmake b/controller_test_drone_modeling/build/CMakeFiles/Makefile.cmake
deleted file mode 100644
index fbb43761347f378401643e9ad4e4c947bb1684c0..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/Makefile.cmake
+++ /dev/null
@@ -1,58 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# The generator used is:
-set(CMAKE_DEPENDS_GENERATOR "Unix Makefiles")
-
-# The top level Makefile was generated from the following files:
-set(CMAKE_MAKEFILE_DEPENDS
- "CMakeCache.txt"
- "../CMakeLists.txt"
- "CMakeFiles/3.10.2/CMakeCCompiler.cmake"
- "CMakeFiles/3.10.2/CMakeCXXCompiler.cmake"
- "CMakeFiles/3.10.2/CMakeSystem.cmake"
- "../crazyflie_cpp/CMake.cmake"
- "../crazyflie_cpp/CMakeLists.txt"
- "/usr/share/cmake-3.10/Modules/CMakeCInformation.cmake"
- "/usr/share/cmake-3.10/Modules/CMakeCXXInformation.cmake"
- "/usr/share/cmake-3.10/Modules/CMakeCommonLanguageInclude.cmake"
- "/usr/share/cmake-3.10/Modules/CMakeGenericSystem.cmake"
- "/usr/share/cmake-3.10/Modules/CMakeLanguageInformation.cmake"
- "/usr/share/cmake-3.10/Modules/CMakeSystemSpecificInformation.cmake"
- "/usr/share/cmake-3.10/Modules/CMakeSystemSpecificInitialize.cmake"
- "/usr/share/cmake-3.10/Modules/CheckIncludeFile.cmake"
- "/usr/share/cmake-3.10/Modules/CheckLibraryExists.cmake"
- "/usr/share/cmake-3.10/Modules/CheckSymbolExists.cmake"
- "/usr/share/cmake-3.10/Modules/Compiler/CMakeCommonCompilerMacros.cmake"
- "/usr/share/cmake-3.10/Modules/Compiler/GNU-C.cmake"
- "/usr/share/cmake-3.10/Modules/Compiler/GNU-CXX.cmake"
- "/usr/share/cmake-3.10/Modules/Compiler/GNU.cmake"
- "/usr/share/cmake-3.10/Modules/FindBoost.cmake"
- "/usr/share/cmake-3.10/Modules/FindPackageHandleStandardArgs.cmake"
- "/usr/share/cmake-3.10/Modules/FindPackageMessage.cmake"
- "/usr/share/cmake-3.10/Modules/FindThreads.cmake"
- "/usr/share/cmake-3.10/Modules/Platform/Linux-GNU-C.cmake"
- "/usr/share/cmake-3.10/Modules/Platform/Linux-GNU-CXX.cmake"
- "/usr/share/cmake-3.10/Modules/Platform/Linux-GNU.cmake"
- "/usr/share/cmake-3.10/Modules/Platform/Linux.cmake"
- "/usr/share/cmake-3.10/Modules/Platform/UnixPaths.cmake"
- )
-
-# The corresponding makefile is:
-set(CMAKE_MAKEFILE_OUTPUTS
- "Makefile"
- "CMakeFiles/cmake.check_cache"
- )
-
-# Byproducts of CMake generate step:
-set(CMAKE_MAKEFILE_PRODUCTS
- "CMakeFiles/CMakeDirectoryInformation.cmake"
- "crazyflie_cpp/CMakeFiles/CMakeDirectoryInformation.cmake"
- )
-
-# Dependency information for all targets:
-set(CMAKE_DEPEND_INFO_FILES
- "CMakeFiles/controller_test.dir/DependInfo.cmake"
- "CMakeFiles/drone_modelling.dir/DependInfo.cmake"
- "crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/DependInfo.cmake"
- )
diff --git a/controller_test_drone_modeling/build/CMakeFiles/Makefile2 b/controller_test_drone_modeling/build/CMakeFiles/Makefile2
deleted file mode 100644
index 0d87d4034c3069d3b29a5abb48cccda0b21a97e8..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/Makefile2
+++ /dev/null
@@ -1,200 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# Default target executed when no arguments are given to make.
-default_target: all
-
-.PHONY : default_target
-
-# The main recursive all target
-all:
-
-.PHONY : all
-
-# The main recursive preinstall target
-preinstall:
-
-.PHONY : preinstall
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-
-# A target that is always out of date.
-cmake_force:
-
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-
-#=============================================================================
-# Target rules for target CMakeFiles/controller_test.dir
-
-# All Build rule for target.
-CMakeFiles/controller_test.dir/all: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all
- $(MAKE) -f CMakeFiles/controller_test.dir/build.make CMakeFiles/controller_test.dir/depend
- $(MAKE) -f CMakeFiles/controller_test.dir/build.make CMakeFiles/controller_test.dir/build
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=1,2 "Built target controller_test"
-.PHONY : CMakeFiles/controller_test.dir/all
-
-# Include target in all.
-all: CMakeFiles/controller_test.dir/all
-
-.PHONY : all
-
-# Build rule for subdir invocation for target.
-CMakeFiles/controller_test.dir/rule: cmake_check_build_system
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles 9
- $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/controller_test.dir/all
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles 0
-.PHONY : CMakeFiles/controller_test.dir/rule
-
-# Convenience name for target.
-controller_test: CMakeFiles/controller_test.dir/rule
-
-.PHONY : controller_test
-
-# clean rule for target.
-CMakeFiles/controller_test.dir/clean:
- $(MAKE) -f CMakeFiles/controller_test.dir/build.make CMakeFiles/controller_test.dir/clean
-.PHONY : CMakeFiles/controller_test.dir/clean
-
-# clean rule for target.
-clean: CMakeFiles/controller_test.dir/clean
-
-.PHONY : clean
-
-#=============================================================================
-# Target rules for target CMakeFiles/drone_modelling.dir
-
-# All Build rule for target.
-CMakeFiles/drone_modelling.dir/all: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all
- $(MAKE) -f CMakeFiles/drone_modelling.dir/build.make CMakeFiles/drone_modelling.dir/depend
- $(MAKE) -f CMakeFiles/drone_modelling.dir/build.make CMakeFiles/drone_modelling.dir/build
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=10,11 "Built target drone_modelling"
-.PHONY : CMakeFiles/drone_modelling.dir/all
-
-# Include target in all.
-all: CMakeFiles/drone_modelling.dir/all
-
-.PHONY : all
-
-# Build rule for subdir invocation for target.
-CMakeFiles/drone_modelling.dir/rule: cmake_check_build_system
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles 9
- $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/drone_modelling.dir/all
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles 0
-.PHONY : CMakeFiles/drone_modelling.dir/rule
-
-# Convenience name for target.
-drone_modelling: CMakeFiles/drone_modelling.dir/rule
-
-.PHONY : drone_modelling
-
-# clean rule for target.
-CMakeFiles/drone_modelling.dir/clean:
- $(MAKE) -f CMakeFiles/drone_modelling.dir/build.make CMakeFiles/drone_modelling.dir/clean
-.PHONY : CMakeFiles/drone_modelling.dir/clean
-
-# clean rule for target.
-clean: CMakeFiles/drone_modelling.dir/clean
-
-.PHONY : clean
-
-#=============================================================================
-# Directory level rules for directory crazyflie_cpp
-
-# Convenience name for "all" pass in the directory.
-crazyflie_cpp/all: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all
-
-.PHONY : crazyflie_cpp/all
-
-# Convenience name for "clean" pass in the directory.
-crazyflie_cpp/clean: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/clean
-
-.PHONY : crazyflie_cpp/clean
-
-# Convenience name for "preinstall" pass in the directory.
-crazyflie_cpp/preinstall:
-
-.PHONY : crazyflie_cpp/preinstall
-
-#=============================================================================
-# Target rules for target crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir
-
-# All Build rule for target.
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all:
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=3,4,5,6,7,8,9 "Built target crazyflie_cpp"
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all
-
-# Include target in all.
-all: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all
-
-.PHONY : all
-
-# Build rule for subdir invocation for target.
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/rule: cmake_check_build_system
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles 7
- $(MAKE) -f CMakeFiles/Makefile2 crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/all
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles 0
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/rule
-
-# Convenience name for target.
-crazyflie_cpp: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/rule
-
-.PHONY : crazyflie_cpp
-
-# clean rule for target.
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/clean:
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/clean
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/clean
-
-# clean rule for target.
-clean: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/clean
-
-.PHONY : clean
-
-#=============================================================================
-# Special targets to cleanup operation of make.
-
-# Special rule to run CMake to check the build system integrity.
-# No rule that depends on this can have commands that come from listfiles
-# because they might be regenerated.
-cmake_check_build_system:
- $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
-.PHONY : cmake_check_build_system
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/TargetDirectories.txt b/controller_test_drone_modeling/build/CMakeFiles/TargetDirectories.txt
deleted file mode 100644
index 8154a3444fd26098e6728c0a9eab45d0b9a89c1b..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/TargetDirectories.txt
+++ /dev/null
@@ -1,7 +0,0 @@
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/rebuild_cache.dir
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/edit_cache.dir
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/rebuild_cache.dir
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/edit_cache.dir
diff --git a/controller_test_drone_modeling/build/CMakeFiles/cmake.check_cache b/controller_test_drone_modeling/build/CMakeFiles/cmake.check_cache
deleted file mode 100644
index 3dccd731726d7faa8b29d8d7dba3b981a53ca497..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/cmake.check_cache
+++ /dev/null
@@ -1 +0,0 @@
-# This file is generated by cmake for dependency checking of the CMakeCache.txt file
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/CXX.includecache b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/CXX.includecache
deleted file mode 100644
index 30622bb22d82b3f38a8fca573ac51c96b0e83c57..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/CXX.includecache
+++ /dev/null
@@ -1,82 +0,0 @@
-#IncludeRegexLine: ^[ ]*[#%][ ]*(include|import)[ ]*[<"]([^">]+)([">])
-
-#IncludeRegexScan: ^.*$
-
-#IncludeRegexComplain: ^$
-
-#IncludeRegexTransform:
-
-../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
-cstring
--
-sstream
--
-functional
--
-math.h
--
-Crazyradio.h
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-crtp.h
-../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-list
--
-set
--
-map
--
-chrono
--
-
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-stdint.h
--
-vector
--
-ITransport.h
-../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-USBDevice.h
-../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-
-../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-stdint.h
--
-fstream
--
-
-../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-stdint.h
--
-
-../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-Crazyradio.h
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-cstdint
--
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/controller_test.cpp
-iostream
--
-memory
--
-thread
--
-boost/program_options.hpp
--
-crazyflie_cpp/Crazyflie.h
--
-map
--
-unistd.h
--
-vector
--
-fstream
--
-atomic
--
-mutex
--
-chrono
--
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/DependInfo.cmake b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/DependInfo.cmake
deleted file mode 100644
index 7c6c8b07e817b63c844b5f08745b6dd19a2bc6b2..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/DependInfo.cmake
+++ /dev/null
@@ -1,23 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-set(CMAKE_DEPENDS_LANGUAGES
- "CXX"
- )
-# The set of files for implicit dependencies of each language:
-set(CMAKE_DEPENDS_CHECK_CXX
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/controller_test.cpp" "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/src/controller_test.cpp.o"
- )
-set(CMAKE_CXX_COMPILER_ID "GNU")
-
-# The include file search paths:
-set(CMAKE_CXX_TARGET_INCLUDE_PATH
- "../include"
- "../crazyflie_cpp/include"
- )
-
-# Targets to which this target links.
-set(CMAKE_TARGET_LINKED_INFO_FILES
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/DependInfo.cmake"
- )
-
-# Fortran module output directory.
-set(CMAKE_Fortran_TARGET_MODULE_DIR "")
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/build.make b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/build.make
deleted file mode 100644
index 763fc9e0834066243fa50e7b52a9cb156e3e97e3..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/build.make
+++ /dev/null
@@ -1,116 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# Delete rule output on recipe failure.
-.DELETE_ON_ERROR:
-
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-
-# A target that is always out of date.
-cmake_force:
-
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-
-# Include any dependencies generated for this target.
-include CMakeFiles/controller_test.dir/depend.make
-
-# Include the progress variables for this target.
-include CMakeFiles/controller_test.dir/progress.make
-
-# Include the compile flags for this target's objects.
-include CMakeFiles/controller_test.dir/flags.make
-
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o: CMakeFiles/controller_test.dir/flags.make
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o: ../src/controller_test.cpp
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_1) "Building CXX object CMakeFiles/controller_test.dir/src/controller_test.cpp.o"
- /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/controller_test.dir/src/controller_test.cpp.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/controller_test.cpp
-
-CMakeFiles/controller_test.dir/src/controller_test.cpp.i: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/controller_test.dir/src/controller_test.cpp.i"
- /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/controller_test.cpp > CMakeFiles/controller_test.dir/src/controller_test.cpp.i
-
-CMakeFiles/controller_test.dir/src/controller_test.cpp.s: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/controller_test.dir/src/controller_test.cpp.s"
- /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/controller_test.cpp -o CMakeFiles/controller_test.dir/src/controller_test.cpp.s
-
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o.requires:
-
-.PHONY : CMakeFiles/controller_test.dir/src/controller_test.cpp.o.requires
-
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o.provides: CMakeFiles/controller_test.dir/src/controller_test.cpp.o.requires
- $(MAKE) -f CMakeFiles/controller_test.dir/build.make CMakeFiles/controller_test.dir/src/controller_test.cpp.o.provides.build
-.PHONY : CMakeFiles/controller_test.dir/src/controller_test.cpp.o.provides
-
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o.provides.build: CMakeFiles/controller_test.dir/src/controller_test.cpp.o
-
-
-# Object files for target controller_test
-controller_test_OBJECTS = \
-"CMakeFiles/controller_test.dir/src/controller_test.cpp.o"
-
-# External object files for target controller_test
-controller_test_EXTERNAL_OBJECTS =
-
-controller_test: CMakeFiles/controller_test.dir/src/controller_test.cpp.o
-controller_test: CMakeFiles/controller_test.dir/build.make
-controller_test: crazyflie_cpp/libcrazyflie_cpp.a
-controller_test: /usr/lib/x86_64-linux-gnu/libboost_program_options.so
-controller_test: /usr/lib/x86_64-linux-gnu/libusb-1.0.so
-controller_test: CMakeFiles/controller_test.dir/link.txt
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable controller_test"
- $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/controller_test.dir/link.txt --verbose=$(VERBOSE)
-
-# Rule to build all files generated by this target.
-CMakeFiles/controller_test.dir/build: controller_test
-
-.PHONY : CMakeFiles/controller_test.dir/build
-
-CMakeFiles/controller_test.dir/requires: CMakeFiles/controller_test.dir/src/controller_test.cpp.o.requires
-
-.PHONY : CMakeFiles/controller_test.dir/requires
-
-CMakeFiles/controller_test.dir/clean:
- $(CMAKE_COMMAND) -P CMakeFiles/controller_test.dir/cmake_clean.cmake
-.PHONY : CMakeFiles/controller_test.dir/clean
-
-CMakeFiles/controller_test.dir/depend:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : CMakeFiles/controller_test.dir/depend
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/cmake_clean.cmake b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/cmake_clean.cmake
deleted file mode 100644
index 76e661af32d7037eb557d3386509333be3c22c22..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,10 +0,0 @@
-file(REMOVE_RECURSE
- "CMakeFiles/controller_test.dir/src/controller_test.cpp.o"
- "controller_test.pdb"
- "controller_test"
-)
-
-# Per-language clean rules from dependency scanning.
-foreach(lang CXX)
- include(CMakeFiles/controller_test.dir/cmake_clean_${lang}.cmake OPTIONAL)
-endforeach()
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/depend.internal b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/depend.internal
deleted file mode 100644
index dd60a1f597923b99ba657a39eb0f0e25f1632eb9..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/depend.internal
+++ /dev/null
@@ -1,10 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o
- ../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
- ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
- ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
- ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
- ../crazyflie_cpp/include/crazyflie_cpp/crtp.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/controller_test.cpp
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/depend.make b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/depend.make
deleted file mode 100644
index 0521657ca7088e43ec7091363f4770dbf8de9751..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/depend.make
+++ /dev/null
@@ -1,10 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-CMakeFiles/controller_test.dir/src/controller_test.cpp.o: ../src/controller_test.cpp
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/flags.make b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/flags.make
deleted file mode 100644
index be4fcd097437e549cb60591bdb44882047a91d8c..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/flags.make
+++ /dev/null
@@ -1,10 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# compile CXX with /usr/bin/c++
-CXX_FLAGS = -Wall -Wextra -Werror -std=c++11
-
-CXX_DEFINES =
-
-CXX_INCLUDES = -I/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/include -I/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/include
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/link.txt b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/link.txt
deleted file mode 100644
index c159f2990495c3b29b422a8932e181b9eba61a97..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/link.txt
+++ /dev/null
@@ -1 +0,0 @@
-/usr/bin/c++ -rdynamic CMakeFiles/controller_test.dir/src/controller_test.cpp.o -o controller_test crazyflie_cpp/libcrazyflie_cpp.a -lboost_program_options -lpthread -lusb-1.0
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/progress.make b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/progress.make
deleted file mode 100644
index abadeb0c3abaa81d622026fcd3ae096d03dd29b7..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/progress.make
+++ /dev/null
@@ -1,3 +0,0 @@
-CMAKE_PROGRESS_1 = 1
-CMAKE_PROGRESS_2 = 2
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/src/controller_test.cpp.o b/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/src/controller_test.cpp.o
deleted file mode 100644
index 8b288d001f300f7ab210cdf477a128be2d076361..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/CMakeFiles/controller_test.dir/src/controller_test.cpp.o and /dev/null differ
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/CXX.includecache b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/CXX.includecache
deleted file mode 100644
index d5848a73e7100f657f9d1e2134ab5edd8f892090..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/CXX.includecache
+++ /dev/null
@@ -1,74 +0,0 @@
-#IncludeRegexLine: ^[ ]*[#%][ ]*(include|import)[ ]*[<"]([^">]+)([">])
-
-#IncludeRegexScan: ^.*$
-
-#IncludeRegexComplain: ^$
-
-#IncludeRegexTransform:
-
-../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
-cstring
--
-sstream
--
-functional
--
-math.h
--
-Crazyradio.h
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-crtp.h
-../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-list
--
-set
--
-map
--
-chrono
--
-
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-stdint.h
--
-vector
--
-ITransport.h
-../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-USBDevice.h
-../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-
-../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-stdint.h
--
-fstream
--
-
-../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-stdint.h
--
-
-../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-Crazyradio.h
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-cstdint
--
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/drone_modelling.cpp
-boost/program_options.hpp
--
-thread
--
-crazyflie_cpp/Crazyflie.h
--
-iostream
--
-atomic
--
-chrono
--
-fstream
--
-mutex
--
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/DependInfo.cmake b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/DependInfo.cmake
deleted file mode 100644
index 024927caa4b641a19e2b9b967c98c2442fbed294..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/DependInfo.cmake
+++ /dev/null
@@ -1,23 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-set(CMAKE_DEPENDS_LANGUAGES
- "CXX"
- )
-# The set of files for implicit dependencies of each language:
-set(CMAKE_DEPENDS_CHECK_CXX
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/drone_modelling.cpp" "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o"
- )
-set(CMAKE_CXX_COMPILER_ID "GNU")
-
-# The include file search paths:
-set(CMAKE_CXX_TARGET_INCLUDE_PATH
- "../include"
- "../crazyflie_cpp/include"
- )
-
-# Targets to which this target links.
-set(CMAKE_TARGET_LINKED_INFO_FILES
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/DependInfo.cmake"
- )
-
-# Fortran module output directory.
-set(CMAKE_Fortran_TARGET_MODULE_DIR "")
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/build.make b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/build.make
deleted file mode 100644
index 94d34e57f5204e5a5e2f074b3e51f4843fe4097f..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/build.make
+++ /dev/null
@@ -1,116 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# Delete rule output on recipe failure.
-.DELETE_ON_ERROR:
-
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-
-# A target that is always out of date.
-cmake_force:
-
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-
-# Include any dependencies generated for this target.
-include CMakeFiles/drone_modelling.dir/depend.make
-
-# Include the progress variables for this target.
-include CMakeFiles/drone_modelling.dir/progress.make
-
-# Include the compile flags for this target's objects.
-include CMakeFiles/drone_modelling.dir/flags.make
-
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o: CMakeFiles/drone_modelling.dir/flags.make
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o: ../src/drone_modelling.cpp
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_1) "Building CXX object CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o"
- /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/drone_modelling.cpp
-
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.i: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.i"
- /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/drone_modelling.cpp > CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.i
-
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.s: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.s"
- /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/drone_modelling.cpp -o CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.s
-
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o.requires:
-
-.PHONY : CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o.requires
-
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o.provides: CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o.requires
- $(MAKE) -f CMakeFiles/drone_modelling.dir/build.make CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o.provides.build
-.PHONY : CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o.provides
-
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o.provides.build: CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o
-
-
-# Object files for target drone_modelling
-drone_modelling_OBJECTS = \
-"CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o"
-
-# External object files for target drone_modelling
-drone_modelling_EXTERNAL_OBJECTS =
-
-drone_modelling: CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o
-drone_modelling: CMakeFiles/drone_modelling.dir/build.make
-drone_modelling: crazyflie_cpp/libcrazyflie_cpp.a
-drone_modelling: /usr/lib/x86_64-linux-gnu/libboost_program_options.so
-drone_modelling: /usr/lib/x86_64-linux-gnu/libusb-1.0.so
-drone_modelling: CMakeFiles/drone_modelling.dir/link.txt
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Linking CXX executable drone_modelling"
- $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/drone_modelling.dir/link.txt --verbose=$(VERBOSE)
-
-# Rule to build all files generated by this target.
-CMakeFiles/drone_modelling.dir/build: drone_modelling
-
-.PHONY : CMakeFiles/drone_modelling.dir/build
-
-CMakeFiles/drone_modelling.dir/requires: CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o.requires
-
-.PHONY : CMakeFiles/drone_modelling.dir/requires
-
-CMakeFiles/drone_modelling.dir/clean:
- $(CMAKE_COMMAND) -P CMakeFiles/drone_modelling.dir/cmake_clean.cmake
-.PHONY : CMakeFiles/drone_modelling.dir/clean
-
-CMakeFiles/drone_modelling.dir/depend:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : CMakeFiles/drone_modelling.dir/depend
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/cmake_clean.cmake b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/cmake_clean.cmake
deleted file mode 100644
index ab26c81a6adf0c26e994becfcd3a72b513c6f688..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,10 +0,0 @@
-file(REMOVE_RECURSE
- "CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o"
- "drone_modelling.pdb"
- "drone_modelling"
-)
-
-# Per-language clean rules from dependency scanning.
-foreach(lang CXX)
- include(CMakeFiles/drone_modelling.dir/cmake_clean_${lang}.cmake OPTIONAL)
-endforeach()
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/depend.internal b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/depend.internal
deleted file mode 100644
index 646104fb4ce92a774460d3fafe9529e340b2c3be..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/depend.internal
+++ /dev/null
@@ -1,10 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o
- ../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
- ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
- ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
- ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
- ../crazyflie_cpp/include/crazyflie_cpp/crtp.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/src/drone_modelling.cpp
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/depend.make b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/depend.make
deleted file mode 100644
index dd86ac2cb7cbc90bb097300ae0947ecc11fc573c..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/depend.make
+++ /dev/null
@@ -1,10 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o: ../src/drone_modelling.cpp
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/flags.make b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/flags.make
deleted file mode 100644
index be4fcd097437e549cb60591bdb44882047a91d8c..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/flags.make
+++ /dev/null
@@ -1,10 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# compile CXX with /usr/bin/c++
-CXX_FLAGS = -Wall -Wextra -Werror -std=c++11
-
-CXX_DEFINES =
-
-CXX_INCLUDES = -I/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/include -I/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/include
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/link.txt b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/link.txt
deleted file mode 100644
index 67988d6697bd5af2a5981558fdbeefc7ce5ef8f8..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/link.txt
+++ /dev/null
@@ -1 +0,0 @@
-/usr/bin/c++ -rdynamic CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o -o drone_modelling crazyflie_cpp/libcrazyflie_cpp.a -lboost_program_options -lpthread -lusb-1.0
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/progress.make b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/progress.make
deleted file mode 100644
index 17875e3238da3ecdefe39c6875b6b441dc576689..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/progress.make
+++ /dev/null
@@ -1,3 +0,0 @@
-CMAKE_PROGRESS_1 = 10
-CMAKE_PROGRESS_2 = 11
-
diff --git a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o b/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o
deleted file mode 100644
index a9f152ba2ee79b3f7f3f80582b879f2e12b5a3e1..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o and /dev/null differ
diff --git a/controller_test_drone_modeling/build/CMakeFiles/feature_tests.bin b/controller_test_drone_modeling/build/CMakeFiles/feature_tests.bin
deleted file mode 100755
index ae0f49908c89c0ae48d68d46e0157414893eb334..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/CMakeFiles/feature_tests.bin and /dev/null differ
diff --git a/controller_test_drone_modeling/build/CMakeFiles/feature_tests.c b/controller_test_drone_modeling/build/CMakeFiles/feature_tests.c
deleted file mode 100644
index 83e86dd8cd85f9f7554f51122b8cd08412ec01f2..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/feature_tests.c
+++ /dev/null
@@ -1,34 +0,0 @@
-
- const char features[] = {"\n"
-"C_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304
-"1"
-#else
-"0"
-#endif
-"c_function_prototypes\n"
-"C_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L
-"1"
-#else
-"0"
-#endif
-"c_restrict\n"
-"C_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201000L
-"1"
-#else
-"0"
-#endif
-"c_static_assert\n"
-"C_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L
-"1"
-#else
-"0"
-#endif
-"c_variadic_macros\n"
-
-};
-
-int main(int argc, char** argv) { (void)argv; return features[argc]; }
diff --git a/controller_test_drone_modeling/build/CMakeFiles/feature_tests.cxx b/controller_test_drone_modeling/build/CMakeFiles/feature_tests.cxx
deleted file mode 100644
index b93418c6ed69feaf1b5c2feb9592bbdb5a5f042c..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/feature_tests.cxx
+++ /dev/null
@@ -1,405 +0,0 @@
-
- const char features[] = {"\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 500 && __cplusplus >= 201402L
-"1"
-#else
-"0"
-#endif
-"cxx_aggregate_default_initializers\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 407 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_alias_templates\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 408 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_alignas\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 408 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_alignof\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 408 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_attributes\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 409 && __cplusplus > 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_attribute_deprecated\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_auto_type\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 409 && __cplusplus > 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_binary_literals\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_constexpr\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 409 && __cplusplus > 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_contextual_conversions\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_decltype\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 409 && __cplusplus > 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_decltype_auto\n"
-"CXX_FEATURE:"
-#if ((__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) >= 40801) && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_decltype_incomplete_return_types\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_default_function_template_args\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_defaulted_functions\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_defaulted_move_initializers\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 407 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_delegating_constructors\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_deleted_functions\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 409 && __cplusplus > 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_digit_separators\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_enum_forward_declarations\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 405 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_explicit_conversions\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 407 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_extended_friend_declarations\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_extern_templates\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 407 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_final\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_func_identifier\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_generalized_initializers\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 409 && __cplusplus > 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_generic_lambdas\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 408 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_inheriting_constructors\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_inline_namespaces\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 405 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_lambdas\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 409 && __cplusplus > 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_lambda_init_captures\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 405 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_local_type_template_args\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_long_long_type\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_noexcept\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 407 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_nonstatic_member_init\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_nullptr\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 407 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_override\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_range_for\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 405 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_raw_string_literals\n"
-"CXX_FEATURE:"
-#if ((__GNUC__ * 10000 + __GNUC_MINOR__ * 100 + __GNUC_PATCHLEVEL__) >= 40801) && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_reference_qualified_functions\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 500 && __cplusplus >= 201402L
-"1"
-#else
-"0"
-#endif
-"cxx_relaxed_constexpr\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 409 && __cplusplus > 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_return_type_deduction\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_right_angle_brackets\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_rvalue_references\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_sizeof_member\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_static_assert\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_strong_enums\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && __cplusplus
-"1"
-#else
-"0"
-#endif
-"cxx_template_template_parameters\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 408 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_thread_local\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_trailing_return_types\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_unicode_literals\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_uniform_initialization\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_unrestricted_unions\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 407 && __cplusplus >= 201103L
-"1"
-#else
-"0"
-#endif
-"cxx_user_literals\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 500 && __cplusplus >= 201402L
-"1"
-#else
-"0"
-#endif
-"cxx_variable_templates\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_variadic_macros\n"
-"CXX_FEATURE:"
-#if (__GNUC__ * 100 + __GNUC_MINOR__) >= 404 && (__cplusplus >= 201103L || (defined(__GXX_EXPERIMENTAL_CXX0X__) && __GXX_EXPERIMENTAL_CXX0X__))
-"1"
-#else
-"0"
-#endif
-"cxx_variadic_templates\n"
-
-};
-
-int main(int argc, char** argv) { (void)argv; return features[argc]; }
diff --git a/controller_test_drone_modeling/build/CMakeFiles/progress.marks b/controller_test_drone_modeling/build/CMakeFiles/progress.marks
deleted file mode 100644
index b4de3947675361a7770d29b8982c407b0ec6b2a0..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/CMakeFiles/progress.marks
+++ /dev/null
@@ -1 +0,0 @@
-11
diff --git a/controller_test_drone_modeling/build/Makefile b/controller_test_drone_modeling/build/Makefile
deleted file mode 100644
index 78d4a2df17f5161d5db8cfaff820539bab3457c4..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/Makefile
+++ /dev/null
@@ -1,236 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# Default target executed when no arguments are given to make.
-default_target: all
-
-.PHONY : default_target
-
-# Allow only one "make -f Makefile2" at a time, but pass parallelism.
-.NOTPARALLEL:
-
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-
-# A target that is always out of date.
-cmake_force:
-
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-
-#=============================================================================
-# Targets provided globally by CMake.
-
-# Special rule for the target rebuild_cache
-rebuild_cache:
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
- /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
-.PHONY : rebuild_cache
-
-# Special rule for the target rebuild_cache
-rebuild_cache/fast: rebuild_cache
-
-.PHONY : rebuild_cache/fast
-
-# Special rule for the target edit_cache
-edit_cache:
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..."
- /usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available.
-.PHONY : edit_cache
-
-# Special rule for the target edit_cache
-edit_cache/fast: edit_cache
-
-.PHONY : edit_cache/fast
-
-# The main all target
-all: cmake_check_build_system
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles/progress.marks
- $(MAKE) -f CMakeFiles/Makefile2 all
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles 0
-.PHONY : all
-
-# The main clean target
-clean:
- $(MAKE) -f CMakeFiles/Makefile2 clean
-.PHONY : clean
-
-# The main clean target
-clean/fast: clean
-
-.PHONY : clean/fast
-
-# Prepare targets for installation.
-preinstall: all
- $(MAKE) -f CMakeFiles/Makefile2 preinstall
-.PHONY : preinstall
-
-# Prepare targets for installation.
-preinstall/fast:
- $(MAKE) -f CMakeFiles/Makefile2 preinstall
-.PHONY : preinstall/fast
-
-# clear depends
-depend:
- $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
-.PHONY : depend
-
-#=============================================================================
-# Target rules for targets named controller_test
-
-# Build rule for target.
-controller_test: cmake_check_build_system
- $(MAKE) -f CMakeFiles/Makefile2 controller_test
-.PHONY : controller_test
-
-# fast build rule for target.
-controller_test/fast:
- $(MAKE) -f CMakeFiles/controller_test.dir/build.make CMakeFiles/controller_test.dir/build
-.PHONY : controller_test/fast
-
-#=============================================================================
-# Target rules for targets named drone_modelling
-
-# Build rule for target.
-drone_modelling: cmake_check_build_system
- $(MAKE) -f CMakeFiles/Makefile2 drone_modelling
-.PHONY : drone_modelling
-
-# fast build rule for target.
-drone_modelling/fast:
- $(MAKE) -f CMakeFiles/drone_modelling.dir/build.make CMakeFiles/drone_modelling.dir/build
-.PHONY : drone_modelling/fast
-
-#=============================================================================
-# Target rules for targets named crazyflie_cpp
-
-# Build rule for target.
-crazyflie_cpp: cmake_check_build_system
- $(MAKE) -f CMakeFiles/Makefile2 crazyflie_cpp
-.PHONY : crazyflie_cpp
-
-# fast build rule for target.
-crazyflie_cpp/fast:
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build
-.PHONY : crazyflie_cpp/fast
-
-src/controller_test.o: src/controller_test.cpp.o
-
-.PHONY : src/controller_test.o
-
-# target to build an object file
-src/controller_test.cpp.o:
- $(MAKE) -f CMakeFiles/controller_test.dir/build.make CMakeFiles/controller_test.dir/src/controller_test.cpp.o
-.PHONY : src/controller_test.cpp.o
-
-src/controller_test.i: src/controller_test.cpp.i
-
-.PHONY : src/controller_test.i
-
-# target to preprocess a source file
-src/controller_test.cpp.i:
- $(MAKE) -f CMakeFiles/controller_test.dir/build.make CMakeFiles/controller_test.dir/src/controller_test.cpp.i
-.PHONY : src/controller_test.cpp.i
-
-src/controller_test.s: src/controller_test.cpp.s
-
-.PHONY : src/controller_test.s
-
-# target to generate assembly for a file
-src/controller_test.cpp.s:
- $(MAKE) -f CMakeFiles/controller_test.dir/build.make CMakeFiles/controller_test.dir/src/controller_test.cpp.s
-.PHONY : src/controller_test.cpp.s
-
-src/drone_modelling.o: src/drone_modelling.cpp.o
-
-.PHONY : src/drone_modelling.o
-
-# target to build an object file
-src/drone_modelling.cpp.o:
- $(MAKE) -f CMakeFiles/drone_modelling.dir/build.make CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.o
-.PHONY : src/drone_modelling.cpp.o
-
-src/drone_modelling.i: src/drone_modelling.cpp.i
-
-.PHONY : src/drone_modelling.i
-
-# target to preprocess a source file
-src/drone_modelling.cpp.i:
- $(MAKE) -f CMakeFiles/drone_modelling.dir/build.make CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.i
-.PHONY : src/drone_modelling.cpp.i
-
-src/drone_modelling.s: src/drone_modelling.cpp.s
-
-.PHONY : src/drone_modelling.s
-
-# target to generate assembly for a file
-src/drone_modelling.cpp.s:
- $(MAKE) -f CMakeFiles/drone_modelling.dir/build.make CMakeFiles/drone_modelling.dir/src/drone_modelling.cpp.s
-.PHONY : src/drone_modelling.cpp.s
-
-# Help Target
-help:
- @echo "The following are some of the valid targets for this Makefile:"
- @echo "... all (the default if no target is provided)"
- @echo "... clean"
- @echo "... depend"
- @echo "... rebuild_cache"
- @echo "... controller_test"
- @echo "... edit_cache"
- @echo "... drone_modelling"
- @echo "... crazyflie_cpp"
- @echo "... src/controller_test.o"
- @echo "... src/controller_test.i"
- @echo "... src/controller_test.s"
- @echo "... src/drone_modelling.o"
- @echo "... src/drone_modelling.i"
- @echo "... src/drone_modelling.s"
-.PHONY : help
-
-
-
-#=============================================================================
-# Special targets to cleanup operation of make.
-
-# Special rule to run CMake to check the build system integrity.
-# No rule that depends on this can have commands that come from listfiles
-# because they might be regenerated.
-cmake_check_build_system:
- $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
-.PHONY : cmake_check_build_system
-
diff --git a/controller_test_drone_modeling/build/cmake_install.cmake b/controller_test_drone_modeling/build/cmake_install.cmake
deleted file mode 100644
index a4b17ebaa7bccd3418cd713fbb691f1de958c4f6..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/cmake_install.cmake
+++ /dev/null
@@ -1,55 +0,0 @@
-# Install script for directory: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-
-# Set the install prefix
-if(NOT DEFINED CMAKE_INSTALL_PREFIX)
- set(CMAKE_INSTALL_PREFIX "/usr/local")
-endif()
-string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
-# Set the install configuration name.
-if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
- if(BUILD_TYPE)
- string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
- CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
- else()
- set(CMAKE_INSTALL_CONFIG_NAME "")
- endif()
- message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
-endif()
-
-# Set the component getting installed.
-if(NOT CMAKE_INSTALL_COMPONENT)
- if(COMPONENT)
- message(STATUS "Install component: \"${COMPONENT}\"")
- set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
- else()
- set(CMAKE_INSTALL_COMPONENT)
- endif()
-endif()
-
-# Install shared libraries without execute permission?
-if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
- set(CMAKE_INSTALL_SO_NO_EXE "1")
-endif()
-
-# Is this installation the result of a crosscompile?
-if(NOT DEFINED CMAKE_CROSSCOMPILING)
- set(CMAKE_CROSSCOMPILING "FALSE")
-endif()
-
-if(NOT CMAKE_INSTALL_LOCAL_ONLY)
- # Include the install script for each subdirectory.
- include("/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/cmake_install.cmake")
-
-endif()
-
-if(CMAKE_INSTALL_COMPONENT)
- set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt")
-else()
- set(CMAKE_INSTALL_MANIFEST "install_manifest.txt")
-endif()
-
-string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT
- "${CMAKE_INSTALL_MANIFEST_FILES}")
-file(WRITE "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/${CMAKE_INSTALL_MANIFEST}"
- "${CMAKE_INSTALL_MANIFEST_CONTENT}")
diff --git a/controller_test_drone_modeling/build/controller_test b/controller_test_drone_modeling/build/controller_test
deleted file mode 100755
index 8159b44f28fd982b1090605027acfbd94f03d5df..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/controller_test and /dev/null differ
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/CMakeDirectoryInformation.cmake b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/CMakeDirectoryInformation.cmake
deleted file mode 100644
index 70cde40ccb0dbe1225c7de672ef9f5d5a8dc14aa..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/CMakeDirectoryInformation.cmake
+++ /dev/null
@@ -1,16 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# Relative path conversion top directories.
-set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling")
-set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build")
-
-# Force unix paths in dependencies.
-set(CMAKE_FORCE_UNIX_PATHS 1)
-
-
-# The C and CXX include file regular expressions for this directory.
-set(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$")
-set(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$")
-set(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN})
-set(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN})
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/CXX.includecache b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/CXX.includecache
deleted file mode 100644
index a9ad929911b41dfa7e785942ff2395ef303fa4ef..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/CXX.includecache
+++ /dev/null
@@ -1,148 +0,0 @@
-#IncludeRegexLine: ^[ ]*[#%][ ]*(include|import)[ ]*[<"]([^">]+)([">])
-
-#IncludeRegexScan: ^.*$
-
-#IncludeRegexComplain: ^$
-
-#IncludeRegexTransform:
-
-../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
-cstring
--
-sstream
--
-functional
--
-math.h
--
-Crazyradio.h
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-crtp.h
-../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-list
--
-set
--
-map
--
-chrono
--
-
-../crazyflie_cpp/include/crazyflie_cpp/CrazyflieUSB.h
-stdint.h
--
-vector
--
-string
--
-ITransport.h
-../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-USBDevice.h
-../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-stdint.h
--
-vector
--
-ITransport.h
-../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-USBDevice.h
-../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-
-../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-stdint.h
--
-fstream
--
-
-../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-stdint.h
--
-
-../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-Crazyradio.h
-../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-cstdint
--
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyflie.cpp
-mutex
--
-Crazyflie.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyflie.h
-crtp.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtp.h
-crtpBootloader.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtpBootloader.h
-crtpNRF51.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtpNRF51.h
-Crazyradio.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyradio.h
-CrazyflieUSB.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/CrazyflieUSB.h
-fstream
--
-cstring
--
-stdexcept
--
-thread
--
-cmath
--
-inttypes.h
--
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/CrazyflieUSB.cpp
-CrazyflieUSB.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/CrazyflieUSB.h
-sstream
--
-stdexcept
--
-libusb-1.0/libusb.h
--
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyradio.cpp
-Crazyradio.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyradio.h
-sstream
--
-stdexcept
--
-libusb-1.0/libusb.h
--
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/ITransport.cpp
-ITransport.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/ITransport.h
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/USBDevice.cpp
-USBDevice.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/USBDevice.h
-sstream
--
-stdexcept
--
-libusb-1.0/libusb.h
--
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtp.cpp
-stdint.h
--
-math.h
--
-cstring
--
-crtp.h
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtp.h
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtpBootloader.h
-cstdint
--
-
-/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtpNRF51.h
-cstdint
--
-
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/DependInfo.cmake b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/DependInfo.cmake
deleted file mode 100644
index 5b11c5733958e06f1f2ec8d4d918044c19d2ff9d..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/DependInfo.cmake
+++ /dev/null
@@ -1,26 +0,0 @@
-# The set of languages for which implicit dependencies are needed:
-set(CMAKE_DEPENDS_LANGUAGES
- "CXX"
- )
-# The set of files for implicit dependencies of each language:
-set(CMAKE_DEPENDS_CHECK_CXX
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyflie.cpp" "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o"
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/CrazyflieUSB.cpp" "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o"
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyradio.cpp" "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o"
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/ITransport.cpp" "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o"
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/USBDevice.cpp" "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o"
- "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtp.cpp" "/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o"
- )
-set(CMAKE_CXX_COMPILER_ID "GNU")
-
-# The include file search paths:
-set(CMAKE_CXX_TARGET_INCLUDE_PATH
- "../crazyflie_cpp/include/crazyflie_cpp"
- )
-
-# Targets to which this target links.
-set(CMAKE_TARGET_LINKED_INFO_FILES
- )
-
-# Fortran module output directory.
-set(CMAKE_Fortran_TARGET_MODULE_DIR "")
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make
deleted file mode 100644
index 0d50aaead7a46d3694a569bfc2c8ae59a15dd402..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make
+++ /dev/null
@@ -1,249 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# Delete rule output on recipe failure.
-.DELETE_ON_ERROR:
-
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-
-# A target that is always out of date.
-cmake_force:
-
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-
-# Include any dependencies generated for this target.
-include crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend.make
-
-# Include the progress variables for this target.
-include crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/progress.make
-
-# Include the compile flags for this target's objects.
-include crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/src/Crazyflie.cpp
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_1) "Building CXX object crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyflie.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.i: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.i"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyflie.cpp > CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.i
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.s: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.s"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyflie.cpp -o CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.s
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o.requires:
-
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o.requires
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o.provides: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o.requires
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o.provides.build
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o.provides
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o.provides.build: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o
-
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o: ../crazyflie_cpp/src/CrazyflieUSB.cpp
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_2) "Building CXX object crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/CrazyflieUSB.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.i: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.i"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/CrazyflieUSB.cpp > CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.i
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.s: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.s"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/CrazyflieUSB.cpp -o CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.s
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o.requires:
-
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o.requires
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o.provides: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o.requires
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o.provides.build
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o.provides
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o.provides.build: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o
-
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o: ../crazyflie_cpp/src/Crazyradio.cpp
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_3) "Building CXX object crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyradio.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.i: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.i"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyradio.cpp > CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.i
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.s: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.s"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyradio.cpp -o CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.s
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o.requires:
-
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o.requires
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o.provides: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o.requires
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o.provides.build
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o.provides
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o.provides.build: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o
-
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o: ../crazyflie_cpp/src/crtp.cpp
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_4) "Building CXX object crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtp.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.i: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.i"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtp.cpp > CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.i
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.s: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.s"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtp.cpp -o CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.s
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o.requires:
-
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o.requires
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o.provides: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o.requires
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o.provides.build
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o.provides
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o.provides.build: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o
-
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o: ../crazyflie_cpp/src/USBDevice.cpp
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_5) "Building CXX object crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/USBDevice.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.i: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.i"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/USBDevice.cpp > CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.i
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.s: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.s"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/USBDevice.cpp -o CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.s
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o.requires:
-
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o.requires
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o.provides: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o.requires
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o.provides.build
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o.provides
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o.provides.build: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o
-
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o: ../crazyflie_cpp/src/ITransport.cpp
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_6) "Building CXX object crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -o CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o -c /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/ITransport.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.i: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Preprocessing CXX source to CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.i"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -E /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/ITransport.cpp > CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.i
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.s: cmake_force
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green "Compiling CXX source to assembly CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.s"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && /usr/bin/c++ $(CXX_DEFINES) $(CXX_INCLUDES) $(CXX_FLAGS) -S /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/ITransport.cpp -o CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.s
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o.requires:
-
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o.requires
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o.provides: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o.requires
- $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o.provides.build
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o.provides
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o.provides.build: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o
-
-
-# Object files for target crazyflie_cpp
-crazyflie_cpp_OBJECTS = \
-"CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o" \
-"CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o" \
-"CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o" \
-"CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o" \
-"CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o" \
-"CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o"
-
-# External object files for target crazyflie_cpp
-crazyflie_cpp_EXTERNAL_OBJECTS =
-
-crazyflie_cpp/libcrazyflie_cpp.a: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o
-crazyflie_cpp/libcrazyflie_cpp.a: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o
-crazyflie_cpp/libcrazyflie_cpp.a: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o
-crazyflie_cpp/libcrazyflie_cpp.a: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o
-crazyflie_cpp/libcrazyflie_cpp.a: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o
-crazyflie_cpp/libcrazyflie_cpp.a: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o
-crazyflie_cpp/libcrazyflie_cpp.a: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make
-crazyflie_cpp/libcrazyflie_cpp.a: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/link.txt
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --green --bold --progress-dir=/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles --progress-num=$(CMAKE_PROGRESS_7) "Linking CXX static library libcrazyflie_cpp.a"
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && $(CMAKE_COMMAND) -P CMakeFiles/crazyflie_cpp.dir/cmake_clean_target.cmake
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && $(CMAKE_COMMAND) -E cmake_link_script CMakeFiles/crazyflie_cpp.dir/link.txt --verbose=$(VERBOSE)
-
-# Rule to build all files generated by this target.
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build: crazyflie_cpp/libcrazyflie_cpp.a
-
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/requires: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o.requires
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/requires: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o.requires
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/requires: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o.requires
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/requires: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o.requires
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/requires: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o.requires
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/requires: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o.requires
-
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/requires
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/clean:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp && $(CMAKE_COMMAND) -P CMakeFiles/crazyflie_cpp.dir/cmake_clean.cmake
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/clean
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(CMAKE_COMMAND) -E cmake_depends "Unix Makefiles" /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/DependInfo.cmake --color=$(COLOR)
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend
-
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/cmake_clean.cmake b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/cmake_clean.cmake
deleted file mode 100644
index 5de436f6c195b5e9a5cb9df646d03b3f4800aa6b..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/cmake_clean.cmake
+++ /dev/null
@@ -1,15 +0,0 @@
-file(REMOVE_RECURSE
- "CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o"
- "CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o"
- "CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o"
- "CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o"
- "CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o"
- "CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o"
- "libcrazyflie_cpp.pdb"
- "libcrazyflie_cpp.a"
-)
-
-# Per-language clean rules from dependency scanning.
-foreach(lang CXX)
- include(CMakeFiles/crazyflie_cpp.dir/cmake_clean_${lang}.cmake OPTIONAL)
-endforeach()
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/cmake_clean_target.cmake b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/cmake_clean_target.cmake
deleted file mode 100644
index 5107e2d43928b93a791664987984c7897ddde3e1..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/cmake_clean_target.cmake
+++ /dev/null
@@ -1,3 +0,0 @@
-file(REMOVE_RECURSE
- "libcrazyflie_cpp.a"
-)
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend.internal b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend.internal
deleted file mode 100644
index 320097a18d2763e767d5da07ccac9e5244a95e50..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend.internal
+++ /dev/null
@@ -1,35 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o
- ../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
- ../crazyflie_cpp/include/crazyflie_cpp/CrazyflieUSB.h
- ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
- ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
- ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
- ../crazyflie_cpp/include/crazyflie_cpp/crtp.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyflie.cpp
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtpBootloader.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtpNRF51.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o
- ../crazyflie_cpp/include/crazyflie_cpp/CrazyflieUSB.h
- ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
- ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/CrazyflieUSB.cpp
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o
- ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
- ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
- ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/Crazyradio.cpp
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o
- ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/ITransport.cpp
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o
- ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/USBDevice.cpp
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o
- ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
- ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
- ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
- ../crazyflie_cpp/include/crazyflie_cpp/crtp.h
- /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/src/crtp.cpp
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend.make b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend.make
deleted file mode 100644
index 30727ad53212038a0f666cdfca6137e7850e913b..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/depend.make
+++ /dev/null
@@ -1,35 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/CrazyflieUSB.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/src/Crazyflie.cpp
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/src/crtpBootloader.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o: ../crazyflie_cpp/src/crtpNRF51.h
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/CrazyflieUSB.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o: ../crazyflie_cpp/src/CrazyflieUSB.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o: ../crazyflie_cpp/src/Crazyradio.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o: ../crazyflie_cpp/src/ITransport.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o: ../crazyflie_cpp/src/USBDevice.cpp
-
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/Crazyradio.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/ITransport.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/USBDevice.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o: ../crazyflie_cpp/include/crazyflie_cpp/crtp.h
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o: ../crazyflie_cpp/src/crtp.cpp
-
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
deleted file mode 100644
index 96acd7a4e65fdf6762f18df0f3117080bf8a6220..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/flags.make
+++ /dev/null
@@ -1,10 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# compile CXX with /usr/bin/c++
-CXX_FLAGS = -Wall -Wextra -Werror -std=c++11
-
-CXX_DEFINES =
-
-CXX_INCLUDES = -I/home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp/include/crazyflie_cpp
-
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/link.txt b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/link.txt
deleted file mode 100644
index 3f2b031bcacafba2dde497d0f5879de26ff5f0ab..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/link.txt
+++ /dev/null
@@ -1,2 +0,0 @@
-/usr/bin/ar qc libcrazyflie_cpp.a CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o
-/usr/bin/ranlib libcrazyflie_cpp.a
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/progress.make b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/progress.make
deleted file mode 100644
index 3daeeb951b56ecfaef9d57b7b5febfbc362f043f..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/progress.make
+++ /dev/null
@@ -1,8 +0,0 @@
-CMAKE_PROGRESS_1 = 3
-CMAKE_PROGRESS_2 = 4
-CMAKE_PROGRESS_3 = 5
-CMAKE_PROGRESS_4 = 6
-CMAKE_PROGRESS_5 = 7
-CMAKE_PROGRESS_6 = 8
-CMAKE_PROGRESS_7 = 9
-
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o
deleted file mode 100644
index e302107e3d8d1022d4b6c9bdef62949d770ec1b7..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o and /dev/null differ
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o
deleted file mode 100644
index cf931e4558f350770e2b036b9d8146b25577b90a..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o and /dev/null differ
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o
deleted file mode 100644
index 7eb83bb95c84e834662b9879cbca21b039162a2b..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o and /dev/null differ
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o
deleted file mode 100644
index 2c15c9636f93a6ae116d588c1cc18b0152048b58..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o and /dev/null differ
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o
deleted file mode 100644
index c191bf6d791966ced8a27119662ad7a3d2718466..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o and /dev/null differ
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o
deleted file mode 100644
index a3b09a5e57954d98da6b198868dda99a343b85a9..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o and /dev/null differ
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/progress.marks b/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/progress.marks
deleted file mode 100644
index 7f8f011eb73d6043d2e6db9d2c101195ae2801f2..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/progress.marks
+++ /dev/null
@@ -1 +0,0 @@
-7
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/Makefile b/controller_test_drone_modeling/build/crazyflie_cpp/Makefile
deleted file mode 100644
index 2c1cb810a3bbac411de386d50a238fe97565b8fa..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/Makefile
+++ /dev/null
@@ -1,330 +0,0 @@
-# CMAKE generated file: DO NOT EDIT!
-# Generated by "Unix Makefiles" Generator, CMake Version 3.10
-
-# Default target executed when no arguments are given to make.
-default_target: all
-
-.PHONY : default_target
-
-# Allow only one "make -f Makefile2" at a time, but pass parallelism.
-.NOTPARALLEL:
-
-
-#=============================================================================
-# Special targets provided by cmake.
-
-# Disable implicit rules so canonical targets will work.
-.SUFFIXES:
-
-
-# Remove some rules from gmake that .SUFFIXES does not remove.
-SUFFIXES =
-
-.SUFFIXES: .hpux_make_needs_suffix_list
-
-
-# Suppress display of executed commands.
-$(VERBOSE).SILENT:
-
-
-# A target that is always out of date.
-cmake_force:
-
-.PHONY : cmake_force
-
-#=============================================================================
-# Set environment variables for the build.
-
-# The shell in which to execute make rules.
-SHELL = /bin/sh
-
-# The CMake executable.
-CMAKE_COMMAND = /usr/bin/cmake
-
-# The command to remove a file.
-RM = /usr/bin/cmake -E remove -f
-
-# Escaping for special characters.
-EQUALS = =
-
-# The top-level source directory on which CMake was run.
-CMAKE_SOURCE_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling
-
-# The top-level build directory on which CMake was run.
-CMAKE_BINARY_DIR = /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build
-
-#=============================================================================
-# Targets provided globally by CMake.
-
-# Special rule for the target rebuild_cache
-rebuild_cache:
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "Running CMake to regenerate build system..."
- /usr/bin/cmake -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR)
-.PHONY : rebuild_cache
-
-# Special rule for the target rebuild_cache
-rebuild_cache/fast: rebuild_cache
-
-.PHONY : rebuild_cache/fast
-
-# Special rule for the target edit_cache
-edit_cache:
- @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --cyan "No interactive CMake dialog available..."
- /usr/bin/cmake -E echo No\ interactive\ CMake\ dialog\ available.
-.PHONY : edit_cache
-
-# Special rule for the target edit_cache
-edit_cache/fast: edit_cache
-
-.PHONY : edit_cache/fast
-
-# The main all target
-all: cmake_check_build_system
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/crazyflie_cpp/CMakeFiles/progress.marks
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f CMakeFiles/Makefile2 crazyflie_cpp/all
- $(CMAKE_COMMAND) -E cmake_progress_start /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build/CMakeFiles 0
-.PHONY : all
-
-# The main clean target
-clean:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f CMakeFiles/Makefile2 crazyflie_cpp/clean
-.PHONY : clean
-
-# The main clean target
-clean/fast: clean
-
-.PHONY : clean/fast
-
-# Prepare targets for installation.
-preinstall: all
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f CMakeFiles/Makefile2 crazyflie_cpp/preinstall
-.PHONY : preinstall
-
-# Prepare targets for installation.
-preinstall/fast:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f CMakeFiles/Makefile2 crazyflie_cpp/preinstall
-.PHONY : preinstall/fast
-
-# clear depends
-depend:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 1
-.PHONY : depend
-
-# Convenience name for target.
-crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/rule:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f CMakeFiles/Makefile2 crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/rule
-.PHONY : crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/rule
-
-# Convenience name for target.
-crazyflie_cpp: crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/rule
-
-.PHONY : crazyflie_cpp
-
-# fast build rule for target.
-crazyflie_cpp/fast:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build
-.PHONY : crazyflie_cpp/fast
-
-src/Crazyflie.o: src/Crazyflie.cpp.o
-
-.PHONY : src/Crazyflie.o
-
-# target to build an object file
-src/Crazyflie.cpp.o:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.o
-.PHONY : src/Crazyflie.cpp.o
-
-src/Crazyflie.i: src/Crazyflie.cpp.i
-
-.PHONY : src/Crazyflie.i
-
-# target to preprocess a source file
-src/Crazyflie.cpp.i:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.i
-.PHONY : src/Crazyflie.cpp.i
-
-src/Crazyflie.s: src/Crazyflie.cpp.s
-
-.PHONY : src/Crazyflie.s
-
-# target to generate assembly for a file
-src/Crazyflie.cpp.s:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyflie.cpp.s
-.PHONY : src/Crazyflie.cpp.s
-
-src/CrazyflieUSB.o: src/CrazyflieUSB.cpp.o
-
-.PHONY : src/CrazyflieUSB.o
-
-# target to build an object file
-src/CrazyflieUSB.cpp.o:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.o
-.PHONY : src/CrazyflieUSB.cpp.o
-
-src/CrazyflieUSB.i: src/CrazyflieUSB.cpp.i
-
-.PHONY : src/CrazyflieUSB.i
-
-# target to preprocess a source file
-src/CrazyflieUSB.cpp.i:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.i
-.PHONY : src/CrazyflieUSB.cpp.i
-
-src/CrazyflieUSB.s: src/CrazyflieUSB.cpp.s
-
-.PHONY : src/CrazyflieUSB.s
-
-# target to generate assembly for a file
-src/CrazyflieUSB.cpp.s:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/CrazyflieUSB.cpp.s
-.PHONY : src/CrazyflieUSB.cpp.s
-
-src/Crazyradio.o: src/Crazyradio.cpp.o
-
-.PHONY : src/Crazyradio.o
-
-# target to build an object file
-src/Crazyradio.cpp.o:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.o
-.PHONY : src/Crazyradio.cpp.o
-
-src/Crazyradio.i: src/Crazyradio.cpp.i
-
-.PHONY : src/Crazyradio.i
-
-# target to preprocess a source file
-src/Crazyradio.cpp.i:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.i
-.PHONY : src/Crazyradio.cpp.i
-
-src/Crazyradio.s: src/Crazyradio.cpp.s
-
-.PHONY : src/Crazyradio.s
-
-# target to generate assembly for a file
-src/Crazyradio.cpp.s:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/Crazyradio.cpp.s
-.PHONY : src/Crazyradio.cpp.s
-
-src/ITransport.o: src/ITransport.cpp.o
-
-.PHONY : src/ITransport.o
-
-# target to build an object file
-src/ITransport.cpp.o:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.o
-.PHONY : src/ITransport.cpp.o
-
-src/ITransport.i: src/ITransport.cpp.i
-
-.PHONY : src/ITransport.i
-
-# target to preprocess a source file
-src/ITransport.cpp.i:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.i
-.PHONY : src/ITransport.cpp.i
-
-src/ITransport.s: src/ITransport.cpp.s
-
-.PHONY : src/ITransport.s
-
-# target to generate assembly for a file
-src/ITransport.cpp.s:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/ITransport.cpp.s
-.PHONY : src/ITransport.cpp.s
-
-src/USBDevice.o: src/USBDevice.cpp.o
-
-.PHONY : src/USBDevice.o
-
-# target to build an object file
-src/USBDevice.cpp.o:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.o
-.PHONY : src/USBDevice.cpp.o
-
-src/USBDevice.i: src/USBDevice.cpp.i
-
-.PHONY : src/USBDevice.i
-
-# target to preprocess a source file
-src/USBDevice.cpp.i:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.i
-.PHONY : src/USBDevice.cpp.i
-
-src/USBDevice.s: src/USBDevice.cpp.s
-
-.PHONY : src/USBDevice.s
-
-# target to generate assembly for a file
-src/USBDevice.cpp.s:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/USBDevice.cpp.s
-.PHONY : src/USBDevice.cpp.s
-
-src/crtp.o: src/crtp.cpp.o
-
-.PHONY : src/crtp.o
-
-# target to build an object file
-src/crtp.cpp.o:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.o
-.PHONY : src/crtp.cpp.o
-
-src/crtp.i: src/crtp.cpp.i
-
-.PHONY : src/crtp.i
-
-# target to preprocess a source file
-src/crtp.cpp.i:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.i
-.PHONY : src/crtp.cpp.i
-
-src/crtp.s: src/crtp.cpp.s
-
-.PHONY : src/crtp.s
-
-# target to generate assembly for a file
-src/crtp.cpp.s:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(MAKE) -f crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/build.make crazyflie_cpp/CMakeFiles/crazyflie_cpp.dir/src/crtp.cpp.s
-.PHONY : src/crtp.cpp.s
-
-# Help Target
-help:
- @echo "The following are some of the valid targets for this Makefile:"
- @echo "... all (the default if no target is provided)"
- @echo "... clean"
- @echo "... depend"
- @echo "... rebuild_cache"
- @echo "... crazyflie_cpp"
- @echo "... edit_cache"
- @echo "... src/Crazyflie.o"
- @echo "... src/Crazyflie.i"
- @echo "... src/Crazyflie.s"
- @echo "... src/CrazyflieUSB.o"
- @echo "... src/CrazyflieUSB.i"
- @echo "... src/CrazyflieUSB.s"
- @echo "... src/Crazyradio.o"
- @echo "... src/Crazyradio.i"
- @echo "... src/Crazyradio.s"
- @echo "... src/ITransport.o"
- @echo "... src/ITransport.i"
- @echo "... src/ITransport.s"
- @echo "... src/USBDevice.o"
- @echo "... src/USBDevice.i"
- @echo "... src/USBDevice.s"
- @echo "... src/crtp.o"
- @echo "... src/crtp.i"
- @echo "... src/crtp.s"
-.PHONY : help
-
-
-
-#=============================================================================
-# Special targets to cleanup operation of make.
-
-# Special rule to run CMake to check the build system integrity.
-# No rule that depends on this can have commands that come from listfiles
-# because they might be regenerated.
-cmake_check_build_system:
- cd /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/build && $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0
-.PHONY : cmake_check_build_system
-
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/cmake_install.cmake b/controller_test_drone_modeling/build/crazyflie_cpp/cmake_install.cmake
deleted file mode 100644
index 8ed8433b41d2d118c75bf5ea375d31af743e1dc7..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/crazyflie_cpp/cmake_install.cmake
+++ /dev/null
@@ -1,39 +0,0 @@
-# Install script for directory: /home/spot/masters_thesis_stevedan/code/control/controller_test_drone_modeling/crazyflie_cpp
-
-# Set the install prefix
-if(NOT DEFINED CMAKE_INSTALL_PREFIX)
- set(CMAKE_INSTALL_PREFIX "/usr/local")
-endif()
-string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
-# Set the install configuration name.
-if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME)
- if(BUILD_TYPE)
- string(REGEX REPLACE "^[^A-Za-z0-9_]+" ""
- CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}")
- else()
- set(CMAKE_INSTALL_CONFIG_NAME "")
- endif()
- message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"")
-endif()
-
-# Set the component getting installed.
-if(NOT CMAKE_INSTALL_COMPONENT)
- if(COMPONENT)
- message(STATUS "Install component: \"${COMPONENT}\"")
- set(CMAKE_INSTALL_COMPONENT "${COMPONENT}")
- else()
- set(CMAKE_INSTALL_COMPONENT)
- endif()
-endif()
-
-# Install shared libraries without execute permission?
-if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE)
- set(CMAKE_INSTALL_SO_NO_EXE "1")
-endif()
-
-# Is this installation the result of a crosscompile?
-if(NOT DEFINED CMAKE_CROSSCOMPILING)
- set(CMAKE_CROSSCOMPILING "FALSE")
-endif()
-
diff --git a/controller_test_drone_modeling/build/crazyflie_cpp/libcrazyflie_cpp.a b/controller_test_drone_modeling/build/crazyflie_cpp/libcrazyflie_cpp.a
deleted file mode 100644
index 6566a45b4464757c6a5753b7b563166eb87f5018..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/crazyflie_cpp/libcrazyflie_cpp.a and /dev/null differ
diff --git a/controller_test_drone_modeling/build/drone_modelling b/controller_test_drone_modeling/build/drone_modelling
deleted file mode 100755
index 7388cd937c7e3c7c52327fc8e5a63480b1101876..0000000000000000000000000000000000000000
Binary files a/controller_test_drone_modeling/build/drone_modelling and /dev/null differ
diff --git a/controller_test_drone_modeling/build/log3533509327.csv b/controller_test_drone_modeling/build/log3533509327.csv
deleted file mode 100644
index 223a480f4157ec7517deae27b9a844c3ba79446f..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/log3533509327.csv
+++ /dev/null
@@ -1,475 +0,0 @@
-id,type,group,name
-0,1,activeMarker,btSns
-1,7,posCtrlIndi,posRef_z
-2,1,aideck,receivebyte
-3,1,motion,motion
-4,5,motion,deltaX
-5,5,motion,deltaY
-6,7,posCtrlIndi,velRef_y
-7,7,posCtrlIndi,velRef_z
-8,7,posCtrlIndi,angS_roll
-9,1,motion,Rawsum
-10,1,motion,outlierCount
-11,7,posCtrlIndi,angF_roll
-12,7,posCtrlIndi,angF_pitch
-13,7,ring,fadeTime
-14,1,loco,mode
-15,7,posCtrlIndi,accRef_y
-16,7,loco,spiRe
-17,7,posCtrlIndi,accS_x
-18,7,ranging,distance0
-19,7,posCtrlIndi,accS_z
-20,7,ranging,distance2
-21,7,ranging,distance3
-22,7,ranging,distance4
-23,7,ranging,distance5
-24,7,posCtrlIndi,accFT_y
-25,7,ranging,distance7
-26,7,ranging,pressure0
-27,7,posCtrlIndi,accErr_y
-28,7,posCtrlIndi,accErr_z
-29,7,ranging,pressure3
-30,7,posCtrlIndi,theta_tilde
-31,7,posCtrlIndi,T_tilde
-32,7,ranging,pressure6
-33,7,ranging,pressure7
-34,7,posCtrlIndi,T_incremented
-35,7,tdoa2,d0-1
-36,7,tdoa2,d1-2
-37,7,tdoa2,d2-3
-38,7,tdoa2,d3-4
-39,7,tdoa2,d4-5
-40,7,tdoa2,d5-6
-41,7,tdoa2,d6-7
-42,7,tdoa2,cc0
-43,7,posCtl,bodyVX
-44,7,tdoa2,cc2
-45,7,tdoa2,cc3
-46,7,tdoa2,cc4
-47,7,tdoa2,cc5
-48,7,posCtl,Yi
-49,7,tdoa2,cc7
-50,7,posCtl,Zp
-51,7,posCtl,Zi
-52,2,tdoa2,dist1-2
-53,2,tdoa2,dist2-3
-54,2,tdoa2,dist3-4
-55,7,posCtl,VXd
-56,2,tdoa2,dist5-6
-57,2,tdoa2,dist6-7
-58,1,twr,rangingSuccessRate0
-59,7,posEstAlt,estimatedZ
-60,1,twr,rangingSuccessRate1
-61,1,twr,rangingPerSec1
-62,3,motor,m1
-63,1,twr,rangingPerSec2
-64,3,motor,m3
-65,1,twr,rangingPerSec3
-66,1,twr,rangingSuccessRate4
-67,2,range,back
-68,1,twr,rangingSuccessRate5
-69,1,twr,rangingPerSec5
-70,2,range,right
-71,2,oa,back
-72,7,sensfusion6,qw
-73,7,sensfusion6,qx
-74,7,sensfusion6,qy
-75,7,usd,spiWrBps
-76,7,usd,spiReBps
-77,7,sensfusion6,gravityY
-78,3,pwm,m1_pwm
-79,3,pwm,m2_pwm
-80,3,pwm,m3_pwm
-81,1,sensfusion6,isCalibrated
-82,7,pm,vbat
-83,2,pm,vbatMV
-84,7,acc,z
-85,2,pm,extVbatMV
-86,7,pm,extCurr
-87,7,baro,pressure
-88,7,ctrltarget,x
-89,1,pm,batteryLevel
-90,1,radio,rssi
-91,1,radio,isConnected
-92,5,gyro,xRaw
-93,5,gyro,yRaw
-94,7,ctrltarget,ax
-95,7,ctrltarget,ay
-96,7,gyro,yVariance
-97,7,gyro,zVariance
-98,7,gyro,x
-99,7,gyro,y
-100,7,gyro,z
-101,5,ctrltargetZ,y
-102,5,ctrltargetZ,z
-103,7,pid_attitude,roll_outD
-104,7,pid_attitude,pitch_outP
-105,7,pid_attitude,pitch_outI
-106,7,pid_attitude,pitch_outD
-107,7,pid_attitude,yaw_outP
-108,5,ctrltargetZ,az
-109,7,mag,x
-110,7,pid_rate,roll_outP
-111,7,pid_rate,roll_outI
-112,7,pid_rate,roll_outD
-113,7,stabilizer,pitch
-114,7,pid_rate,pitch_outI
-115,7,pid_rate,pitch_outD
-116,7,pid_rate,yaw_outP
-117,7,pid_rate,yaw_outI
-118,7,pid_rate,yaw_outD
-119,3,colAv,latency
-120,7,ctrlINDI,cmd_thrust
-121,7,ctrlINDI,cmd_roll
-122,7,stateEstimate,vy
-123,7,stateEstimate,vz
-124,7,ctrlINDI,r_roll
-125,7,ctrlINDI,r_pitch
-126,7,stateEstimate,az
-127,7,ctrlINDI,u_act_dyn_p
-128,7,ctrlINDI,u_act_dyn_q
-129,7,stateEstimate,yaw
-130,7,ctrlINDI,du_p
-131,7,ctrlINDI,du_q
-132,7,ctrlINDI,du_r
-133,7,ctrlINDI,ang_accel_ref_p
-134,7,ctrlINDI,ang_accel_ref_q
-135,7,ctrlINDI,ang_accel_ref_r
-136,7,ctrlINDI,rate_d[0]
-137,7,ctrlINDI,rate_d[1]
-138,7,ctrlINDI,rate_d[2]
-139,7,ctrlINDI,uf_p
-140,7,ctrlINDI,uf_q
-141,7,ctrlINDI,uf_r
-142,7,ctrlINDI,Omega_f_p
-143,7,ctrlINDI,Omega_f_q
-144,5,stateEstimateZ,rateRoll
-145,7,ctrlINDI,n_p
-146,7,ctrlINDI,n_q
-147,7,ctrlINDI,n_r
-148,1,sys,isFlying
-149,1,sys,isTumbled
-150,7,ctrlMel,cmd_pitch
-151,7,ctrlMel,cmd_yaw
-152,7,ctrlMel,r_roll
-153,7,ctrlMel,r_pitch
-154,7,ctrlMel,r_yaw
-155,7,ctrlMel,accelz
-156,7,ctrlMel,zdx
-157,7,ctrlMel,zdy
-158,7,ctrlMel,zdz
-159,7,ctrlMel,i_err_x
-160,7,ctrlMel,i_err_y
-161,7,ctrlMel,i_err_z
-162,7,controller,cmd_thrust
-163,7,controller,cmd_roll
-164,7,controller,cmd_pitch
-165,7,controller,cmd_yaw
-166,7,controller,r_roll
-167,7,lighthouse,rawAngle0y
-168,7,lighthouse,rawAngle1x
-169,7,lighthouse,rawAngle1y
-170,7,lighthouse,angle0x
-171,7,lighthouse,angle0y
-172,7,lighthouse,angle1x
-173,7,lighthouse,angle1y
-174,7,lighthouse,angle0x_1
-175,7,lighthouse,angle0y_1
-176,7,lighthouse,angle1x_1
-177,7,lighthouse,angle1y_1
-178,7,ext_pos,X
-179,7,ext_pos,Y
-180,7,ext_pos,Z
-181,7,locSrv,x
-182,7,lighthouse,angle0x_3
-183,7,lighthouse,angle0y_3
-184,7,locSrv,qx
-185,7,locSrv,qy
-186,7,locSrv,qz
-187,7,locSrv,qw
-188,2,locSrvZ,tick
-189,2,crtp,rxRate
-190,2,crtp,txRate
-191,1,kalman,inFlight
-192,7,kalman,stateX
-193,7,kalman,stateY
-194,7,kalman,stateZ
-195,7,kalman,statePX
-196,7,kalman,statePY
-197,7,kalman,statePZ
-198,7,kalman,stateD0
-199,7,kalman,stateD1
-200,7,kalman,stateD2
-201,7,kalman,varX
-202,7,kalman,varY
-203,7,kalman,varZ
-204,7,kalman,varPX
-205,7,kalman,varPY
-206,7,kalman,varPZ
-207,7,kalman,varD0
-208,7,kalman,varD1
-209,7,kalman,varD2
-210,7,kalman,q0
-211,7,kalman,q1
-212,7,kalman,q2
-213,7,kalman,q3
-214,7,kalman,rtUpdate
-215,7,kalman,rtPred
-216,7,kalman,rtFinal
-217,6,outlierf,lhWin
-218,6,outlierf,bucket0
-219,6,outlierf,bucket1
-220,6,outlierf,bucket2
-221,6,outlierf,bucket3
-222,6,outlierf,bucket4
-223,7,outlierf,accLev
-224,7,outlierf,errD
-225,7,estimator,rtApnd
-226,7,estimator,rtRej
-227,7,extrx,thrust
-228,7,extrx,roll
-229,7,extrx,pitch
-230,7,extrx,yawRate
-231,7,extrx,zVel
-232,1,extrx,AltHold
-233,1,extrx,Arm
-234,2,extrx_raw,ch0
-235,2,extrx_raw,ch1
-236,2,extrx_raw,ch2
-237,2,extrx_raw,ch3
-238,2,extrx_raw,ch4
-239,2,extrx_raw,ch5
-240,2,extrx_raw,ch6
-241,2,extrx_raw,ch7
-242,7,health,motorVarXM1
-243,7,health,motorVarYM1
-244,7,health,motorVarXM2
-245,7,health,motorVarYM2
-246,7,health,motorVarXM3
-247,7,health,motorVarYM3
-248,7,health,motorVarXM4
-249,7,health,motorVarYM4
-250,1,health,motorPass
-251,7,health,batterySag
-252,1,health,batteryPass
-253,2,health,motorTestCount
-254,3,memTst,errCntW
-255,7,posCtrlIndi,posRef_x
-256,7,posCtrlIndi,posRef_y
-257,7,posCtrlIndi,posRef_z
-258,7,posCtrlIndi,velS_x
-259,7,posCtrlIndi,velS_y
-260,7,posCtrlIndi,velS_z
-261,7,posCtrlIndi,velRef_x
-262,7,posCtrlIndi,velRef_y
-263,7,posCtrlIndi,velRef_z
-264,7,posCtrlIndi,angS_roll
-265,7,posCtrlIndi,angS_pitch
-266,7,posCtrlIndi,angS_yaw
-267,7,posCtrlIndi,angF_roll
-268,7,posCtrlIndi,angF_pitch
-269,7,posCtrlIndi,angF_yaw
-270,7,posCtrlIndi,accRef_x
-271,7,posCtrlIndi,accRef_y
-272,7,posCtrlIndi,accRef_z
-273,7,posCtrlIndi,accS_x
-274,7,posCtrlIndi,accS_y
-275,7,posCtrlIndi,accS_z
-276,7,posCtrlIndi,accF_x
-277,7,posCtrlIndi,accF_y
-278,7,posCtrlIndi,accF_z
-279,7,posCtrlIndi,accFT_x
-280,7,posCtrlIndi,accFT_y
-281,7,posCtrlIndi,accFT_z
-282,7,posCtrlIndi,accErr_x
-283,7,posCtrlIndi,accErr_y
-284,7,posCtrlIndi,accErr_z
-285,7,posCtrlIndi,phi_tilde
-286,7,posCtrlIndi,theta_tilde
-287,7,posCtrlIndi,T_tilde
-288,7,posCtrlIndi,T_inner
-289,7,posCtrlIndi,T_inner_f
-290,7,posCtrlIndi,T_incremented
-291,7,posCtrlIndi,cmd_phi
-292,7,posCtrlIndi,cmd_theta
-293,7,posCtl,targetVX
-294,7,posCtl,targetVY
-295,7,posCtl,targetVZ
-296,7,posCtl,targetX
-297,7,posCtl,targetY
-298,7,posCtl,targetZ
-299,7,posCtl,bodyVX
-300,7,posCtl,bodyVY
-301,7,posCtl,bodyX
-302,7,posCtl,bodyY
-303,7,posCtl,Yp
-304,7,posCtl,Yi
-305,7,posCtl,Yd
-306,7,posCtl,Zp
-307,7,posCtl,Zi
-308,7,posCtl,Zd
-309,7,posCtl,VXp
-310,7,posCtl,VXi
-311,7,posCtl,VXd
-312,7,posCtl,VZp
-313,7,posCtl,VZi
-314,7,posCtl,VZd
-315,7,posEstAlt,estimatedZ
-316,7,posEstAlt,estVZ
-317,7,posEstAlt,velocityZ
-318,3,motor,m1
-319,3,motor,m2
-320,3,motor,m3
-321,3,motor,m4
-322,2,range,front
-323,2,range,back
-324,2,range,up
-325,2,range,left
-326,2,range,right
-327,2,range,zrange
-328,7,sensfusion6,qw
-329,7,sensfusion6,qx
-330,7,sensfusion6,qy
-331,7,sensfusion6,qz
-332,7,sensfusion6,gravityX
-333,7,sensfusion6,gravityY
-334,7,sensfusion6,gravityZ
-335,7,sensfusion6,accZbase
-336,1,sensfusion6,isInit
-337,1,sensfusion6,isCalibrated
-338,7,acc,x
-339,7,acc,y
-340,7,acc,z
-341,7,baro,asl
-342,7,baro,temp
-343,7,baro,pressure
-344,7,ctrltarget,x
-345,7,ctrltarget,y
-346,7,ctrltarget,z
-347,7,ctrltarget,vx
-348,7,ctrltarget,vy
-349,7,ctrltarget,vz
-350,7,ctrltarget,ax
-351,7,ctrltarget,ay
-352,7,ctrltarget,az
-353,7,ctrltarget,roll
-354,7,ctrltarget,pitch
-355,7,ctrltarget,yaw
-356,5,ctrltargetZ,x
-357,5,ctrltargetZ,y
-358,5,ctrltargetZ,z
-359,5,ctrltargetZ,vx
-360,5,ctrltargetZ,vy
-361,5,ctrltargetZ,vz
-362,5,ctrltargetZ,ax
-363,5,ctrltargetZ,ay
-364,5,ctrltargetZ,az
-365,7,mag,x
-366,7,mag,y
-367,7,mag,z
-368,7,stabilizer,roll
-369,7,stabilizer,pitch
-370,7,stabilizer,yaw
-371,7,stabilizer,thrust
-372,7,stabilizer,rtStab
-373,3,stabilizer,intToOut
-374,7,stateEstimate,x
-375,7,stateEstimate,y
-376,7,stateEstimate,z
-377,7,stateEstimate,vx
-378,7,stateEstimate,vy
-379,7,stateEstimate,vz
-380,7,stateEstimate,ax
-381,7,stateEstimate,ay
-382,7,stateEstimate,az
-383,7,stateEstimate,roll
-384,7,stateEstimate,pitch
-385,7,stateEstimate,yaw
-386,7,stateEstimate,qx
-387,7,stateEstimate,qy
-388,7,stateEstimate,qz
-389,7,stateEstimate,qw
-390,5,stateEstimateZ,x
-391,5,stateEstimateZ,y
-392,5,stateEstimateZ,z
-393,5,stateEstimateZ,vx
-394,5,stateEstimateZ,vy
-395,5,stateEstimateZ,vz
-396,5,stateEstimateZ,ax
-397,5,stateEstimateZ,ay
-398,5,stateEstimateZ,az
-399,3,stateEstimateZ,quat
-400,5,stateEstimateZ,rateRoll
-401,5,stateEstimateZ,ratePitch
-402,5,stateEstimateZ,rateYaw
-403,1,sys,canfly
-404,1,sys,isFlying
-405,1,sys,isTumbled
-406,4,sys,armed
-407,7,tdoaEngine,stRx
-408,7,tdoaEngine,stEst
-409,7,tdoaEngine,stTime
-410,7,tdoaEngine,stFound
-411,7,tdoaEngine,stCc
-412,7,tdoaEngine,stHit
-413,7,tdoaEngine,stMiss
-414,7,tdoaEngine,cc
-415,2,tdoaEngine,tof
-416,7,tdoaEngine,tdoa
-417,7,kalman_pred,predNX
-418,7,kalman_pred,predNY
-419,7,kalman_pred,measNX
-420,7,kalman_pred,measNY
-421,1,lighthouse,validAngles
-422,7,lighthouse,rawAngle0x
-423,7,lighthouse,rawAngle0y
-424,7,lighthouse,rawAngle1x
-425,7,lighthouse,rawAngle1y
-426,7,lighthouse,angle0x
-427,7,lighthouse,angle0y
-428,7,lighthouse,angle1x
-429,7,lighthouse,angle1y
-430,7,lighthouse,angle0x_1
-431,7,lighthouse,angle0y_1
-432,7,lighthouse,angle1x_1
-433,7,lighthouse,angle1y_1
-434,7,lighthouse,angle0x_2
-435,7,lighthouse,angle0y_2
-436,7,lighthouse,angle1x_2
-437,7,lighthouse,angle1y_2
-438,7,lighthouse,angle0x_3
-439,7,lighthouse,angle0y_3
-440,7,lighthouse,angle1x_3
-441,7,lighthouse,angle1y_3
-442,7,lighthouse,rawAngle0xlh2
-443,7,lighthouse,rawAngle0ylh2
-444,7,lighthouse,rawAngle1xlh2
-445,7,lighthouse,rawAngle1ylh2
-446,7,lighthouse,angle0x_0lh2
-447,7,lighthouse,angle0y_0lh2
-448,7,lighthouse,angle1x_0lh2
-449,7,lighthouse,angle1y_0lh2
-450,7,lighthouse,serRt
-451,7,lighthouse,frmRt
-452,7,lighthouse,cycleRt
-453,7,lighthouse,bs0Rt
-454,7,lighthouse,bs1Rt
-455,2,lighthouse,width0
-456,2,lighthouse,width1
-457,2,lighthouse,width2
-458,2,lighthouse,width3
-459,1,lighthouse,comSync
-460,2,lighthouse,bsReceive
-461,2,lighthouse,bsActive
-462,2,lighthouse,bsCalUd
-463,2,lighthouse,bsCalCon
-464,1,lighthouse,status
-465,7,lighthouse,posRt
-466,7,lighthouse,estBs0Rt
-467,7,lighthouse,estBs1Rt
-468,7,lighthouse,x
-469,7,lighthouse,y
-470,7,lighthouse,z
-471,7,lighthouse,delta
-472,2,lighthouse,bsGeoVal
-473,2,lighthouse,bsCalVal
diff --git a/controller_test_drone_modeling/build/log3761402556.csv b/controller_test_drone_modeling/build/log3761402556.csv
deleted file mode 100644
index b72d1b1e1a07d217408e76b3485d1994a1d08712..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/build/log3761402556.csv
+++ /dev/null
@@ -1,473 +0,0 @@
-id,type,group,name
-0,5,gyro,xRaw
-1,5,gyro,yRaw
-2,5,gyro,zRaw
-3,7,gyro,xVariance
-4,7,gyro,yVariance
-5,7,gyro,zVariance
-6,3,pwm,m1_pwm
-7,3,pwm,m2_pwm
-8,3,pwm,m3_pwm
-9,3,pwm,m4_pwm
-10,2,crtp,rxRate
-11,2,crtp,txRate
-12,7,pm,vbat
-13,2,pm,vbatMV
-14,7,pm,extVbat
-15,2,pm,extVbatMV
-16,7,pm,extCurr
-17,7,pm,chargeCurrent
-18,3,colAv,latency
-19,1,pm,batteryLevel
-20,1,radio,rssi
-21,1,radio,isConnected
-22,4,sys,armed
-23,2,extrx,ch0
-24,2,extrx,ch1
-25,2,extrx,ch2
-26,2,extrx,ch3
-27,2,extrx,thrust
-28,7,extrx,roll
-29,7,extrx,pitch
-30,7,extrx,yaw
-31,3,memTst,errCntW
-32,2,range,front
-33,2,range,back
-34,2,range,up
-35,2,range,left
-36,2,range,right
-37,2,range,zrange
-38,1,sys,canfly
-39,1,sys,isFlying
-40,1,sys,isTumbled
-41,7,ext_pos,X
-42,7,ext_pos,Y
-43,7,ext_pos,Z
-44,7,locSrv,x
-45,7,locSrv,y
-46,7,kalman,varPZ
-47,7,locSrv,qx
-48,7,locSrv,qy
-49,7,locSrv,qz
-50,7,locSrv,qw
-51,2,locSrvZ,tick
-52,7,pid_attitude,roll_outP
-53,7,pid_attitude,roll_outI
-54,7,pid_attitude,roll_outD
-55,7,pid_attitude,pitch_outP
-56,7,pid_attitude,pitch_outI
-57,7,pid_attitude,pitch_outD
-58,7,pid_attitude,yaw_outP
-59,7,pid_attitude,yaw_outI
-60,7,pid_attitude,yaw_outD
-61,7,pid_rate,roll_outP
-62,7,pid_rate,roll_outI
-63,7,pid_rate,roll_outD
-64,7,pid_rate,pitch_outP
-65,7,pid_rate,pitch_outI
-66,7,pid_rate,pitch_outD
-67,7,pid_rate,yaw_outP
-68,7,pid_rate,yaw_outI
-69,7,pid_rate,yaw_outD
-70,7,sensfusion6,qw
-71,7,sensfusion6,qx
-72,7,sensfusion6,qy
-73,7,sensfusion6,qz
-74,7,sensfusion6,gravityX
-75,7,sensfusion6,gravityY
-76,7,sensfusion6,gravityZ
-77,7,sensfusion6,accZbase
-78,1,sensfusion6,isInit
-79,1,sensfusion6,isCalibrated
-80,7,acc,x
-81,7,acc,y
-82,7,acc,z
-83,7,baro,asl
-84,7,baro,temp
-85,7,baro,pressure
-86,5,controller,ctr_yaw
-87,7,ctrltarget,x
-88,7,ctrltarget,y
-89,7,ctrltarget,z
-90,7,ctrltarget,vx
-91,7,ctrltarget,vy
-92,7,ctrltarget,vz
-93,7,ctrltarget,ax
-94,7,ctrltarget,ay
-95,7,ctrltarget,az
-96,7,ctrltarget,roll
-97,7,ctrltarget,pitch
-98,7,ctrltarget,yaw
-99,5,ctrltargetZ,x
-100,5,ctrltargetZ,y
-101,5,ctrltargetZ,z
-102,5,ctrltargetZ,vx
-103,5,ctrltargetZ,vy
-104,5,ctrltargetZ,vz
-105,5,ctrltargetZ,ax
-106,5,ctrltargetZ,ay
-107,5,ctrltargetZ,az
-108,7,gyro,x
-109,7,gyro,y
-110,7,gyro,z
-111,7,mag,x
-112,7,tdoa2,cc0
-113,7,mag,z
-114,7,stabilizer,roll
-115,7,stabilizer,pitch
-116,7,stabilizer,yaw
-117,7,stabilizer,thrust
-118,7,stabilizer,rtStab
-119,3,stabilizer,intToOut
-120,7,stateEstimate,x
-121,7,stateEstimate,y
-122,7,stateEstimate,z
-123,7,stateEstimate,vx
-124,7,stateEstimate,vy
-125,7,stateEstimate,vz
-126,7,stateEstimate,ax
-127,7,stateEstimate,ay
-128,7,stateEstimate,az
-129,7,stateEstimate,roll
-130,7,stateEstimate,pitch
-131,7,stateEstimate,yaw
-132,7,stateEstimate,qx
-133,7,stateEstimate,qy
-134,7,stateEstimate,qz
-135,7,stateEstimate,qw
-136,5,stateEstimateZ,x
-137,5,stateEstimateZ,y
-138,5,stateEstimateZ,z
-139,5,stateEstimateZ,vx
-140,5,stateEstimateZ,vy
-141,5,stateEstimateZ,vz
-142,5,stateEstimateZ,ax
-143,5,stateEstimateZ,ay
-144,5,stateEstimateZ,az
-145,3,stateEstimateZ,quat
-146,5,stateEstimateZ,rateRoll
-147,5,stateEstimateZ,ratePitch
-148,5,stateEstimateZ,rateYaw
-149,7,posEstAlt,estimatedZ
-150,7,posEstAlt,estVZ
-151,7,posEstAlt,velocityZ
-152,7,posCtl,targetVX
-153,7,posCtl,targetVY
-154,7,posCtl,targetVZ
-155,7,posCtl,targetX
-156,7,posCtl,targetY
-157,7,posCtl,targetZ
-158,7,posCtl,Xp
-159,7,posCtl,Xi
-160,7,posCtl,Xd
-161,7,posCtl,Yp
-162,7,posCtl,Yi
-163,7,posCtl,Yd
-164,7,posCtl,Zp
-165,7,posCtl,Zi
-166,7,posCtl,Zd
-167,7,posCtl,VXp
-168,7,posCtl,VXi
-169,7,posCtl,VXd
-170,7,posCtl,VZp
-171,7,posCtl,VZi
-172,7,posCtl,VZd
-173,7,posCtrlIndi,posRef_x
-174,7,posCtrlIndi,posRef_y
-175,7,posCtrlIndi,posRef_z
-176,7,posCtrlIndi,velS_x
-177,7,posCtrlIndi,velS_y
-178,7,posCtrlIndi,velS_z
-179,7,posCtrlIndi,velRef_x
-180,7,posCtrlIndi,velRef_y
-181,7,posCtrlIndi,velRef_z
-182,7,posCtrlIndi,angS_roll
-183,7,posCtrlIndi,angS_pitch
-184,7,posCtrlIndi,angS_yaw
-185,7,posCtrlIndi,angF_roll
-186,7,posCtrlIndi,angF_pitch
-187,7,posCtrlIndi,angF_yaw
-188,7,posCtrlIndi,accRef_x
-189,7,posCtrlIndi,accRef_y
-190,7,posCtrlIndi,accRef_z
-191,7,posCtrlIndi,accS_x
-192,7,posCtrlIndi,accS_y
-193,7,posCtrlIndi,accS_z
-194,7,posCtrlIndi,accF_x
-195,7,posCtrlIndi,accF_y
-196,7,posCtrlIndi,accF_z
-197,7,posCtrlIndi,accFT_x
-198,7,posCtrlIndi,accFT_y
-199,7,posCtrlIndi,accFT_z
-200,7,posCtrlIndi,accErr_x
-201,7,posCtrlIndi,accErr_y
-202,7,posCtrlIndi,accErr_z
-203,7,posCtrlIndi,phi_tilde
-204,7,posCtrlIndi,theta_tilde
-205,7,posCtrlIndi,T_tilde
-206,7,posCtrlIndi,T_inner
-207,7,posCtrlIndi,T_inner_f
-208,7,posCtrlIndi,T_incremented
-209,7,posCtrlIndi,cmd_phi
-210,7,posCtrlIndi,cmd_theta
-211,7,estimator,rtApnd
-212,7,estimator,rtRej
-213,7,controller,cmd_thrust
-214,7,controller,cmd_roll
-215,7,controller,cmd_pitch
-216,7,controller,cmd_yaw
-217,7,controller,r_roll
-218,7,controller,r_pitch
-219,7,controller,r_yaw
-220,7,controller,accelz
-221,7,controller,actuatorThrust
-222,7,controller,roll
-223,7,controller,pitch
-224,7,controller,yaw
-225,7,controller,rollRate
-226,7,controller,pitchRate
-227,7,controller,yawRate
-228,7,ctrlMel,cmd_thrust
-229,7,ctrlMel,cmd_roll
-230,7,ctrlMel,cmd_pitch
-231,7,ctrlMel,cmd_yaw
-232,7,ctrlMel,r_roll
-233,7,ctrlMel,r_pitch
-234,7,ctrlMel,r_yaw
-235,7,ctrlMel,accelz
-236,7,ctrlMel,zdx
-237,7,ctrlMel,zdy
-238,7,ctrlMel,zdz
-239,7,ctrlMel,i_err_x
-240,7,ctrlMel,i_err_y
-241,7,ctrlMel,i_err_z
-242,7,ctrlINDI,cmd_thrust
-243,7,ctrlINDI,cmd_roll
-244,7,ctrlINDI,cmd_pitch
-245,7,ctrlINDI,cmd_yaw
-246,7,ctrlINDI,r_roll
-247,7,ctrlINDI,r_pitch
-248,7,ctrlINDI,r_yaw
-249,7,ctrlINDI,u_act_dyn_p
-250,7,ctrlINDI,u_act_dyn_q
-251,7,ctrlINDI,u_act_dyn_r
-252,7,ctrlINDI,du_p
-253,7,ctrlINDI,du_q
-254,7,ctrlINDI,du_r
-255,7,ctrlINDI,ang_accel_ref_p
-256,7,ctrlINDI,ang_accel_ref_q
-257,7,ctrlINDI,ang_accel_ref_r
-258,7,ctrlINDI,rate_d[0]
-259,7,ctrlINDI,rate_d[1]
-260,7,ctrlINDI,rate_d[2]
-261,7,ctrlINDI,uf_p
-262,7,ctrlINDI,uf_q
-263,7,ctrlINDI,uf_r
-264,7,ctrlINDI,Omega_f_p
-265,7,ctrlINDI,Omega_f_q
-266,7,ctrlINDI,Omega_f_r
-267,7,ctrlINDI,n_p
-268,7,ctrlINDI,n_q
-269,7,ctrlINDI,n_r
-270,3,motor,m1
-271,3,motor,m2
-272,3,motor,m3
-273,3,motor,m4
-274,3,colAv,latency
-275,7,health,motorVarXM1
-276,7,health,motorVarYM1
-277,7,health,motorVarXM2
-278,7,health,motorVarYM2
-279,7,health,motorVarXM3
-280,7,health,motorVarYM3
-281,7,health,motorVarXM4
-282,7,health,motorVarYM4
-283,1,health,motorPass
-284,7,health,batterySag
-285,1,health,batteryPass
-286,2,health,motorTestCount
-287,1,kalman,inFlight
-288,7,kalman,stateX
-289,7,kalman,stateY
-290,7,kalman,stateZ
-291,7,kalman,statePX
-292,7,kalman,statePY
-293,7,kalman,statePZ
-294,7,kalman,stateD0
-295,7,kalman,stateD1
-296,7,kalman,stateD2
-297,7,kalman,varX
-298,7,kalman,varY
-299,7,kalman,varZ
-300,7,kalman,varPX
-301,7,kalman,varPY
-302,7,kalman,varPZ
-303,7,kalman,varD0
-304,7,kalman,varD1
-305,7,kalman,varD2
-306,7,kalman,q0
-307,7,kalman,q1
-308,7,kalman,q2
-309,7,kalman,q3
-310,7,kalman,rtUpdate
-311,7,kalman,rtPred
-312,7,kalman,rtFinal
-313,6,outlierf,lhWin
-314,7,kalman_pred,predNX
-315,7,kalman_pred,predNY
-316,7,kalman_pred,measNX
-317,7,kalman_pred,measNY
-318,7,ring,fadeTime
-319,6,gps,lat
-320,6,gps,lon
-321,7,gps,hMSL
-322,7,gps,hAcc
-323,6,gps,nsat
-324,6,gps,fix
-325,7,usd,spiWrBps
-326,7,usd,spiReBps
-327,7,usd,fatWrBps
-328,1,loco,mode
-329,7,loco,spiWr
-330,7,loco,spiRe
-331,2,ranging,state
-332,7,ranging,distance0
-333,7,ranging,distance1
-334,7,ranging,distance2
-335,7,ranging,distance3
-336,7,ranging,distance4
-337,7,ranging,distance5
-338,7,ranging,distance6
-339,7,ranging,distance7
-340,7,ranging,pressure0
-341,7,ranging,pressure1
-342,7,ranging,pressure2
-343,7,ranging,pressure3
-344,7,ranging,pressure4
-345,7,ranging,pressure5
-346,7,ranging,pressure6
-347,7,ranging,pressure7
-348,1,twr,rangingSuccessRate0
-349,1,twr,rangingPerSec0
-350,1,twr,rangingSuccessRate1
-351,1,twr,rangingPerSec1
-352,1,twr,rangingSuccessRate2
-353,1,twr,rangingPerSec2
-354,1,twr,rangingSuccessRate3
-355,1,twr,rangingPerSec3
-356,1,twr,rangingSuccessRate4
-357,1,twr,rangingPerSec4
-358,1,twr,rangingSuccessRate5
-359,1,twr,rangingPerSec5
-360,7,tdoa2,d7-0
-361,7,tdoa2,d0-1
-362,7,tdoa2,d1-2
-363,7,tdoa2,d2-3
-364,7,tdoa2,d3-4
-365,7,tdoa2,d4-5
-366,7,tdoa2,d5-6
-367,7,tdoa2,d6-7
-368,7,tdoa2,cc0
-369,7,tdoa2,cc1
-370,7,tdoa2,cc2
-371,7,tdoa2,cc3
-372,7,tdoa2,cc4
-373,7,tdoa2,cc5
-374,7,tdoa2,cc6
-375,7,tdoa2,cc7
-376,2,tdoa2,dist7-0
-377,2,tdoa2,dist0-1
-378,2,tdoa2,dist1-2
-379,2,tdoa2,dist2-3
-380,2,tdoa2,dist3-4
-381,2,tdoa2,dist4-5
-382,2,tdoa2,dist5-6
-383,2,tdoa2,dist6-7
-384,7,tdoaEngine,stRx
-385,7,tdoaEngine,stEst
-386,7,tdoaEngine,stTime
-387,7,tdoaEngine,stFound
-388,7,tdoaEngine,stCc
-389,7,tdoaEngine,stHit
-390,7,tdoaEngine,stMiss
-391,7,tdoaEngine,cc
-392,2,tdoaEngine,tof
-393,7,tdoaEngine,tdoa
-394,6,outlierf,bucket0
-395,6,outlierf,bucket1
-396,6,outlierf,bucket2
-397,6,outlierf,bucket3
-398,6,outlierf,bucket4
-399,7,outlierf,accLev
-400,7,outlierf,errD
-401,1,motion,motion
-402,5,motion,deltaX
-403,5,motion,deltaY
-404,2,motion,shutter
-405,1,motion,maxRaw
-406,1,motion,minRaw
-407,1,motion,Rawsum
-408,1,motion,outlierCount
-409,1,motion,squal
-410,7,motion,std
-411,2,oa,front
-412,2,oa,back
-413,2,oa,up
-414,2,oa,left
-415,2,oa,right
-416,1,activeMarker,btSns
-417,1,activeMarker,i2cOk
-418,1,aideck,receivebyte
-419,1,lighthouse,validAngles
-420,7,lighthouse,rawAngle0x
-421,7,lighthouse,rawAngle0y
-422,7,lighthouse,rawAngle1x
-423,7,lighthouse,rawAngle1y
-424,7,lighthouse,angle0x
-425,7,lighthouse,angle0y
-426,7,lighthouse,angle1x
-427,7,lighthouse,angle1y
-428,7,lighthouse,angle0x_1
-429,7,lighthouse,angle0y_1
-430,7,lighthouse,angle1x_1
-431,7,lighthouse,angle1y_1
-432,7,lighthouse,angle0x_2
-433,7,lighthouse,angle0y_2
-434,7,lighthouse,angle1x_2
-435,7,lighthouse,angle1y_2
-436,7,lighthouse,angle0x_3
-437,7,lighthouse,angle0y_3
-438,7,lighthouse,angle1x_3
-439,7,lighthouse,angle1y_3
-440,7,lighthouse,rawAngle0xlh2
-441,7,lighthouse,rawAngle0ylh2
-442,7,lighthouse,rawAngle1xlh2
-443,7,lighthouse,rawAngle1ylh2
-444,7,lighthouse,angle0x_0lh2
-445,7,lighthouse,angle0y_0lh2
-446,7,lighthouse,angle1x_0lh2
-447,7,lighthouse,angle1y_0lh2
-448,7,lighthouse,serRt
-449,7,lighthouse,frmRt
-450,7,lighthouse,cycleRt
-451,7,lighthouse,bs0Rt
-452,7,lighthouse,bs1Rt
-453,2,lighthouse,width0
-454,2,lighthouse,width1
-455,2,lighthouse,width2
-456,2,lighthouse,width3
-457,1,lighthouse,comSync
-458,2,lighthouse,bsReceive
-459,2,lighthouse,bsActive
-460,2,lighthouse,bsCalUd
-461,2,lighthouse,bsCalCon
-462,1,lighthouse,status
-463,7,lighthouse,posRt
-464,7,lighthouse,estBs0Rt
-465,7,lighthouse,estBs1Rt
-466,7,lighthouse,x
-467,7,lighthouse,y
-468,7,lighthouse,z
-469,7,lighthouse,delta
-470,2,lighthouse,bsGeoVal
-471,2,lighthouse,bsCalVal
diff --git a/controller_test_drone_modeling/crazyflie_cpp/.github/workflows/cmake.yml b/controller_test_drone_modeling/crazyflie_cpp/.github/workflows/cmake.yml
new file mode 100644
index 0000000000000000000000000000000000000000..94781ab8b14bb80f0fc5560d69616d84c3a2f1e7
--- /dev/null
+++ b/controller_test_drone_modeling/crazyflie_cpp/.github/workflows/cmake.yml
@@ -0,0 +1,38 @@
+name: CMake
+
+on:
+ push:
+ branches: [ master ]
+ pull_request:
+ branches: [ master ]
+
+env:
+ # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+ BUILD_TYPE: Release
+
+jobs:
+ build:
+ # The CMake configure and build commands are platform agnostic and should work equally
+ # well on Windows or Mac. You can convert this to a matrix build if you need
+ # cross-platform coverage.
+ # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+ runs-on: ubuntu-latest
+
+ steps:
+ - uses: actions/checkout@v2
+ with:
+ # recursively checkout all submodules
+ submodules: 'recursive'
+
+ - name: install dependencies
+ run: |
+ sudo apt-get install -y libusb-1.0-0-dev
+
+ - name: Configure CMake
+ # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
+ # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
+ run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+ - name: Build
+ # Build your program with the given configuration
+ run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
diff --git a/controller_test_drone_modeling/crazyflie_cpp/.travis.yml b/controller_test_drone_modeling/crazyflie_cpp/.travis.yml
deleted file mode 100644
index 1abc26ab5bd0fc00ccb90d09731c7c4e90fd4d61..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/crazyflie_cpp/.travis.yml
+++ /dev/null
@@ -1,36 +0,0 @@
-language: cpp
-
-matrix:
- include:
- - os: linux
- dist: xenial
- - os: osx
- # osx_image: xcode7.2
-
-# dist: xenial
-
-# os:
- # - linux
- # - osx
-
-# compiler:
-# - gcc
-# - clang
-
-before_script:
- - mkdir build
- - cd build
- - cmake ..
-
-# additional linux dependencies
-addons:
- apt:
- packages:
- - libusb-1.0-0-dev
-
-# additional mac dependencies
-before_install:
- - if [[ "$TRAVIS_OS_NAME" == "osx" ]]; then brew update ; fi
- - if [[ "$TRAVIS_OS_NAME" == "osx" ]]; then brew install libusb; fi
-
-script: make
diff --git a/controller_test_drone_modeling/crazyflie_cpp/CMake.cmake b/controller_test_drone_modeling/crazyflie_cpp/CMake.cmake
deleted file mode 100644
index 3e71cafb3b8370bd4833a474c4ac1a846ac3788a..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/crazyflie_cpp/CMake.cmake
+++ /dev/null
@@ -1,28 +0,0 @@
-cmake_minimum_required(VERSION 3.1)
-project(crazyflie_cpp)
-
-find_library(USB_LIB usb-1.0)
-
-# Enable C++11 and warnings
-set(CMAKE_CXX_STANDARD 11)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-
-add_compile_options(-Wall -Wextra -Werror)
-
-include_directories(
- include/crazyflie_cpp
-)
-
-add_library(crazyflie_cpp
- src/Crazyflie.cpp
- src/CrazyflieUSB.cpp
- src/Crazyradio.cpp
- src/crtp.cpp
- src/USBDevice.cpp
- src/ITransport.cpp
-)
-
-target_link_libraries(crazyflie_cpp
- ${USB_LIB}
-)
diff --git a/controller_test_drone_modeling/crazyflie_cpp/CMakeLists.txt b/controller_test_drone_modeling/crazyflie_cpp/CMakeLists.txt
index 24bf4d548ffa029c141ca6051824f5cc9afc7080..902c9be2076f7a9dadef8ef8c93ac7f53975b90c 100644
--- a/controller_test_drone_modeling/crazyflie_cpp/CMakeLists.txt
+++ b/controller_test_drone_modeling/crazyflie_cpp/CMakeLists.txt
@@ -1,10 +1,73 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(crazyflie_cpp)
-
-if(DEFINED CATKIN_DEVEL_PREFIX)
- message("Using ROS (catkin_make)")
- include(${CMAKE_CURRENT_SOURCE_DIR}/ROS.cmake)
-else()
- message("Using standard cmake")
- include(${CMAKE_CURRENT_SOURCE_DIR}/CMake.cmake)
-endif()
+cmake_minimum_required(VERSION 3.5)
+project(crazyflie_cpp VERSION 0.0.1)
+
+find_library(USB_LIB usb-1.0)
+
+# Enable C++11 and warnings
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_EXTENSIONS OFF)
+
+add_compile_options(-Wall -Wextra -Werror)
+
+add_library(crazyflie_cpp
+ src/Crazyflie.cpp
+ src/CrazyflieUSB.cpp
+ src/Crazyradio.cpp
+ src/crtp.cpp
+ src/USBDevice.cpp
+ src/ITransport.cpp
+)
+
+target_include_directories(crazyflie_cpp
+ PUBLIC
+ $<INSTALL_INTERFACE:include/crazyflie_cpp>
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/crazyflie_cpp>
+)
+
+target_link_libraries(crazyflie_cpp
+ PRIVATE
+ ${USB_LIB}
+)
+
+include(CMakePackageConfigHelpers)
+write_basic_package_version_file(
+ crazyflie_cpp-config-version.cmake
+ VERSION ${CRAZYFLIE_CPP_VERSION}
+ COMPATIBILITY AnyNewerVersion
+)
+
+install(
+ DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/crazyflie_cpp
+ DESTINATION include
+)
+
+install(
+ TARGETS crazyflie_cpp
+ EXPORT crazyflie_cpp-targets
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib
+ RUNTIME DESTINATION bin
+ INCLUDES DESTINATION include
+)
+
+install(EXPORT crazyflie_cpp-targets
+ FILE crazyflie_cpp-targets.cmake
+ NAMESPACE crazyflie_cpp::
+ DESTINATION lib/cmake/crazyflie_cpp
+)
+
+install(
+ FILES
+ "${CMAKE_CURRENT_SOURCE_DIR}/cmake/crazyflie_cpp-config.cmake"
+ DESTINATION
+ lib/cmake/crazyflie_cpp
+)
+
+# Install ROS package information
+install(
+ FILES
+ package.xml
+ DESTINATION
+ share/crazyflie_cpp
+)
diff --git a/controller_test_drone_modeling/crazyflie_cpp/README.md b/controller_test_drone_modeling/crazyflie_cpp/README.md
index f8b2fa7ba051438bad9bfa179ce893bbc50fb50d..dc2770f402904eddd3bf4f4f5de66b89f2b0ad80 100644
--- a/controller_test_drone_modeling/crazyflie_cpp/README.md
+++ b/controller_test_drone_modeling/crazyflie_cpp/README.md
@@ -1,4 +1,4 @@
-[](https://travis-ci.org/whoenig/crazyflie_cpp)
+[](https://github.com/whoenig/crazyflie_cpp/actions/workflows/cmake.yml)
# Crazyflie_cpp
diff --git a/controller_test_drone_modeling/crazyflie_cpp/ROS.cmake b/controller_test_drone_modeling/crazyflie_cpp/ROS.cmake
deleted file mode 100644
index 06cb69907b9dae898fc476422ad692a05c1c6994..0000000000000000000000000000000000000000
--- a/controller_test_drone_modeling/crazyflie_cpp/ROS.cmake
+++ /dev/null
@@ -1,95 +0,0 @@
-cmake_minimum_required(VERSION 2.8.3)
-project(crazyflie_cpp)
-find_package(catkin)
-
-find_library(USB_LIB usb-1.0)
-# Enable C++11
-SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if you package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
- INCLUDE_DIRS include
- LIBRARIES crazyflie_cpp
- CATKIN_DEPENDS
- DEPENDS ${USB_LIB}
-)
-
-###########
-## Build ##
-###########
-
-## Additional include folders
-include_directories(
- include/crazyflie_cpp
-)
-
-## Declare a cpp library
-add_library(crazyflie_cpp
- src/USBDevice.cpp
- src/Crazyradio.cpp
- src/CrazyflieUSB.cpp
- src/Crazyflie.cpp
- src/crtp.cpp
- src/ITransport.cpp
-)
-
-## Specify libraries to link a library or executable target against
-target_link_libraries(crazyflie_cpp
- ${USB_LIB}
-)
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-# scripts/my_python_script
-# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS crazyflie crazyflie_node
-# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-# FILES_MATCHING PATTERN "*.h"
-# PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_crazyflie.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
diff --git a/controller_test_drone_modeling/crazyflie_cpp/cmake/crazyflie_cpp-config.cmake b/controller_test_drone_modeling/crazyflie_cpp/cmake/crazyflie_cpp-config.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..57dd60ff6b932e6649efa912b1429b2bf681754a
--- /dev/null
+++ b/controller_test_drone_modeling/crazyflie_cpp/cmake/crazyflie_cpp-config.cmake
@@ -0,0 +1 @@
+include("${CMAKE_CURRENT_LIST_DIR}/crazyflie_cpp-targets.cmake")
\ No newline at end of file
diff --git a/controller_test_drone_modeling/crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h b/controller_test_drone_modeling/crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
index 6a10884b9cc541f0ef5a7a0a1e9fed7c5e68ff2d..bdaedfaa719d6b0ceb57d4b1ace9a665ee920868 100644
--- a/controller_test_drone_modeling/crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
+++ b/controller_test_drone_modeling/crazyflie_cpp/include/crazyflie_cpp/Crazyflie.h
@@ -436,6 +436,15 @@ private:
return reinterpret_cast<const R*>(m_batchRequests[index].ack.data);
}
+ void throwTimeout()
+ {
+ std::stringstream message;
+ int lastbyte = 0xFF & m_address;
+ message << "timeout for Crazyflie " << std::hex << m_address
+ << " (last byte " << std::dec << lastbyte << ")";
+ throw std::runtime_error(message.str().c_str());
+ }
+
private:
struct logInfo {
uint8_t len;
diff --git a/controller_test_drone_modeling/crazyflie_cpp/package.xml b/controller_test_drone_modeling/crazyflie_cpp/package.xml
index 017d61d93c336d79858d5fb9833092eabab816f7..933f5a5181eb194e9319da7fc26446160242dc71 100644
--- a/controller_test_drone_modeling/crazyflie_cpp/package.xml
+++ b/controller_test_drone_modeling/crazyflie_cpp/package.xml
@@ -1,5 +1,5 @@
<?xml version="1.0"?>
-<package>
+<package format="2">
<name>crazyflie_cpp</name>
<version>0.0.1</version>
<description>CPP interface for Crazyflie Nano Quadcopter from Bitcraze</description>
@@ -13,8 +13,11 @@
<author email="whoenig@usc.edu">Wolfgang Hoenig</author>
- <buildtool_depend>catkin</buildtool_depend>
+ <buildtool_depend>cmake</buildtool_depend>
- <build_depend>libusb-1.0-dev</build_depend>
- <run_depend>libusb-1.0-dev</run_depend>
+ <depend>libusb-1.0-dev</depend>
+
+ <export>
+ <build_type>cmake</build_type>
+ </export>
</package>
diff --git a/real_robot/README.MD b/real_robot/README.MD
new file mode 100644
index 0000000000000000000000000000000000000000..420b615bf865f6637428db9a3b3e598fe3133455
--- /dev/null
+++ b/real_robot/README.MD
@@ -0,0 +1 @@
+# the files of the crazyswam, crazyswarm_tools
diff --git a/real_robot/coordination_formation_control_pkg/README.md b/real_robot/coordination_formation_control_pkg/README.md
index 0e9471c15b08e7418677df151f61e6977d234aa3..03ff56ea2b6f6ab7aa0bcb5c73289cfe8e3461fd 100644
--- a/real_robot/coordination_formation_control_pkg/README.md
+++ b/real_robot/coordination_formation_control_pkg/README.md
@@ -16,6 +16,7 @@ implement modules
- [X] send command should include conversion to velocity
- [X] test omniwheels node
- [X] Implement swam controller
+-
diff --git a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
index 8d7912f8688fa833d2f7f81712501c026589cbef..ccb63c8d161409967bd8c20b9074d7bdda110517 100644
--- a/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/crazyfliesConfig.yaml
@@ -7,10 +7,10 @@ crazyflies:
channel: 90
initialPosition: [0.0, 0.0, 0.0]
type: default
-- id: 3
- channel: 80
- initialPosition: [0.0, 0.0, 0.0]
- type: default
+# - id: 3
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
# - id: 4
# channel: 80
# initialPosition: [0.0, 0.0, 0.0]
diff --git a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
index c47102d90da190351e76498bef19e913ff287ce1..eb2d76ce8246fe278e1ed31810273e52fdb8a510 100644
--- a/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/formationConfig.yaml
@@ -1,16 +1,24 @@
# Uav
-uav_c1_alpha: 0.2
-uav_c2_alpha: 1 # this is recomputed
+uav_c1_alpha: 0.20
+uav_c2_alpha: 0.3 # this is recomputed
uav_c1_beta: 0.3
-uav_c2_beta: 1 # this is recomputed
+uav_c2_beta: 0.1 # this is recomputed
uav_c1_gamma: 0.25
uav_c2_gamma: 0.2
uav_c1_theta: 0.2
uav_c1_delta: 0.09
+# uav_c1_alpha: 0.035
+# uav_c2_alpha: 0.04 # this is recomputed
+# uav_c1_beta: 0.04
+# uav_c2_beta: 0.02 # this is recomputed
+# uav_c1_gamma: 0.02
+# uav_c2_gamma: 0.03
+# uav_c1_theta: 0.3
+# uav_c1_delta: 0.01
-uav_d: 0.6
-uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_d: 0.8
+uav_formation_t: 2 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
uav_k: 7
uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
uav_eps: 0.1
@@ -19,8 +27,8 @@ uav_b: 5
uav_h_alpha: 0.2
uav_h_beta: 0.9
uav_d_obs: 0
-uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
-uav_integrator: 2 #/* single/ double integrator*/
+uav_nav_type: 2 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 1 #/* single/ double integrator*/
uav_dt: 0.01 #/* sample time*/
uav_int_max: 0.1 #/* maximum integral*/
@@ -48,6 +56,6 @@ ugv_h_alpha: 0.2
ugv_h_beta: 0.9
ugv_d_obs: 0
ugv_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
-ugv_integrator: 2 #/* single/ double integrator*/
+ugv_integrator: 1 #/* single/ double integrator*/
ugv_dt: 0.02 #/* sample time*/
ugv_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
index 89b9b4c1b84a1b644bda8c89a4bae8df6b4681d2..0791816e6e7337e7841488268f54087ca68b2cda 100644
--- a/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
+++ b/real_robot/coordination_formation_control_pkg/config/missionConfig.yaml
@@ -1,10 +1,10 @@
-n_drones: 3
+n_drones: 2
n_ugvs: 0
hz_freq: 50 # controller frquency
include_obstacle: false
include_orientation: false
n_obs: 0
-log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation"
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_6/experiment_6_trianglular_no_formation"
enable_log: true
# waypoint
diff --git a/real_robot/coordination_formation_control_pkg/include/integration.h b/real_robot/coordination_formation_control_pkg/include/integration.h
index e48652110f237d7d33ecb36967f600fe3527b1f6..f89f9a744bf3e45b9e9fc3e0198d0fbe051f7a1a 100644
--- a/real_robot/coordination_formation_control_pkg/include/integration.h
+++ b/real_robot/coordination_formation_control_pkg/include/integration.h
@@ -8,6 +8,8 @@ private:
Eigen::Vector2d accel;
Eigen::Vector2d vel;
+Eigen::Vector2d current_vel;
+
float dt;
@@ -18,6 +20,8 @@ public:
this->accel(1) = ay_initial;
this->vel(0) = 0;
this->vel(1) = 0;
+ this->current_vel(0) = 0;
+ this->current_vel(1) = 0;
this->dt = dt;
}
@@ -26,12 +30,14 @@ public:
}
- Eigen::Vector2d integrate(float ax, float ay)
+ Eigen::Vector2d integrate(float ax, float ay, float vx, float vy)
{
/* returns the vx and vy*/
this->accel(0) = ax;
this->accel(1) = ay;
- this->vel = this->vel + this->accel*dt;
+ this->current_vel(0) = vx;
+ this->current_vel(1) = vy;
+ this->vel = this->current_vel + this->accel*dt;
return this->vel;
}
diff --git a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
index 15a1ad9b5a54c8af2358f21227bb27e4762da3e9..279a0cb28bc17f0d2be76804b7179efa6baf8de2 100644
--- a/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/bringup_crazyflie_servers.launch
@@ -10,7 +10,7 @@
<rosparam>
# Logging configuration (Use enable_logging to actually enable logging)
genericLogTopics: ["position", "velocity","acceleration"]
- genericLogTopicFrequencies: [10,10,10]
+ genericLogTopicFrequencies: [50,50,50]
genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z", "stateEstimate.yaw"]
genericLogTopic_velocity_Variables: ["stateEstimateZ.vx", "stateEstimateZ.vy","stateEstimateZ.vz"]
genericLogTopic_acceleration_Variables: ["stateEstimateZ.ax", "stateEstimateZ.ay","stateEstimateZ.az"]
diff --git a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
index 8e386d9d1ed06b4ddc230b25cf17119e9c68ebf5..b302d49cfaf497d06d278fd18c1285197e6fbc52 100644
--- a/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
+++ b/real_robot/coordination_formation_control_pkg/launch/mission_controller.launch
@@ -17,10 +17,10 @@
<node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="2" launch-prefix="xterm -e">
</node>
</group>
- <group ns="uav_3">
+ <!-- <group ns="uav_3">
<node name="drone_node" pkg="coordination_formation_control_pkg" type="drone_node" output="screen" args="3" launch-prefix="xterm -e">
</node>
- </group>
+ </group> -->
<!-- <group ns="uav_4">
diff --git a/real_robot/coordination_formation_control_pkg/msg/logData.msg b/real_robot/coordination_formation_control_pkg/msg/logData.msg
index a261fc0ae11d032c1ac1b50e1309cea1a4df7918..9816f0758f69bcdece5b74d3229f16f1438ea873 100644
--- a/real_robot/coordination_formation_control_pkg/msg/logData.msg
+++ b/real_robot/coordination_formation_control_pkg/msg/logData.msg
@@ -10,3 +10,4 @@ geometry_msgs/Vector3 input_orientation
geometry_msgs/Vector3 input_navigation
geometry_msgs/Vector3 input_obstacle
geometry_msgs/Vector3 input_integration
+geometry_msgs/Vector3 input_to_system
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
index 35fe3e23a519a290214ef7f2c5a9a838545b46c2..1b98962fc40bcad8a2131e388f8430e1d3b6c23d 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
index 4455b7ce4a8d7ef782371cff0b0412254ee99347..2facd9bf17032326dc3c9ce1f1e45ca7d2285422 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
index fa364b222ae44a2c20ba6d4dcb771a26b539a400..37c53140615ece16a56f7f5284c32a39e856e293 100644
Binary files a/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag and b/real_robot/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/crazyfliesConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8d7912f8688fa833d2f7f81712501c026589cbef
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/crazyfliesConfig.yaml
@@ -0,0 +1,17 @@
+crazyflies:
+- id: 1
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 2
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 3
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+# - id: 4
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/formationConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3284e1cdf2db9d94fcf05c8a3b0197734f5b031f
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/formationConfig.yaml
@@ -0,0 +1,25 @@
+# Uav
+uav_c1_alpha: 0.2
+uav_c2_alpha: 1 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 1 # this is recomputed
+uav_c1_gamma: 0.25
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+uav_c1_delta: 0.09
+
+
+uav_d: 0.6
+uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 2 #/* single/ double integrator*/
+uav_dt: 0.01 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/missionConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..89b9b4c1b84a1b644bda8c89a4bae8df6b4681d2
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_2/config_files/missionConfig.yaml
@@ -0,0 +1,15 @@
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frquency
+include_obstacle: false
+include_orientation: false
+n_obs: 0
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation"
+enable_log: true
+
+# waypoint
+# x: 0
+# y: 0
+# orienation: 0
+# vx: 0
+# vy: 0
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..b0bcfdaf609523f475e18d3a9c2a92ffaeb841d9
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..7857badcf0b217119b30cec194c2107bbcdcde08
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..cefc63fe297ed25adef77709914454d9c0ea41f1
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_2/experiment_2_trianglular_no_formation_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_2/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_2/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..95cc05d2f7861362760c78774eb95c2d7f682e82
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_2/notes.txt
@@ -0,0 +1,5 @@
+Test conditions
+No orienation
+No obstacle
+Only 3 drones
+mantain position at [0,0]
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/crazyfliesConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8d7912f8688fa833d2f7f81712501c026589cbef
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/crazyfliesConfig.yaml
@@ -0,0 +1,17 @@
+crazyflies:
+- id: 1
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 2
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 3
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+# - id: 4
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/formationConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3284e1cdf2db9d94fcf05c8a3b0197734f5b031f
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/formationConfig.yaml
@@ -0,0 +1,25 @@
+# Uav
+uav_c1_alpha: 0.2
+uav_c2_alpha: 1 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 1 # this is recomputed
+uav_c1_gamma: 0.25
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+uav_c1_delta: 0.09
+
+
+uav_d: 0.6
+uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 2 #/* single/ double integrator*/
+uav_dt: 0.01 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/missionConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..89b9b4c1b84a1b644bda8c89a4bae8df6b4681d2
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_3/config_files/missionConfig.yaml
@@ -0,0 +1,15 @@
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frquency
+include_obstacle: false
+include_orientation: false
+n_obs: 0
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation"
+enable_log: true
+
+# waypoint
+# x: 0
+# y: 0
+# orienation: 0
+# vx: 0
+# vy: 0
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..25b4f91533d5a72e0ca6715c3b34c871f3d811bd
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..13cd9b0b0035b2270cd010b79dffcfe9d585cb8c
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_3/experiment_3_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_3/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_3/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..95cc05d2f7861362760c78774eb95c2d7f682e82
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_3/notes.txt
@@ -0,0 +1,5 @@
+Test conditions
+No orienation
+No obstacle
+Only 3 drones
+mantain position at [0,0]
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/crazyfliesConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8d7912f8688fa833d2f7f81712501c026589cbef
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/crazyfliesConfig.yaml
@@ -0,0 +1,17 @@
+crazyflies:
+- id: 1
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 2
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 3
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+# - id: 4
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/formationConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3284e1cdf2db9d94fcf05c8a3b0197734f5b031f
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/formationConfig.yaml
@@ -0,0 +1,25 @@
+# Uav
+uav_c1_alpha: 0.2
+uav_c2_alpha: 1 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 1 # this is recomputed
+uav_c1_gamma: 0.25
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+uav_c1_delta: 0.09
+
+
+uav_d: 0.6
+uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 2 #/* single/ double integrator*/
+uav_dt: 0.01 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/missionConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..89b9b4c1b84a1b644bda8c89a4bae8df6b4681d2
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_4/config_files/missionConfig.yaml
@@ -0,0 +1,15 @@
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frquency
+include_obstacle: false
+include_orientation: false
+n_obs: 0
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation"
+enable_log: true
+
+# waypoint
+# x: 0
+# y: 0
+# orienation: 0
+# vx: 0
+# vy: 0
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..1b98962fc40bcad8a2131e388f8430e1d3b6c23d
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..2facd9bf17032326dc3c9ce1f1e45ca7d2285422
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..37c53140615ece16a56f7f5284c32a39e856e293
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..4c902daa3eda37171aa3a2e124dbbad0713c0a75
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..35a68cfa576afa66b09902f29d4d6862dce1a5c4
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_4/experiment_4_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_4/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_4/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..95cc05d2f7861362760c78774eb95c2d7f682e82
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_4/notes.txt
@@ -0,0 +1,5 @@
+Test conditions
+No orienation
+No obstacle
+Only 3 drones
+mantain position at [0,0]
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/crazyfliesConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8d7912f8688fa833d2f7f81712501c026589cbef
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/crazyfliesConfig.yaml
@@ -0,0 +1,17 @@
+crazyflies:
+- id: 1
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 2
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 3
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+# - id: 4
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/formationConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3284e1cdf2db9d94fcf05c8a3b0197734f5b031f
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/formationConfig.yaml
@@ -0,0 +1,25 @@
+# Uav
+uav_c1_alpha: 0.2
+uav_c2_alpha: 1 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 1 # this is recomputed
+uav_c1_gamma: 0.25
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+uav_c1_delta: 0.09
+
+
+uav_d: 0.6
+uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 2 #/* single/ double integrator*/
+uav_dt: 0.01 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/missionConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..89b9b4c1b84a1b644bda8c89a4bae8df6b4681d2
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_5/config_files/missionConfig.yaml
@@ -0,0 +1,15 @@
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frquency
+include_obstacle: false
+include_orientation: false
+n_obs: 0
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation"
+enable_log: true
+
+# waypoint
+# x: 0
+# y: 0
+# orienation: 0
+# vx: 0
+# vy: 0
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..1b98962fc40bcad8a2131e388f8430e1d3b6c23d
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..2facd9bf17032326dc3c9ce1f1e45ca7d2285422
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..37c53140615ece16a56f7f5284c32a39e856e293
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..30c354594a6f9ac72fd9927cb7e45c3b78416b08
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..6fa0e241a75d21ebc93342dbc0a5e677be967e85
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_5/experiment_5_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_5/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_5/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..95cc05d2f7861362760c78774eb95c2d7f682e82
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_5/notes.txt
@@ -0,0 +1,5 @@
+Test conditions
+No orienation
+No obstacle
+Only 3 drones
+mantain position at [0,0]
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/crazyfliesConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/crazyfliesConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8d7912f8688fa833d2f7f81712501c026589cbef
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/crazyfliesConfig.yaml
@@ -0,0 +1,17 @@
+crazyflies:
+- id: 1
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 2
+ channel: 90
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+- id: 3
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+# - id: 4
+# channel: 80
+# initialPosition: [0.0, 0.0, 0.0]
+# type: default
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/formationConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/formationConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..3284e1cdf2db9d94fcf05c8a3b0197734f5b031f
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/formationConfig.yaml
@@ -0,0 +1,25 @@
+# Uav
+uav_c1_alpha: 0.2
+uav_c2_alpha: 1 # this is recomputed
+uav_c1_beta: 0.3
+uav_c2_beta: 1 # this is recomputed
+uav_c1_gamma: 0.25
+uav_c2_gamma: 0.2
+uav_c1_theta: 0.2
+uav_c1_delta: 0.09
+
+
+uav_d: 0.6
+uav_formation_t: 3 #/* type must be minimum 2, 3: tuangle formation, 4 suare, 5 pentagon*/
+uav_k: 7
+uav_ratio: 0.8 # /* ratio between dist inter and obstacle*/
+uav_eps: 0.1
+uav_a: 5
+uav_b: 5
+uav_h_alpha: 0.2
+uav_h_beta: 0.9
+uav_d_obs: 0
+uav_nav_type: 1 #/* convergence approach -1, parallel approach 2*/
+uav_integrator: 2 #/* single/ double integrator*/
+uav_dt: 0.01 #/* sample time*/
+uav_int_max: 0.1 #/* maximum integral*/
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/missionConfig.yaml b/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/missionConfig.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..89b9b4c1b84a1b644bda8c89a4bae8df6b4681d2
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_6/config_files/missionConfig.yaml
@@ -0,0 +1,15 @@
+n_drones: 3
+n_ugvs: 0
+hz_freq: 50 # controller frquency
+include_obstacle: false
+include_orientation: false
+n_obs: 0
+log_name: "/home/spot/steven_master_ws/src/coordination_formation_control_pkg/results/experiment_1/experiment_1_trianglular_no_formation"
+enable_log: true
+
+# waypoint
+# x: 0
+# y: 0
+# orienation: 0
+# vx: 0
+# vy: 0
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..1b98962fc40bcad8a2131e388f8430e1d3b6c23d
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..2facd9bf17032326dc3c9ce1f1e45ca7d2285422
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..37c53140615ece16a56f7f5284c32a39e856e293
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_1_trianglular_no_formation_crazyflie_3_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_6_trianglular_no_formation_crazyflie_1_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_6_trianglular_no_formation_crazyflie_1_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..769d5ff768e00da638acc8c7eca354025d260977
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_6_trianglular_no_formation_crazyflie_1_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_6_trianglular_no_formation_crazyflie_2_logdata.bag b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_6_trianglular_no_formation_crazyflie_2_logdata.bag
new file mode 100644
index 0000000000000000000000000000000000000000..5289d02764ec1cfbc4360f43d8ef9177e3cf49d9
Binary files /dev/null and b/real_robot/coordination_formation_control_pkg/results/experiment_6/experiment_6_trianglular_no_formation_crazyflie_2_logdata.bag differ
diff --git a/real_robot/coordination_formation_control_pkg/results/experiment_6/notes.txt b/real_robot/coordination_formation_control_pkg/results/experiment_6/notes.txt
new file mode 100644
index 0000000000000000000000000000000000000000..95cc05d2f7861362760c78774eb95c2d7f682e82
--- /dev/null
+++ b/real_robot/coordination_formation_control_pkg/results/experiment_6/notes.txt
@@ -0,0 +1,5 @@
+Test conditions
+No orienation
+No obstacle
+Only 3 drones
+mantain position at [0,0]
diff --git a/real_robot/coordination_formation_control_pkg/scripts/autonomy_test.py b/real_robot/coordination_formation_control_pkg/scripts/autonomy_test.py
index 0acc3aeefc5c88a344e88d7251f373f1f7fe9322..42ee89e4cdd53e2d2d85e09da217173566ac17ac 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/autonomy_test.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/autonomy_test.py
@@ -6,7 +6,9 @@
# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
#
-# Copyright (C) 2018 Bitcraze AB
+# Copyright (C) 2014 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
#
# This program is free software; you can redistribute it and/or
# modify it under the terms of the GNU General Public License
@@ -20,172 +22,88 @@
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
"""
-Simple example that connects to one crazyflie (check the address at the top
-and update it to your crazyflie address) and uses the high level commander
-to send setpoints and trajectory to fly a figure 8.
-This example is intended to work with any positioning system (including LPS).
-It aims at documenting how to set the Crazyflie in position control mode
-and how to send setpoints using the high level commander.
+Simple example that connects to the first Crazyflie found, ramps up/down
+the motors and disconnects.
"""
-import sys
+import logging
import time
+from threading import Thread
-import cflib.crtp
+import cflib
from cflib.crazyflie import Crazyflie
-from cflib.crazyflie.high_level_commander import HighLevelCommander
-from cflib.crazyflie.log import LogConfig
-from cflib.crazyflie.mem import CompressedSegment
-from cflib.crazyflie.mem import CompressedStart
-from cflib.crazyflie.mem import MemoryElement
-from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
-from cflib.crazyflie.syncLogger import SyncLogger
from cflib.utils import uri_helper
-# URI to the Crazyflie to connect to
-uri = uri_helper.uri_from_env(default='radio://0/90/2M/E7E7E7E701')
-
-# The trajectory to fly
-a = 0.9
-b = 0.5
-c = 0.5
-trajectory = [
- CompressedStart(0.0, 0.0, 0.0, 0.0),
- CompressedSegment(2.0, [0.0, 1.0, 1.0], [0.0, a, 0.0], [], []),
- CompressedSegment(2.0, [1.0, b, 0.0], [-a, -c, 0.0], [], []),
- CompressedSegment(2.0, [-b, -1.0, -1.0], [c, a, 0.0], [], []),
- CompressedSegment(2.0, [-1.0, 0.0, 0.0], [-a, 0.0, 0.0], [], []),
-]
-
-
-class Uploader:
- def __init__(self):
- self._is_done = False
- self._sucess = True
-
- def upload(self, trajectory_mem):
- print('Uploading data')
- trajectory_mem.write_data(self._upload_done,
- write_failed_cb=self._upload_failed)
-
- while not self._is_done:
- time.sleep(0.2)
-
- return self._sucess
-
- def _upload_done(self, mem, addr):
- print('Data uploaded')
- self._is_done = True
- self._sucess = True
-
- def _upload_failed(self, mem, addr):
- print('Data upload failed')
- self._is_done = True
- self._sucess = False
-
+uri = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E703')
-def wait_for_position_estimator(scf):
- print('Waiting for estimator to find position...')
+logging.basicConfig(level=logging.ERROR)
- log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
- log_config.add_variable('kalman.varPX', 'float')
- log_config.add_variable('kalman.varPY', 'float')
- log_config.add_variable('kalman.varPZ', 'float')
- var_y_history = [1000] * 10
- var_x_history = [1000] * 10
- var_z_history = [1000] * 10
+class MotorRampExample:
+ """Example that connects to a Crazyflie and ramps the motors up/down and
+ the disconnects"""
- threshold = 0.001
+ def __init__(self, link_uri):
+ """ Initialize and run the example with the specified link_uri """
- with SyncLogger(scf, log_config) as logger:
- for log_entry in logger:
- data = log_entry[1]
+ self._cf = Crazyflie(rw_cache='./cache')
- var_x_history.append(data['kalman.varPX'])
- var_x_history.pop(0)
- var_y_history.append(data['kalman.varPY'])
- var_y_history.pop(0)
- var_z_history.append(data['kalman.varPZ'])
- var_z_history.pop(0)
+ self._cf.connected.add_callback(self._connected)
+ self._cf.disconnected.add_callback(self._disconnected)
+ self._cf.connection_failed.add_callback(self._connection_failed)
+ self._cf.connection_lost.add_callback(self._connection_lost)
- min_x = min(var_x_history)
- max_x = max(var_x_history)
- min_y = min(var_y_history)
- max_y = max(var_y_history)
- min_z = min(var_z_history)
- max_z = max(var_z_history)
+ self._cf.open_link(link_uri)
- # print("{} {} {}".
- # format(max_x - min_x, max_y - min_y, max_z - min_z))
+ print('Connecting to %s' % link_uri)
- if (max_x - min_x) < threshold and (
- max_y - min_y) < threshold and (
- max_z - min_z) < threshold:
- break
+ def _connected(self, link_uri):
+ """ This callback is called form the Crazyflie API when a Crazyflie
+ has been connected and the TOCs have been downloaded."""
+ # Start a separate thread to do the motor test.
+ # Do not hijack the calling thread!
+ Thread(target=self._ramp_motors).start()
-def reset_estimator(cf):
- cf.param.set_value('kalman.resetEstimation', '1')
- time.sleep(0.1)
- cf.param.set_value('kalman.resetEstimation', '0')
+ def _connection_failed(self, link_uri, msg):
+ """Callback when connection initial connection fails (i.e no Crazyflie
+ at the specified address)"""
+ print('Connection to %s failed: %s' % (link_uri, msg))
- wait_for_position_estimator(cf)
+ def _connection_lost(self, link_uri, msg):
+ """Callback when disconnected after a connection has been made (i.e
+ Crazyflie moves out of range)"""
+ print('Connection to %s lost: %s' % (link_uri, msg))
+ def _disconnected(self, link_uri):
+ """Callback when the Crazyflie is disconnected (called in all cases)"""
+ print('Disconnected from %s' % link_uri)
-def activate_high_level_commander(cf):
- cf.param.set_value('commander.enHighLevel', '1')
+ def _ramp_motors(self):
+ thrust_mult = 1
+ thrust_step = 500
+ thrust = 20000
+ pitch = 0
+ roll = 0
+ yawrate = 0
+ # Unlock startup thrust protection
+ self._cf.commander.send_setpoint(0, 0, 0, 0)
-def activate_mellinger_controller(cf):
- cf.param.set_value('stabilizer.controller', '2')
-
-
-def upload_trajectory(cf, trajectory_id, trajectory):
- trajectory_mem = cf.mem.get_mems(MemoryElement.TYPE_TRAJ)[0]
-
- trajectory_mem.trajectory = trajectory
-
- upload_result = Uploader().upload(trajectory_mem)
- if not upload_result:
- print('Upload failed, aborting!')
- sys.exit(1)
- cf.high_level_commander.define_trajectory(
- trajectory_id,
- 0,
- len(trajectory),
- type=HighLevelCommander.TRAJECTORY_TYPE_POLY4D_COMPRESSED)
-
- total_duration = 0
- # Skip the start element
- for segment in trajectory[1:]:
- total_duration += segment.duration
-
- return total_duration
-
-
-def run_sequence(cf, trajectory_id, duration):
- commander = cf.high_level_commander
-
- commander.takeoff(1.0, 2.0)
- time.sleep(3.0)
- relative = True
- commander.start_trajectory(trajectory_id, 1.0, relative)
- time.sleep(duration)
- commander.land(0.0, 2.0)
- time.sleep(2)
- commander.stop()
+ while thrust >= 20000:
+ self._cf.commander.send_setpoint(roll, pitch, yawrate, 40000)
+ time.sleep(0.1)
+ if thrust >= 50000:
+ thrust_mult = -1
+ thrust += thrust_step * thrust_mult
+ self._cf.commander.send_setpoint(0, 0, 0, 0)
+ # Make sure that the last packet leaves before the link is closed
+ # since the message queue is not flushed before closing
+ time.sleep(0.1)
+ self._cf.close_link()
if __name__ == '__main__':
+ # Initialize the low-level drivers
cflib.crtp.init_drivers()
- with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
- cf = scf.cf
- trajectory_id = 1
-
- activate_high_level_commander(cf)
- # activate_mellinger_controller(cf)
- duration = upload_trajectory(cf, trajectory_id, trajectory)
- print('The sequence is {:.1f} seconds long'.format(duration))
- reset_estimator(cf)
- run_sequence(cf, trajectory_id, duration)
+ le = MotorRampExample(uri)
diff --git a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
index 34017b44f2d475666c6d71391ed504b5e55dd995..65f3a9c7a69b18bfdd83aa1063e6e05f6f91b35c 100644
--- a/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
+++ b/real_robot/coordination_formation_control_pkg/scripts/plot_results.py
@@ -29,6 +29,7 @@ input_orientation_data = []
input_navigation_data = []
input_obstacle_data = []
input_integration_data = []
+input_to_system_data = []
pose_row_ = [0] * n_robots_logged*2;
twist_row_ = [0] * n_robots_logged*2;
@@ -65,6 +66,8 @@ for topics, msg, t in bag.read_messages(topics =[topic_name]):
input_navigation_data.append([msg.input_navigation.x, msg.input_navigation.y])
input_obstacle_data.append([msg.input_obstacle.x, msg.input_obstacle.y])
input_integration_data.append([msg.input_integration.x, msg.input_integration.y])
+ input_to_system_data.append([msg.input_to_system.x, msg.input_to_system.y])
+
# print(pose_data)
bag.close()
@@ -90,6 +93,7 @@ input_orientation_data_np = np.array(input_orientation_data);
input_navigation_data_np = np.array(input_navigation_data);
input_obstacle_data_np = np.array(input_obstacle_data);
input_integration_data_np = np.array(input_integration_data);
+input_to_system_data_np = np.array(input_to_system_data);
# Start plotting
####################### command vs centroid ###################################
@@ -237,8 +241,7 @@ axs5[5, 0].set_title("input_integration")
axs5[5, 1].set_ylabel("a(m2/s)")
axs5[5, 1].set_xlabel("time(s)")
axs5[5, 1].set_title("input_integration")
-axs5[0, 0].set_ylabel("ax(m)")
-axs5[0, 0].set_xlabel("time(s)")
+
####################### position plot ###################################
fig6 = plt.figure()
@@ -257,6 +260,24 @@ axs6.set_ylabel("y(m)")
axs6.set_xlabel("x(m)")
axs6.legend(robot_names)
+# multple robot plot for comparision
+####################### Direct command ###################################
+fig6, axs6 = plt.subplots(nrows=2, ncols=2, figsize=(10, 10))
+st = fig6.suptitle("Direct command to robot (velocity)", fontsize="x-large")
+axs6[0, 0].plot(t_np,input_to_system_data_np[:,0])
+axs6[0, 1].plot(t_np,input_to_system_data_np[:,1])
+vx = input_to_system_data_np[:,0];
+vy = input_to_system_data_np[:,1]
+axs6[1, 0].plot(t_np,np.sqrt(np.multiply(vx,vx)+np.multiply(vy,vy)))
+axs6[0, 0].set_ylabel("vx(m)")
+axs6[0, 0].set_xlabel("time(s)")
+axs6[0, 0].legend(robot_names)
+axs6[0, 1].set_ylabel("vy(m)")
+axs6[0, 1].set_xlabel("time(s)")
+axs6[0, 1].legend(robot_names)
+axs6[1, 0].set_ylabel("v(m/s)")
+axs6[1, 0].set_xlabel("time(s)")
+axs6[1, 0].legend(robot_names)
plt.show()
diff --git a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
index d8db631eefcaf27befb0664089ff3dc3aeafd8e3..905efd9f89dfeab31009ee911c83496cae765d36 100644
--- a/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/drone_node.cpp
@@ -27,7 +27,27 @@ bool enable_log;
bool waypoint_received = false;
+row_vector_2d_t saturate(row_vector_2d_t input, row_vector_2d_t range)
+{
+ row_vector_2d_t return_value;
+ for(int i = 0; i <2; i++)
+ {
+ if(input(i) > range(1))
+ {
+ return_value(i) = range(i);
+ }
+ else if(input(i) < range(0))
+ {
+ return_value(i) = range(0);
+ }
+ else
+ {
+ return_value(i) = input(i);
+ }
+ }
+ return return_value;
+}
void configurationCallback(const coordination_formation_control_pkg::coordination::ConstPtr& msg );
void waypointCallback(const coordination_formation_control_pkg::waypoint::ConstPtr& msg);
@@ -42,6 +62,7 @@ int main(int argc, char **argv) {
int hz_freq, n_obs;
bool use_obstacle,use_orientation;
nh.getParam("/n_drones", n_drones);
+ std::cout << "NUmber of drones " << n_drones << std::endl;
nh.getParam("/hz_freq", hz_freq); //* might not be a good idea*/
nh.getParam("/include_obstacle", use_obstacle);
nh.getParam("/include_orientation", use_orientation);
@@ -189,6 +210,8 @@ int main(int argc, char **argv) {
robot_ind_t uav_def;
bool sucess_initialization;
input_vector_t input;
+ input << 0.0,0.0,0.0,0.0,0.0,0.0,
+ 0.0,0.0,0.0,0.0,0.0,0.0;
for (int i = 1; i <= n_drones; i++)
{
uav_def.robot_name = "cf";
@@ -211,6 +234,7 @@ int main(int argc, char **argv) {
/* main loop*/
/* TODO improve state machine*/
+ float vxx = 0, vyy = 0;
while(ros::ok())
{
/*get current state of all the robot*/
@@ -234,7 +258,7 @@ int main(int argc, char **argv) {
current_command.vy = 0;
current_command.yawrate = 0;
current_command.zDistance = 0.5;
- // vel_cmd_publisher.publish(current_command);
+ vel_cmd_publisher.publish(current_command);
}
@@ -285,7 +309,9 @@ int main(int argc, char **argv) {
input = swam_controller.controller(uav_state_q, uav_state_p,current_waypoint,current_obs);
// std::cout << input << std::endl;
/* integrate acceleration*/
- Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0));
+ Eigen::Vector2d vel_command = acc_int.integrate(input(0,0), input(1,0),uav_state_p(0, robotid-1),uav_state_p(1, robotid-1) );
+ Eigen::Vector2d vel_command_;
+ vel_command_ << input(0,0), input(1,0);
/* reached a certain waypoint and we want to stop sending command*/
if(next_waypoint->stop) // change back TODO
{
@@ -299,11 +325,20 @@ int main(int argc, char **argv) {
robot_state_t current_state_;
current_state_ = uavs[robotid-1]->get_state();
float yaw = current_state_.p(2)*M_PI/180.0; // TODO confirm that indeed it is in degrees
- float vxl = vel_command(0)*std::cos(yaw) + vel_command(1)*std::sin(yaw) ;
- float vyl = - vel_command(0)*std::sin(yaw) + vel_command(1)*std::cos(yaw) ;
-
- current_command.vx = vxl;
- current_command.vy = vyl;
+ float vxl = vel_command_(0)*std::cos(yaw) + vel_command_(1)*std::sin(yaw) ;
+ float vyl = - vel_command_(0)*std::sin(yaw) + vel_command_(1)*std::cos(yaw) ;
+
+ // current_command.vx = vxl;
+ // current_command.vy = vyl;
+ row_vector_2d_t in;
+ in << vxl, vyl;
+ row_vector_2d_t range;
+ range << -0.15, 0.15;
+ row_vector_2d_t out = saturate(in, range);
+ current_command.vx = out(0);
+ current_command.vy = out(1);
+ // vxx = vxl;
+ // vyy = vyl;
}
@@ -325,7 +360,7 @@ int main(int argc, char **argv) {
// std::cout << "vy: transformed: " << vxl << "vy transformed: " << vyl << std::endl;
- // vel_cmd_publisher.publish(current_command);
+ vel_cmd_publisher.publish(current_command);
}
@@ -343,15 +378,15 @@ int main(int argc, char **argv) {
ros::Duration d(3);
land_drone_srv.request.duration = d;
- // if(land_drone_client.call(land_drone_srv))
- // {
- // std::cout << "Shutting down uav: " << std::to_string(robotid) << "Successfull" << std::endl;
- // }
- // else
- // {
- // std::cout << "Shutting down uav: " << std::to_string(robotid) << "unsuccessfull" << std::endl;
- //
- // }
+ if(land_drone_client.call(land_drone_srv))
+ {
+ std::cout << "Shutting down uav: " << std::to_string(robotid) << "Successfull" << std::endl;
+ }
+ else
+ {
+ std::cout << "Shutting down uav: " << std::to_string(robotid) << "unsuccessfull" << std::endl;
+
+ }
sleep(3);
break;
@@ -389,6 +424,9 @@ int main(int argc, char **argv) {
logdata->input_orientation.y = input(1,4);
logdata->input_integration.x = input(0,5);
logdata->input_integration.y = input(1,5);
+ logdata->input_to_system.x = current_command.vx;
+ logdata->input_to_system.y = current_command.vy;
+
}
@@ -404,7 +442,7 @@ int main(int argc, char **argv) {
for(int i = 0; i <= 1000; i++)
{
std_msgs::Empty stop_drone;
- // stop_robot_publisher.publish(stop_drone);
+ stop_robot_publisher.publish(stop_drone);
}
if(enable_log)
{
diff --git a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
index fad5e41e7b4798baa2a64f1d03ba38cb27c12fc5..04bc8dc087e07188ca628d21ae2722aabd8f928a 100644
--- a/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/omniwheel_node.cpp
@@ -188,6 +188,8 @@ int main(int argc, char **argv) {
}
input_vector_t input;
+ input << 0.0,0.0,0.0,0.0,0.0,0.0,
+ 0.0,0.0,0.0,0.0,0.0,0.0;
@@ -353,6 +355,8 @@ int main(int argc, char **argv) {
logdata->input_orientation.y = input(1,4);
logdata->input_integration.x = input(0,5);
logdata->input_integration.y = input(1,5);
+ logdata->input_to_system.x = current_command.linear.x;
+ logdata->input_to_system.y = current_command.linear.x;
// bag.write("robot_1",logdata->header.stamp, *logdata);
diff --git a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
index 1c9a6813175978c1c0d2e39950813f8ac8a0c4bc..bd9157150db320c2d54a630ec6d0bf1a76011b84 100644
--- a/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
+++ b/real_robot/coordination_formation_control_pkg/src/swamControllerAlg.cpp
@@ -16,11 +16,11 @@ swamControllerAlg::swamControllerAlg(robot_ind_t indentification, formation_conf
/* we want to ensure the following condition is met
* c1_alpha < c1_gamma < c1_beta*/
- if((this->form_param.gain.c1_alpha > this->form_param.gain.c1_gamma) || (this->form_param.gain.c1_gamma > this->form_param.gain.c1_beta))
- {
- ROS_ERROR("Swam controller: Following condition not met!!!!!! c1_alpha < c1_gamma < c1_beta");
- EXIT_FAILURE;
- }
+ // if((this->form_param.gain.c1_alpha > this->form_param.gain.c1_gamma) || (this->form_param.gain.c1_gamma > this->form_param.gain.c1_beta))
+ // {
+ // ROS_ERROR("Swam controller: Following condition not met!!!!!! c1_alpha < c1_gamma < c1_beta");
+ // // EXIT_FAILURE;
+ // }
/* make sure that the following codition is met
* 0 < a <= b*/
if((0 > this->form_param.param.a) || (this->form_param.param.a > this->form_param.param.b))
@@ -304,13 +304,14 @@ input_vector_t swamControllerAlg::controller(Eigen::MatrixXf q, Eigen::MatrixXf
if(this->form_param.param.nav_type == 1)
{
- u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne((qi-q_ref)) -this->form_param.gain.c2_gamma*(pi-p_ref);
+ u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne((qi-q_ref));//-this->form_param.gain.c2_gamma*(pi-p_ref);
+ // ROS_ERROR("Here: %f:%f " (qi-q_ref)(0), (qi-q_ref)(1)l;
}
else
{
// std::cout << centroid_q << std::endl;
// std::cout << centroid_p << std::endl;
- u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne(centroid_q-q_ref) -this->form_param.gain.c2_gamma*(centroid_p-p_ref);
+ u_navigation = -this->form_param.gain.c1_gamma*this->sigmaOne(centroid_q-q_ref);// -this->form_param.gain.c2_gamma*(centroid_p-p_ref);
}
}
diff --git a/real_robot/crazyflie-firmware b/real_robot/crazyflie-firmware
new file mode 160000
index 0000000000000000000000000000000000000000..8c606d1f8f89f1f00462dfa1f294a431a321242d
--- /dev/null
+++ b/real_robot/crazyflie-firmware
@@ -0,0 +1 @@
+Subproject commit 8c606d1f8f89f1f00462dfa1f294a431a321242d
diff --git a/real_robot/crazyflie_tools b/real_robot/crazyflie_tools
new file mode 160000
index 0000000000000000000000000000000000000000..420dcc647bc4212c982de66c8809d975fe937952
--- /dev/null
+++ b/real_robot/crazyflie_tools
@@ -0,0 +1 @@
+Subproject commit 420dcc647bc4212c982de66c8809d975fe937952
diff --git a/real_robot/crazyswarm/CMakeLists.txt b/real_robot/crazyswarm/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..a9c980e3a0cf4cd01109b29c5fd80e9dfe3f6b05
--- /dev/null
+++ b/real_robot/crazyswarm/CMakeLists.txt
@@ -0,0 +1,244 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(crazyswarm)
+
+
+# On 64-bit Linux we can enable optitrack_closed_source
+if(CMAKE_HOST_SYSTEM_PROCESSOR STREQUAL x86_64 OR CMAKE_HOST_SYSTEM_PROCESSOR STREQUAL amd64)
+ option(LIBMOTIONCAPTURE_ENABLE_OPTITRACK_CLOSED_SOURCE "Enable optitrack closed source" OFF)
+endif()
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/externalDependencies/libmotioncapture EXCLUDE_FROM_ALL)
+add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/externalDependencies/libobjecttracker EXCLUDE_FROM_ALL)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ message_generation
+ std_msgs
+ tf
+ crazyflie_tools
+ libobjecttracker
+ tf_conversions
+)
+
+set (CMAKE_CXX_STANDARD 11)
+set (CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(PCL REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate services in the 'srv' folder
+add_service_files(
+ FILES
+ GoTo.srv
+ Land.srv
+ NotifySetpointsStop.srv
+ SetGroupMask.srv
+ StartTrajectory.srv
+ Stop.srv
+ Takeoff.srv
+ UpdateParams.srv
+ UploadTrajectory.srv
+)
+
+add_message_files(
+ FILES
+ LogBlock.msg
+ GenericLogData.msg
+ FullState.msg
+ VelocityWorld.msg
+ TrajectoryPolynomialPiece.msg
+ Hover.msg
+ Position.msg
+ ZDistance.msg
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+ geometry_msgs
+)
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## uRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES crazyflie
+ CATKIN_DEPENDS
+ message_runtime
+ std_msgs
+ tf
+ # crazyflie_cpp
+ crazyflie_tools
+ libobjecttracker
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+ ${PCL_INCLUDE_DIRS}
+ externalDependencies/libmotioncapture/include
+ ../externalDependencies/libobjecttracker/include
+ ## TODO remove this dependency
+
+ # ../crazyflie-firmware/src/modules/interface/
+ # crazyflie_cpp/include
+ ../crazyflie_tools/crazyflie_cpp/include
+)
+
+
+## Declare a cpp executable
+add_executable(crazyswarm_server
+ src/crazyswarm_server.cpp
+)
+add_dependencies(crazyswarm_server
+ crazyswarm_gencpp
+)
+target_link_libraries(crazyswarm_server
+ ${catkin_LIBRARIES}
+ libmotioncapture
+ crazyflie_cpp
+)
+
+## Declare a cpp executable
+add_executable(crazyswarm_teleop
+ src/crazyswarm_teleop.cpp
+)
+add_dependencies(crazyswarm_teleop
+ crazyswarm_gencpp
+)
+target_link_libraries(crazyswarm_teleop
+ ${catkin_LIBRARIES}
+)
+## Declare a cpp executable
+#add_executable(mocap_helper
+# src/mocap_helper.cpp
+#)
+
+#target_link_libraries(mocap_helper
+# ${catkin_LIBRARIES}
+# libmotioncapture
+#)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+install(PROGRAMS
+ scripts/hello_world
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark executables and/or libraries for installation
+# install(TARGETS crazyswarm crazyswarm_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_crazyswarm.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/real_robot/crazyswarm/externalDependencies/libmotioncapture b/real_robot/crazyswarm/externalDependencies/libmotioncapture
new file mode 160000
index 0000000000000000000000000000000000000000..aa029718e6b5288319d3c75834575e0cb1306127
--- /dev/null
+++ b/real_robot/crazyswarm/externalDependencies/libmotioncapture
@@ -0,0 +1 @@
+Subproject commit aa029718e6b5288319d3c75834575e0cb1306127
diff --git a/real_robot/crazyswarm/externalDependencies/libobjecttracker b/real_robot/crazyswarm/externalDependencies/libobjecttracker
new file mode 160000
index 0000000000000000000000000000000000000000..1aa8eeca5e917852d6b89100ded6943e1dd3913e
--- /dev/null
+++ b/real_robot/crazyswarm/externalDependencies/libobjecttracker
@@ -0,0 +1 @@
+Subproject commit 1aa8eeca5e917852d6b89100ded6943e1dd3913e
diff --git a/real_robot/crazyswarm/launch/USC.yaml b/real_robot/crazyswarm/launch/USC.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..06ec51eb928dae827613bd868e09d3136edd8797
--- /dev/null
+++ b/real_robot/crazyswarm/launch/USC.yaml
@@ -0,0 +1,203 @@
+crazyflies:
+ - id: 1
+ channel: 100
+ type: "default"
+ initialPosition: [1.5, 1.5, 0.0]
+ - id: 2
+ channel: 110
+ type: "default"
+ initialPosition: [1.5, 1.0, 0.0]
+ - id: 3
+ channel: 120
+ type: "default"
+ initialPosition: [1.5, 0.5, 0.0]
+ - id: 4
+ channel: 100
+ type: "default"
+ initialPosition: [1.5, 0.0, 0.0]
+ - id: 5
+ channel: 110
+ type: "default"
+ initialPosition: [1.5, -0.5, 0.0]
+ - id: 6
+ channel: 120
+ type: "default"
+ initialPosition: [1.5, -1.0, 0.0]
+ - id: 7
+ channel: 100
+ type: "default"
+ initialPosition: [1.5, -1.5, 0.0]
+
+ - id: 8
+ channel: 110
+ type: "default"
+ initialPosition: [1.0, 1.5, 0.0]
+ - id: 9
+ channel: 120
+ type: "default"
+ initialPosition: [1.0, 1.0, 0.0]
+ - id: 10
+ channel: 100
+ type: "default"
+ initialPosition: [1.0, 0.5, 0.0]
+ - id: 11
+ channel: 110
+ type: "default"
+ initialPosition: [1.0, 0.0, 0.0]
+ - id: 12
+ channel: 120
+ type: "default"
+ initialPosition: [1.0, -0.5, 0.0]
+ - id: 13
+ channel: 100
+ type: "default"
+ initialPosition: [1.0, -1.0, 0.0]
+ - id: 14
+ channel: 110
+ type: "default"
+ initialPosition: [1.0, -1.5, 0.0]
+
+ - id: 15
+ channel: 120
+ type: "default"
+ initialPosition: [0.5, 1.5, 0.0]
+ - id: 16
+ channel: 100
+ type: "default"
+ initialPosition: [0.5, 1.0, 0.0]
+ - id: 17
+ channel: 110
+ type: "default"
+ initialPosition: [0.5, 0.5, 0.0]
+ - id: 18
+ channel: 120
+ type: "default"
+ initialPosition: [0.5, 0.0, 0.0]
+ - id: 19
+ channel: 100
+ type: "default"
+ initialPosition: [0.5, -0.5, 0.0]
+ - id: 20
+ channel: 110
+ type: "default"
+ initialPosition: [0.5, -1.0, 0.0]
+ - id: 21
+ channel: 120
+ type: "default"
+ initialPosition: [0.5, -1.5, 0.0]
+
+ - id: 22
+ channel: 100
+ type: "default"
+ initialPosition: [0.0, 1.5, 0.0]
+ - id: 23
+ channel: 110
+ type: "default"
+ initialPosition: [0.0, 1.0, 0.0]
+ - id: 24
+ channel: 120
+ type: "default"
+ initialPosition: [0.0, 0.5, 0.0]
+ - id: 25
+ channel: 100
+ type: "default"
+ initialPosition: [0.0, 0.0, 0.0]
+ - id: 26
+ channel: 110
+ type: "default"
+ initialPosition: [0.0, -0.5, 0.0]
+ - id: 27
+ channel: 120
+ type: "default"
+ initialPosition: [0.0, -1.0, 0.0]
+ - id: 28
+ channel: 100
+ type: "default"
+ initialPosition: [0.0, -1.5, 0.0]
+
+ - id: 29
+ channel: 110
+ type: "default"
+ initialPosition: [-0.5, 1.5, 0.0]
+ - id: 30
+ channel: 120
+ type: "default"
+ initialPosition: [-0.5, 1.0, 0.0]
+ - id: 31
+ channel: 100
+ type: "default"
+ initialPosition: [-0.5, 0.5, 0.0]
+ - id: 32
+ channel: 110
+ type: "default"
+ initialPosition: [-0.5, 0.0, 0.0]
+ - id: 33
+ channel: 120
+ type: "default"
+ initialPosition: [-0.5, -0.5, 0.0]
+ - id: 34
+ channel: 100
+ type: "default"
+ initialPosition: [-0.5, -1.0, 0.0]
+ - id: 35
+ channel: 110
+ type: "default"
+ initialPosition: [-0.5, -1.5, 0.0]
+
+ - id: 36
+ channel: 120
+ type: "default"
+ initialPosition: [-1.0, 1.5, 0.0]
+ - id: 37
+ channel: 100
+ type: "default"
+ initialPosition: [-1.0, 1.0, 0.0]
+ - id: 38
+ channel: 110
+ type: "default"
+ initialPosition: [-1.0, 0.5, 0.0]
+ - id: 39
+ channel: 120
+ type: "default"
+ initialPosition: [-1.0, 0.0, 0.0]
+ - id: 40
+ channel: 100
+ type: "default"
+ initialPosition: [-1.0, -0.5, 0.0]
+ - id: 41
+ channel: 110
+ type: "default"
+ initialPosition: [-1.0, -1.0, 0.0]
+ - id: 42
+ channel: 120
+ type: "default"
+ initialPosition: [-1.0, -1.5, 0.0]
+
+ - id: 43
+ channel: 100
+ type: "default"
+ initialPosition: [-1.5, 1.5, 0.0]
+ - id: 44
+ channel: 110
+ type: "default"
+ initialPosition: [-1.5, 1.0, 0.0]
+ - id: 45
+ channel: 120
+ type: "default"
+ initialPosition: [-1.5, 0.5, 0.0]
+ - id: 46
+ channel: 100
+ type: "default"
+ initialPosition: [-1.5, 0.0, 0.0]
+ - id: 47
+ channel: 110
+ type: "default"
+ initialPosition: [-1.5, -0.5, 0.0]
+ - id: 48
+ channel: 120
+ type: "default"
+ initialPosition: [-1.5, -1.0, 0.0]
+ - id: 49
+ channel: 100
+ type: "default"
+ initialPosition: [-1.5, -1.5, 0.0]
diff --git a/real_robot/crazyswarm/launch/allCrazyflies.yaml b/real_robot/crazyswarm/launch/allCrazyflies.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5a87618fc5dff5161365f87bd579605266238418
--- /dev/null
+++ b/real_robot/crazyswarm/launch/allCrazyflies.yaml
@@ -0,0 +1,9 @@
+crazyflies:
+ - id: 1
+ channel: 100
+ initialPosition: [1.5, 1.5, 0.0]
+ type: medium
+ - id: 40
+ channel: 100
+ initialPosition: [-1.0, -0.5, 0.0]
+ type: default
diff --git a/real_robot/crazyswarm/launch/crazyflieTypes.yaml b/real_robot/crazyswarm/launch/crazyflieTypes.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..671ac328c98a9b0128cd80be9186785a0719d4f6
--- /dev/null
+++ b/real_robot/crazyswarm/launch/crazyflieTypes.yaml
@@ -0,0 +1,180 @@
+crazyflieTypes:
+ default:
+ bigQuad: False
+ batteryVoltageWarning: 3.8 # V
+ batteryVoltateCritical: 3.7 # V
+ markerConfiguration: 0 # change to 3 for single marker tracking
+ dynamicsConfiguration: 0
+ firmwareParams:
+ ctrlMel:
+ kp_xy: 0.4 #0.6 # p for position
+ kd_xy: 0.2 # 0.35 # d for position
+ ki_xy: 0.05
+ i_range_xy: 2.0
+ kR_xy: 70000 # p for attitude
+ kw_xy: 20000 # d for attitude
+ kR_z: 60000 # 70000 # p for yaw
+ kw_z: 12000 # 30000 # d for yaw
+ ki_m_z: 500 #0 #20000 # i for yaw
+ i_range_m_z: 1500 # i range for yaw
+ kd_omega_rp: 200 # roll and pitch angular velocity d gain
+ kp_z: 1.25
+ kd_z: 0.4
+ ki_z: 0.05
+ i_range_z: 0.4
+ mass: 0.032
+ massThrust: 132000
+ defaultSingleMarker:
+ bigQuad: False
+ batteryVoltageWarning: 3.8 # V
+ batteryVoltateCritical: 3.7 # V
+ markerConfiguration: 3 # changed to 3 for single marker tracking
+ dynamicsConfiguration: 0
+ firmwareParams:
+ kalman:
+ pNAcc_xy: 4.0 # default 0.5
+ pNAcc_z: 8.0 # default 1.0
+ mNGyro_rollpitch: 0.2 # default 0.1
+ mNGyro_yaw: 0.1 # default 0.1
+ ctrlMel:
+ kp_xy: 0.4 #0.6 # p for position
+ kd_xy: 0.2 # 0.35 # d for position
+ ki_xy: 0.05
+ i_range_xy: 2.0
+ kR_xy: 70000 # p for attitude
+ kw_xy: 20000 # d for attitude
+ kR_z: 60000 # 70000 # p for yaw
+ kw_z: 12000 # 30000 # d for yaw
+ ki_m_z: 500 #0 #20000 # i for yaw
+ i_range_m_z: 1500 # i range for yaw
+ kd_omega_rp: 200 # roll and pitch angular velocity d gain
+ kp_z: 1.25
+ kd_z: 0.4
+ ki_z: 0.05
+ i_range_z: 0.4
+ mass: 0.032
+ massThrust: 132000
+ CF21SingleMarker:
+ bigQuad: False
+ batteryVoltageWarning: 3.8 # V
+ batteryVoltateCritical: 3.7 # V
+ markerConfiguration: 3 # changed to 3 for single marker tracking
+ dynamicsConfiguration: 0
+ firmwareParams:
+ kalman:
+ pNAcc_xy: 1.0 # default 0.5
+ pNAcc_z: 2.0 # default 1.0
+ mNGyro_rollpitch: 0.1 # default 0.1
+ mNGyro_yaw: 0.1 # default 0.1
+ ctrlMel:
+ kp_xy: 0.4 #0.6 # p for position
+ kd_xy: 0.2 # 0.35 # d for position
+ ki_xy: 0.05
+ i_range_xy: 2.0
+ kR_xy: 70000 # p for attitude
+ kw_xy: 20000 # d for attitude
+ kR_z: 60000 # 70000 # p for yaw
+ kw_z: 12000 # 30000 # d for yaw
+ ki_m_z: 500 #0 #20000 # i for yaw
+ i_range_m_z: 1500 # i range for yaw
+ kd_omega_rp: 200 # roll and pitch angular velocity d gain
+ kp_z: 1.25
+ kd_z: 0.4
+ ki_z: 0.05
+ i_range_z: 0.4
+ mass: 0.032
+ massThrust: 132000
+ medium:
+ bigQuad: True
+ batteryVoltageWarning: 7.6 # V
+ batteryVoltateCritical: 7.4 # V
+ markerConfiguration: 1
+ dynamicsConfiguration: 0
+ firmwareParams:
+ ctrlMel:
+ kp_xy: 2.0 # p for position
+ kd_xy: 1.0 # d for position
+ ki_xy: 0.1 # i for position
+ i_range_xy: 2.0 # range for position
+ kR_xy: 50000 # 50000 # p for attitude
+ kw_xy: 16000 # 16000 # d for attitude
+ kR_z: 60000 # p for yaw
+ kw_z: 12000 # d for yaw
+ ki_m_z: 500 # i for yaw
+ i_range_m_z: 1500 # i range for yaw
+ kd_omega_rp: 200 # roll and pitch angular velocity d gain
+ kp_z: 3 # 1.25
+ kd_z: 1.0
+ ki_z: 0.5 # 0.05
+ i_range_z: 2.0
+ mass: 0.110
+ massThrust: 23000 # 23000 - full battery
+ large:
+ bigQuad: True
+ batteryVoltageWarning: 11.4 # V
+ batteryVoltateCritical: 11.1 # V
+ markerConfiguration: 2
+ dynamicsConfiguration: 0
+ firmwareParams:
+ ctrlMel:
+ kp_xy: 8.0 # 0.4 #0.6 # p for position
+ kd_xy: 3.0 # 0.2 # 0.35 # d for position
+ ki_xy: 1.0 # 0.05
+ i_range_xy: 2.0
+ kR_xy: 30000 # 40000 # p for attitude
+ kw_xy: 9000 # 13000 # d for attitude
+ kR_z: 40000 # 70000 # p for yaw
+ kw_z: 10000 # 30000 # d for yaw
+ ki_m_z: 500 #0 #20000 # i for yaw
+ i_range_m_z: 1500 # i range for yaw
+ kd_omega_rp: 100 # roll and pitch angular velocity d gain
+ kp_z: 8.0 # 1.25
+ kd_z: 3.0 # 0.4
+ ki_z: 2.0 # 0.05
+ i_range_z: 4.0
+ mass: 0.380
+ massThrust: 5400 # 5500 - full battery
+numMarkerConfigurations: 4
+markerConfigurations:
+ "0": # for standard Crazyflie
+ numPoints: 4
+ offset: [0.0, -0.01, -0.04]
+ points:
+ "0": [0.0177184,0.0139654,0.0557585]
+ "1": [-0.0262914,0.0509139,0.0402475]
+ "2": [-0.0328889,-0.02757,0.0390601]
+ "3": [0.0431307,-0.0331216,0.0388839]
+ "1": # medium frame
+ numPoints: 4
+ offset: [0.0, 0.0, -0.03]
+ points:
+ "0": [-0.00896228,-0.000716753,0.0716129]
+ "1": [-0.0156318,0.0997402,0.0508162]
+ "2": [0.0461693,-0.0881012,0.0380672]
+ "3": [-0.0789959,-0.0269793,0.0461144]
+ "2": # big frame
+ numPoints: 4
+ offset: [0.0, 0.0, -0.06]
+ points:
+ "0": [0.0558163,-0.00196302,0.0945539]
+ "1": [-0.0113941,0.00945842,0.0984811]
+ "2": [-0.0306277,0.0514879,0.0520456]
+ "3": [0.0535816,-0.0400775,0.0432799]
+ "3": # for standard Crazyflie with a single marker
+ numPoints: 1
+ offset: [0.0, -0.01, -0.04]
+ points:
+ "0": [0.0177184,0.0139654,0.0557585]
+numDynamicsConfigurations: 1
+dynamicsConfigurations:
+ "0":
+ maxXVelocity: 2.0
+ maxYVelocity: 2.0
+ maxZVelocity: 3.0
+ maxPitchRate: 20.0
+ maxRollRate: 20.0
+ maxYawRate: 10.0
+ maxRoll: 1.4
+ maxPitch: 1.4
+ maxFitnessScore: 0.001
+
diff --git a/real_robot/crazyswarm/launch/crazyflies.yaml b/real_robot/crazyswarm/launch/crazyflies.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..57c1fe1c7be5cf3e6b24b712533a9f354a4a20ac
--- /dev/null
+++ b/real_robot/crazyswarm/launch/crazyflies.yaml
@@ -0,0 +1,17 @@
+crazyflies:
+ - id: 4
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+ - id: 2
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+ - id: 3
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
+ - id: 1
+ channel: 80
+ initialPosition: [0.0, 0.0, 0.0]
+ type: default
diff --git a/real_robot/crazyswarm/launch/figure8_smooth.csv b/real_robot/crazyswarm/launch/figure8_smooth.csv
new file mode 100644
index 0000000000000000000000000000000000000000..4b25059b3735c8846a276f517a647c04faf51264
--- /dev/null
+++ b/real_robot/crazyswarm/launch/figure8_smooth.csv
@@ -0,0 +1,11 @@
+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,
+1.050000,0.000000,-0.000000,0.000000,-0.000000,0.830443,-0.276140,-0.384219,0.180493,-0.000000,0.000000,-0.000000,0.000000,-1.356107,0.688430,0.587426,-0.329106,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.710000,0.396058,0.918033,0.128965,-0.773546,0.339704,0.034310,-0.026417,-0.030049,-0.445604,-0.684403,0.888433,1.493630,-1.361618,-0.139316,0.158875,0.095799,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.620000,0.922409,0.405715,-0.582968,-0.092188,-0.114670,0.101046,0.075834,-0.037926,-0.291165,0.967514,0.421451,-1.086348,0.545211,0.030109,-0.050046,-0.068177,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.700000,0.923174,-0.431533,-0.682975,0.177173,0.319468,-0.043852,-0.111269,0.023166,0.289869,0.724722,-0.512011,-0.209623,-0.218710,0.108797,0.128756,-0.055461,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.560000,0.405364,-0.834716,0.158939,0.288175,-0.373738,-0.054995,0.036090,0.078627,0.450742,-0.385534,-0.954089,0.128288,0.442620,0.055630,-0.060142,-0.076163,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.560000,0.001062,-0.646270,-0.012560,-0.324065,0.125327,0.119738,0.034567,-0.063130,0.001593,-1.031457,0.015159,0.820816,-0.152665,-0.130729,-0.045679,0.080444,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.700000,-0.402804,-0.820508,-0.132914,0.236278,0.235164,-0.053551,-0.088687,0.031253,-0.449354,-0.411507,0.902946,0.185335,-0.239125,-0.041696,0.016857,0.016709,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.620000,-0.921641,-0.464596,0.661875,0.286582,-0.228921,-0.051987,0.004669,0.038463,-0.292459,0.777682,0.565788,-0.432472,-0.060568,-0.082048,-0.009439,0.041158,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.710000,-0.923935,0.447832,0.627381,-0.259808,-0.042325,-0.032258,0.001420,0.005294,0.288570,0.873350,-0.515586,-0.730207,-0.026023,0.288755,0.215678,-0.148061,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.053185,-0.398611,0.850510,-0.144007,-0.485368,-0.079781,0.176330,0.234482,-0.153567,0.447039,-0.532729,-0.855023,0.878509,0.775168,-0.391051,-0.713519,0.391628,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/launch/hover_swarm.launch b/real_robot/crazyswarm/launch/hover_swarm.launch
new file mode 100644
index 0000000000000000000000000000000000000000..be77e2543f875fba225df1128ff91eba81d5f89c
--- /dev/null
+++ b/real_robot/crazyswarm/launch/hover_swarm.launch
@@ -0,0 +1,63 @@
+<?xml version="1.0"?>
+<launch>
+ <arg name="joy_dev" default="/dev/input/js0" />
+
+ <rosparam command="load" file="$(find crazyswarm)/launch/crazyflieTypes.yaml" />
+ <rosparam command="load" file="$(find crazyswarm)/launch/crazyflies.yaml" />
+
+ <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
+ <rosparam>
+ # Logging configuration (Use enable_logging to actually enable logging)
+ genericLogTopics: ["log1"]
+ genericLogTopicFrequencies: [10]
+ genericLogTopic_log1_Variables: ["stateEstimate.x", "ctrltarget.x"]
+ # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or
+ # allCrazyflies.yaml to set per drone)
+ firmwareParams:
+ commander:
+ enHighLevel: 1
+ stabilizer:
+ estimator: 2 # 1: complementary, 2: kalman
+ controller: 2 # 1: PID, 2: mellinger
+ ring:
+ effect: 16 # 6: double spinner, 7: solid color, 16: packetRate
+ solidBlue: 255 # if set to solid color
+ solidGreen: 0 # if set to solid color
+ solidRed: 0 # if set to solid color
+ headlightEnable: 0
+ locSrv:
+ extPosStdDev: 1e-3
+ extQuatStdDev: 0.5e-1
+ kalman:
+ resetEstimation: 1
+ # tracking
+ motion_capture_type: "vicon" # one of none,vicon,optitrack,optitrack_closed_source,qualisys,vrpn
+ object_tracking_type: "libobjecttracker" # one of motionCapture,libobjecttracker
+ send_position_only: False # set to False to send position+orientation; set to True to send position only
+ motion_capture_host_name: "vicon"
+ # motion_capture_interface_ip: "" # optional for optitrack with multiple interfaces
+ save_point_clouds: "/dev/null" # set to a valid path to log mocap point cloud binary file.
+ print_latency: False
+ write_csvs: False
+ force_no_cache: False
+ enable_parameters: True
+ enable_logging: False
+ enable_logging_pose: True
+ </rosparam>
+ </node>
+
+ <node name="joy" pkg="joy" type="joy_node" output="screen">
+ <param name="dev" value="$(arg joy_dev)" />
+ </node>
+
+ <node pkg="crazyswarm" type="crazyswarm_teleop" name="crazyswarm_teleop" output="screen">
+ <param name="csv_file" value="$(find crazyswarm)/launch/figure8_smooth.csv" />
+ <param name="timescale" value="0.8" />
+ </node>
+
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find crazyswarm)/launch/test.rviz"/>
+
+ <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_x" args="/cf2/log1/values[0]"/> -->
+ <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_roll" args="/cf1/log1/values[2] /cf1/log1/values[3]"/> -->
+
+</launch>
diff --git a/real_robot/crazyswarm/launch/hover_swarm_test.launch b/real_robot/crazyswarm/launch/hover_swarm_test.launch
new file mode 100644
index 0000000000000000000000000000000000000000..2962e8305d5b226e910354745cb1c8bf2c3e6414
--- /dev/null
+++ b/real_robot/crazyswarm/launch/hover_swarm_test.launch
@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<launch>
+ <arg name="joy_dev" default="/dev/input/js0" />
+
+ <rosparam command="load" file="$(find crazyswarm)/launch/crazyflieTypes.yaml" />
+ <rosparam command="load" file="$(find crazyswarm)/launch/crazyflies.yaml" />
+
+ <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
+ <rosparam>
+ # Logging configuration (Use enable_logging to actually enable logging)
+ genericLogTopics: ["position", "velocity"]
+ genericLogTopicFrequencies: [20,20]
+ genericLogTopic_position_Variables: ["stateEstimateZ.x", "stateEstimateZ.y","stateEstimateZ.z", "stateEstimate.yaw"]
+ genericLogTopic_velocity_Variables: ["stateEstimateZ.vx", "stateEstimateZ.vy","stateEstimateZ.vz", "stateEstimateZ.rateYaw"]
+ # firmware parameters for all drones (use crazyflieTypes.yaml to set per type, or
+ # allCrazyflies.yaml to set per drone)
+ firmwareParams:
+ commander:
+ enHighLevel: 1
+ stabilizer:
+ estimator: 2 # 1: complementary, 2: kalman
+ controller: 2 # 1: PID, 2: mellinger
+ ring:
+ effect: 16 # 6: double spinner, 7: solid color, 16: packetRate
+ solidBlue: 255 # if set to solid color
+ solidGreen: 0 # if set to solid color
+ solidRed: 0 # if set to solid color
+ headlightEnable: 0
+ locSrv:
+ extPosStdDev: 1e-3
+ extQuatStdDev: 0.5e-1
+ kalman:
+ resetEstimation: 1
+ # tracking
+ motion_capture_type: "none" # one of none,vicon,optitrack,optitrack_closed_source,qualisys,vrpn
+ object_tracking_type: "none" # one of motionCapture,libobjecttracker
+ send_position_only: False # set to False to send position+orientation; set to True to send position only
+ motion_capture_host_name: "vicon"
+ # motion_capture_interface_ip: "" # optional for optitrack with multiple interfaces
+ save_point_clouds: "/dev/null" # set to a valid path to log mocap point cloud binary file.
+ print_latency: False
+ write_csvs: False
+ force_no_cache: False
+ enable_parameters: True
+ enable_logging: True
+ enable_logging_pose: True
+ </rosparam>
+ </node>
+
+ <!-- <node name="joy" pkg="joy" type="joy_node" output="screen">
+ <param name="dev" value="$(arg joy_dev)" />
+ </node> -->
+
+ <!-- <node pkg="crazyswarm" type="crazyswarm_teleop" name="crazyswarm_teleop" output="screen">
+ <param name="csv_file" value="$(find crazyswarm)/launch/figure8_smooth.csv" />
+ <param name="timescale" value="0.8" />
+ </node> -->
+
+ <node name="rviz" pkg="rviz" type="rviz" args="-d $(find crazyswarm)/launch/test.rviz"/>
+
+ <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_x" args="/cf2/log1/values[0]"/> -->
+ <!-- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_roll" args="/cf1/log1/values[2] /cf1/log1/values[3]"/> -->
+
+</launch>
diff --git a/real_robot/crazyswarm/launch/mocap_helper.launch b/real_robot/crazyswarm/launch/mocap_helper.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a6247dee13f4fca206c13a2a9f90667490f84e39
--- /dev/null
+++ b/real_robot/crazyswarm/launch/mocap_helper.launch
@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+
+ <node pkg="crazyswarm" type="mocap_helper" name="mocap_helper" output="screen" >
+ <rosparam>
+ # tracking
+ motion_capture_type: "vicon" # one of vicon,optitrack,optitrack_closed_source,qualisys,vrpn
+ motion_capture_host_name: "vicon"
+ # motion_capture_interface_ip: "" # optional for optitrack with multiple interfaces
+ </rosparam>
+ </node>
+</launch>
diff --git a/real_robot/crazyswarm/launch/object_tracker.yaml b/real_robot/crazyswarm/launch/object_tracker.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..38bc8168db1383dd27042b745674ae35c0a48f69
--- /dev/null
+++ b/real_robot/crazyswarm/launch/object_tracker.yaml
@@ -0,0 +1,122 @@
+hostName: vicon
+numMarkerConfigurations: 2
+markerConfigurations:
+ "0":
+ numPoints: 4
+ offset: [0.0, -0.01, -0.04]
+ points:
+ "0": [0.0177184,0.0139654,0.0557585]
+ "1": [-0.0262914,0.0509139,0.0402475]
+ "2": [-0.0328889,-0.02757,0.0390601]
+ "3": [0.0431307,-0.0331216,0.0388839]
+
+ "1":
+ numPoints: 6
+ offset: [0.0, -0.01, -0.01]
+ points:
+ "0": [0.0475664,0.0440101,0.0066103]
+ "1": [0.0479611,-0.0333709,0.00626755]
+ "2": [0.0186841,0.0113895,0.020396]
+ "3": [-0.0136136,0.0296063,0.0112455]
+ "4": [-0.0292848,0.0437507,0.00688335]
+ "5": [-0.028405,-0.0334036,0.00734073]
+numDynamicsConfigurations: 1
+dynamicsConfigurations:
+ "0":
+ maxXVelocity: 2.0
+ maxYVelocity: 2.0
+ maxZVelocity: 2.0
+ maxPitchRate: 20.0
+ maxRollRate: 20.0
+ maxYawRate: 5.0
+ maxRoll: 1.4
+ maxPitch: 1.4
+numObjects: 5
+objects:
+ "0":
+ name: cf01/cf01
+ markerConfiguration: 0
+ dynamicsConfiguration: 0
+ initialPosition: [0, 1.0, 0]
+ initialYaw: 0
+ "1":
+ name: cf03/cf03
+ markerConfiguration: 0
+ dynamicsConfiguration: 0
+ initialPosition: [0, 0.50, 0]
+ initialYaw: 0
+ "2":
+ name: cf05/cf05
+ markerConfiguration: 0
+ dynamicsConfiguration: 0
+ initialPosition: [0, 0.00, 0]
+ initialYaw: 0
+ "3":
+ name: cf07/cf07
+ markerConfiguration: 0
+ dynamicsConfiguration: 0
+ initialPosition: [0, -0.5, 0]
+ initialYaw: 0
+ "4":
+ name: cf09/cf09
+ markerConfiguration: 0
+ dynamicsConfiguration: 0
+ initialPosition: [0, -1.0, 0]
+ initialYaw: 0
+# numObjects: 9
+# objects:
+# "0":
+# name: cf01/cf01
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, 1.0, 0]
+# initialYaw: 0
+# "1":
+# name: cf02/cf02
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, 0.75, 0]
+# initialYaw: 0
+# "2":
+# name: cf03/cf03
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, 0.50, 0]
+# initialYaw: 0
+# "3":
+# name: cf04/cf04
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, 0.25, 0]
+# initialYaw: 0
+# "4":
+# name: cf05/cf05
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, 0.00, 0]
+# initialYaw: 0
+# "5":
+# name: cf06/cf06
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, -0.25, 0]
+# initialYaw: 0
+# "6":
+# name: cf07/cf07
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, -0.5, 0]
+# initialYaw: 0
+# "7":
+# name: cf08/cf08
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, -0.75, 0]
+# initialYaw: 0
+# "8":
+# name: cf09/cf09
+# markerConfiguration: 0
+# dynamicsConfiguration: 0
+# initialPosition: [0, -1.0, 0]
+# initialYaw: 0
+
diff --git a/real_robot/crazyswarm/launch/rqt.perspective b/real_robot/crazyswarm/launch/rqt.perspective
new file mode 100644
index 0000000000000000000000000000000000000000..f9733bc6ac93eb2233cbde03a8fa0fd6d236fb57
--- /dev/null
+++ b/real_robot/crazyswarm/launch/rqt.perspective
@@ -0,0 +1,407 @@
+{
+ "keys": {},
+ "groups": {
+ "pluginmanager": {
+ "keys": {
+ "running-plugins": {
+ "type": "repr",
+ "repr": "{u'rqt_top/TOP': [1], u'rqt_console/Console': [1]}"
+ }
+ },
+ "groups": {
+ "plugin__rqt_pose_view__PoseView__1": {
+ "keys": {},
+ "groups": {
+ "plugin": {
+ "keys": {
+ "view_matrix": {
+ "type": "repr",
+ "repr": "u'[[0.2507918179035187, 0.15131781995296478, -0.9561414122581482, 0.0], [-0.9592954516410828, 0.1713167428970337, -0.22450637817382812, 0.0], [0.12983129918575287, 0.9735260605812073, 0.18812307715415955, 0.0], [0.0, -3.0, -17.5, 1.0]]'"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_plot__Plot__1": {
+ "keys": {},
+ "groups": {
+ "dock_widget__DataPlotWidget": {
+ "keys": {
+ "dockable": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "parent": {
+ "type": "repr",
+ "repr": "None"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "autoscroll": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "plot_type": {
+ "type": "repr",
+ "repr": "u'0'"
+ },
+ "topics": {
+ "type": "repr",
+ "repr": "u''"
+ },
+ "y_limits": {
+ "type": "repr",
+ "repr": "[u'-0.65374175167526', u'4.45411825773817']"
+ },
+ "x_limits": {
+ "type": "repr",
+ "repr": "[u'8.13676911709831', u'115.200047016144']"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_plot__Plot__2": {
+ "keys": {},
+ "groups": {
+ "dock_widget__DataPlotWidget": {
+ "keys": {
+ "dockable": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "parent": {
+ "type": "repr",
+ "repr": "None"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "autoscroll": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "plot_type": {
+ "type": "repr",
+ "repr": "u'0'"
+ },
+ "topics": {
+ "type": "repr",
+ "repr": "u''"
+ },
+ "y_limits": {
+ "type": "repr",
+ "repr": "[u'-0.245330169725861', u'33.3296037777205']"
+ },
+ "x_limits": {
+ "type": "repr",
+ "repr": "[u'-58.8226690666928', u'106.694982051849']"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_graph__RosGraph__1": {
+ "keys": {},
+ "groups": {
+ "plugin": {
+ "keys": {
+ "graph_type_combo_box_index": {
+ "type": "repr",
+ "repr": "u'0'"
+ },
+ "topic_filter_line_edit_text": {
+ "type": "repr",
+ "repr": "u'/'"
+ },
+ "filter_line_edit_text": {
+ "type": "repr",
+ "repr": "u'/'"
+ },
+ "highlight_connections_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "actionlib_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "quiet_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "dead_sinks_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "leaf_topics_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "namespace_cluster_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "auto_fit_graph_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_topic__TopicPlugin__1": {
+ "keys": {},
+ "groups": {
+ "plugin": {
+ "keys": {
+ "tree_widget_header_state": {
+ "type": "repr(QByteArray.hex)",
+ "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff000000000000000100000000000000000100000000000000000000000000000000000002e5000000050101000100000000000000000500000064ffffffff000000810000000300000005000000c700000001000000030000011800000001000000030000006c0000000100000003000000360000000100000003000000640000000100000003')",
+ "pretty-print": " d l 6 d "
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_top__TOP__1": {
+ "keys": {},
+ "groups": {
+ "dock_widget__": {
+ "keys": {
+ "dockable": {
+ "type": "repr",
+ "repr": "True"
+ },
+ "parent": {
+ "type": "repr",
+ "repr": "None"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "is_regex": {
+ "type": "repr",
+ "repr": "0"
+ },
+ "filter_text": {
+ "type": "repr",
+ "repr": "u''"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_robot_monitor__RobotMonitor__1": {
+ "keys": {},
+ "groups": {
+ "plugin": {
+ "keys": {
+ "splitter": {
+ "type": "repr(QByteArray.hex)",
+ "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000000000030000006400000064000000c801000000090100000002')",
+ "pretty-print": " d d "
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_dep__RosPackGraph__1": {
+ "keys": {},
+ "groups": {
+ "plugin": {
+ "keys": {
+ "filter_line_edit_text": {
+ "type": "repr",
+ "repr": "u'(Separate pkgs by comma)'"
+ },
+ "highlight_connections_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "with_stacks_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "depth_combo_box_index": {
+ "type": "repr",
+ "repr": "u'0'"
+ },
+ "hide_transitives_state": {
+ "type": "repr",
+ "repr": "u'false'"
+ },
+ "package_type_combo_box": {
+ "type": "repr",
+ "repr": "u'0'"
+ },
+ "directions_combo_box_index": {
+ "type": "repr",
+ "repr": "u'0'"
+ },
+ "colorize_state": {
+ "type": "repr",
+ "repr": "u'false'"
+ },
+ "mark_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "auto_fit_graph_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_tf_tree__RosTfTree__1": {
+ "keys": {},
+ "groups": {
+ "plugin": {
+ "keys": {
+ "highlight_connections_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ },
+ "auto_fit_graph_check_box_state": {
+ "type": "repr",
+ "repr": "u'true'"
+ }
+ },
+ "groups": {}
+ }
+ }
+ },
+ "plugin__rqt_console__Console__1": {
+ "keys": {},
+ "groups": {
+ "dock_widget__ConsoleWidget": {
+ "keys": {
+ "dockable": {
+ "type": "repr",
+ "repr": "True"
+ },
+ "parent": {
+ "type": "repr",
+ "repr": "None"
+ }
+ },
+ "groups": {}
+ },
+ "plugin": {
+ "keys": {
+ "show_highlighted_only": {
+ "type": "repr",
+ "repr": "False"
+ },
+ "message_limit": {
+ "type": "repr",
+ "repr": "20000"
+ },
+ "exclude_filters": {
+ "type": "repr",
+ "repr": "u'severity'"
+ },
+ "table_splitter": {
+ "type": "repr(QByteArray.hex)",
+ "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000001910000000001000000090100000002')",
+ "pretty-print": " "
+ },
+ "highlight_filters": {
+ "type": "repr",
+ "repr": "u'message'"
+ },
+ "paused": {
+ "type": "repr",
+ "repr": "False"
+ },
+ "settings_exist": {
+ "type": "repr",
+ "repr": "True"
+ },
+ "filter_splitter": {
+ "type": "repr(QByteArray.hex)",
+ "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff0000000000000002000000ea0000000001000000090100000002')",
+ "pretty-print": " "
+ }
+ },
+ "groups": {
+ "exclude_filter_0": {
+ "keys": {
+ "enabled": {
+ "type": "repr",
+ "repr": "True"
+ },
+ "itemlist": {
+ "type": "repr",
+ "repr": "u''"
+ }
+ },
+ "groups": {}
+ },
+ "highlight_filter_0": {
+ "keys": {
+ "regex": {
+ "type": "repr",
+ "repr": "False"
+ },
+ "text": {
+ "type": "repr",
+ "repr": "u''"
+ },
+ "enabled": {
+ "type": "repr",
+ "repr": "True"
+ }
+ },
+ "groups": {}
+ }
+ }
+ }
+ }
+ }
+ }
+ },
+ "mainwindow": {
+ "keys": {
+ "geometry": {
+ "type": "repr(QByteArray.hex)",
+ "repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb00010000000000000000028400000590000004af000000010000029c0000058f000004ae000000000000')",
+ "pretty-print": " "
+ },
+ "state": {
+ "type": "repr(QByteArray.hex)",
+ "repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd00000001000000030000058f000001e2fc010000000dfb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0031005f005f00440061007400610050006c006f00740057006900640067006500740100000000000003d50000000000000000fb00000042007200710074005f0070006c006f0074005f005f0050006c006f0074005f005f0032005f005f00440061007400610050006c006f007400570069006400670065007401000001f2000001e30000000000000000fb00000044007200710074005f00670072006100700068005f005f0052006f007300470072006100700068005f005f0031005f005f0052006f007300470072006100700068005500690100000000000003d50000000000000000fb00000050007200710074005f006400650070005f005f0052006f0073005000610063006b00470072006100700068005f005f0031005f005f0052006f0073005000610063006b00470072006100700068005500690100000000000003d50000000000000000fb00000022007200710074005f0074006f0070005f005f0054004f0050005f005f0031005f005f0100000000000002f7000000dc00fffffffb00000066007200710074005f006c006f0067006700650072005f006c006500760065006c005f005f004c006f0067006700650072004c006500760065006c005f005f0031005f005f004c006f0067006700650072004c006500760065006c0057006900640067006500740100000220000001b50000000000000000fb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c006500570069006400670065007401000002fd000002920000019500fffffffb00000062007200710074005f0072006f0062006f0074005f006d006f006e00690074006f0072005f005f0052006f0062006f0074004d006f006e00690074006f0072005f005f0031005f005f0052006f0062006f00740020004d006f006e00690074006f00720100000427000000d20000000000000000fb00000078007200710074005f00720075006e00740069006d0065005f006d006f006e00690074006f0072005f005f00520075006e00740069006d0065004d006f006e00690074006f0072005f005f0031005f005f00520075006e00740069006d0065004d006f006e00690074006f0072005700690064006700650074010000038e0000016b0000000000000000fb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f00700069006300570069006400670065007401000003750000021a0000000000000000fb0000004c007200710074005f00740066005f0074007200650065005f005f0052006f0073005400660054007200650065005f005f0031005f005f0052006f00730054006600540072006500650055006901000003ca000001c50000000000000000fb00000050007200710074005f0070006f00730065005f0076006900650077005f005f0050006f007300650056006900650077005f005f0031005f005f0047004c005600690065007700570069006400670065007401000004da000000b50000000000000000fb00000042007200710074005f006e00610076005f0076006900650077005f005f004e0061007600690067006100740069006f006e0056006900650077005f005f0031005f005f01000004a9000000e600000000000000000000058f0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
+ "pretty-print": " Brqt_plot__Plot__1__DataPlotWidget Brqt_plot__Plot__2__DataPlotWidget Drqt_graph__RosGraph__1__RosGraphUi Prqt_dep__RosPackGraph__1__RosPackGraphUi \"rqt_top__TOP__1__ frqt_logger_level__LoggerLevel__1__LoggerLevelWidget Lrqt_console__Console__1__ConsoleWidget brqt_robot_monitor__RobotMonitor__1__Robot Monitor xrqt_runtime_monitor__RuntimeMonitor__1__RuntimeMonitorWidget Lrqt_topic__TopicPlugin__1__TopicWidget Lrqt_tf_tree__RosTfTree__1__RosTfTreeUi Prqt_pose_view__PoseView__1__GLViewWidget Brqt_nav_view__NavigationView__1__ 6MinimizedDockWidgetsToolbar "
+ }
+ },
+ "groups": {
+ "toolbar_areas": {
+ "keys": {
+ "MinimizedDockWidgetsToolbar": {
+ "type": "repr",
+ "repr": "8"
+ }
+ },
+ "groups": {}
+ }
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/real_robot/crazyswarm/launch/test.rviz b/real_robot/crazyswarm/launch/test.rviz
new file mode 100644
index 0000000000000000000000000000000000000000..bdf143ac97eba63d629114d262f19fac63c42f34
--- /dev/null
+++ b/real_robot/crazyswarm/launch/test.rviz
@@ -0,0 +1,182 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 89
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ Splitter Ratio: 0.5
+ Tree Height: 348
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.588679016
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: PointCloud
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.0299999993
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ cf36:
+ Value: true
+ cf37:
+ Value: true
+ cf38:
+ Value: true
+ cf39:
+ Value: true
+ cf40:
+ Value: true
+ cf41:
+ Value: true
+ cf42:
+ Value: true
+ world:
+ Value: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ world:
+ cf36:
+ {}
+ cf37:
+ {}
+ cf38:
+ {}
+ cf39:
+ {}
+ cf40:
+ {}
+ cf41:
+ {}
+ cf42:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Max Intensity: 4096
+ Min Color: 0; 0; 0
+ Min Intensity: 0
+ Name: PointCloud
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.0199999996
+ Style: Spheres
+ Topic: /pointCloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Fixed Frame: world
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Topic: /initialpose
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 8.83146
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.0599999987
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Focal Point:
+ X: 0.732263565
+ Y: -0.013760739
+ Z: -0.270162851
+ Name: Current View
+ Near Clip Distance: 0.00999999978
+ Pitch: 0.695397973
+ Target Frame: <Fixed Frame>
+ Value: Orbit (rviz)
+ Yaw: 3.13357997
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: true
+ Height: 1160
+ Hide Left Dock: true
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000016a000003e9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006f00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000041000003e9000000f300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000003e9000000bb00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000027c0000003efc0100000002fb0000000800540069006d006501000000000000027c0000027600fffffffb0000000800540069006d006501000000000000045000000000000000000000027c000003e900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 636
+ X: 640
+ Y: 18
diff --git a/real_robot/crazyswarm/msg/FullState.msg b/real_robot/crazyswarm/msg/FullState.msg
new file mode 100644
index 0000000000000000000000000000000000000000..298fa8e9938294809a9681087bb91b2fd0cd65d5
--- /dev/null
+++ b/real_robot/crazyswarm/msg/FullState.msg
@@ -0,0 +1,4 @@
+Header header
+geometry_msgs/Pose pose
+geometry_msgs/Twist twist
+geometry_msgs/Vector3 acc
diff --git a/real_robot/crazyswarm/msg/GenericLogData.msg b/real_robot/crazyswarm/msg/GenericLogData.msg
new file mode 100644
index 0000000000000000000000000000000000000000..58a133b2326f72fd4291cb24430ab6d261053136
--- /dev/null
+++ b/real_robot/crazyswarm/msg/GenericLogData.msg
@@ -0,0 +1,2 @@
+Header header
+float64[] values
diff --git a/real_robot/crazyswarm/msg/Hover.msg b/real_robot/crazyswarm/msg/Hover.msg
new file mode 100644
index 0000000000000000000000000000000000000000..69f12e6f14e09109bc61dd2690a235eb599426ff
--- /dev/null
+++ b/real_robot/crazyswarm/msg/Hover.msg
@@ -0,0 +1,5 @@
+Header header
+float32 vx
+float32 vy
+float32 yawrate
+float32 zDistance
diff --git a/real_robot/crazyswarm/msg/LogBlock.msg b/real_robot/crazyswarm/msg/LogBlock.msg
new file mode 100644
index 0000000000000000000000000000000000000000..dbfa6c3f09ec7e12168ca0d5e75ce683c64717f5
--- /dev/null
+++ b/real_robot/crazyswarm/msg/LogBlock.msg
@@ -0,0 +1,3 @@
+string topic_name
+int16 frequency
+string[] variables
diff --git a/real_robot/crazyswarm/msg/Position.msg b/real_robot/crazyswarm/msg/Position.msg
new file mode 100644
index 0000000000000000000000000000000000000000..3339bb8e5b14f9a99211cd23ae0a15728c8cebcf
--- /dev/null
+++ b/real_robot/crazyswarm/msg/Position.msg
@@ -0,0 +1,5 @@
+Header header
+float32 x
+float32 y
+float32 z
+float32 yaw
diff --git a/real_robot/crazyswarm/msg/TrajectoryPolynomialPiece.msg b/real_robot/crazyswarm/msg/TrajectoryPolynomialPiece.msg
new file mode 100644
index 0000000000000000000000000000000000000000..9e8bc3cad3b55a31d802f246d3099bb5a696e014
--- /dev/null
+++ b/real_robot/crazyswarm/msg/TrajectoryPolynomialPiece.msg
@@ -0,0 +1,7 @@
+#
+
+float32[] poly_x
+float32[] poly_y
+float32[] poly_z
+float32[] poly_yaw
+duration duration
\ No newline at end of file
diff --git a/real_robot/crazyswarm/msg/VelocityWorld.msg b/real_robot/crazyswarm/msg/VelocityWorld.msg
new file mode 100644
index 0000000000000000000000000000000000000000..ac1e784f7f6b5fb355bae2a7f0b1e439927b335b
--- /dev/null
+++ b/real_robot/crazyswarm/msg/VelocityWorld.msg
@@ -0,0 +1,3 @@
+Header header
+geometry_msgs/Vector3 vel
+float32 yawRate
\ No newline at end of file
diff --git a/real_robot/crazyswarm/msg/ZDistance.msg b/real_robot/crazyswarm/msg/ZDistance.msg
new file mode 100644
index 0000000000000000000000000000000000000000..0f3abd03ee9f05ea31e80bbfe7ca502220bd4de6
--- /dev/null
+++ b/real_robot/crazyswarm/msg/ZDistance.msg
@@ -0,0 +1,5 @@
+Header header
+float32 roll
+float32 pitch
+float32 yawrate
+float32 zDistance
diff --git a/real_robot/crazyswarm/package.xml b/real_robot/crazyswarm/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8ce28831283eb875f785603b34184690cd62ee11
--- /dev/null
+++ b/real_robot/crazyswarm/package.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<package>
+ <name>crazyswarm</name>
+ <version>0.0.3</version>
+ <description>Crazyswarm: A swarm of Crazyflie-based quadrotors</description>
+
+ <maintainer email="whoenig@usc.edu">Wolfgang Hoenig</maintainer>
+
+ <license>MIT</license>
+
+ <url type="website">http://crazyswarm.readthedocs.io</url>
+ <url type="bugtracker">https://github.com/USC-ACTLab/crazyswarm/issues</url>
+
+ <author email="whoenig@usc.edu">Wolfgang Hoenig</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <build_depend>message_generation</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>tf</build_depend>
+ <build_depend>crazyflie_cpp</build_depend>
+ <build_depend>libobjecttracker</build_depend>
+ <build_depend>libmotioncapture</build_depend>
+ <build_depend>tf_conversion</build_depend>
+ <build_depend>crazyflie_tools</build_depend>
+
+ <run_depend>message_runtime</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>tf</run_depend>
+ <run_depend>crazyflie_cpp</run_depend>
+ <run_depend>libobjecttracker</run_depend>
+ <run_depend>libmotioncapture</run_depend>
+ <run_depend>crazyflie_tools</run_depend>
+
+</package>
diff --git a/real_robot/crazyswarm/scripts/backgroundComputation.py b/real_robot/crazyswarm/scripts/backgroundComputation.py
new file mode 100644
index 0000000000000000000000000000000000000000..c89f3b3c20e62d51c06cb8a5ce012013b659a888
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/backgroundComputation.py
@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+
+"""Demonstrate running a slow computation without blocking the main thread.
+
+Computations that take more than a few milliseconds, such as multi-robot
+planning algorithms, often cannot run in the main script thread:
+
+1. In simulation, the 3d graphics will not update during a blocking call.
+2. In a streaming setpoint mode such as cmdVelocityWorld, the hardware expects
+ to receive setpoints several times per second, even if the setpoint is
+ constant.
+
+Therefore, we must run the computations in a separate thread or process while
+the main thread (1) allows the visualizer to update, and/or (2) sends a
+setpoint on the radio.
+
+Due to the global interpreter lock (GIL) in CPython, it's better to use
+processs instead of threads. We illustrate one method here, including passing
+arguments to the long-running function and receiving outputs using a queue.
+"""
+
+from __future__ import print_function
+import multiprocessing
+import time
+
+import numpy as np
+
+from pycrazyswarm import *
+
+
+crazyflies_yaml = """
+crazyflies:
+- channel: 100
+ id: 1
+ initialPosition: [1.0, 0.0, 0.0]
+"""
+
+Z = 1.0 # Altitude.
+
+
+def slow(output_queue, seconds):
+ time.sleep(seconds)
+ queue.put("OK")
+
+
+if __name__ == "__main__":
+
+ # The amount of time the slow computation will take. Also demonstrates how
+ # to pass args.
+ computation_time = 4.0
+
+ # The queue is used to get outputs back from the background process.
+ queue = multiprocessing.Queue()
+
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml)
+ timeHelper = swarm.timeHelper
+ cf = swarm.allcfs.crazyflies[0]
+
+ # Demonstration using high-level commands: timeHelper.sleep loops for us.
+ p = multiprocessing.Process(target=slow, args=(queue, computation_time))
+ p.start()
+ print("first process started.")
+ cf.takeoff(targetHeight=Z, duration=2.0)
+ timeHelper.sleep(3.0)
+ p.join()
+ print("result:", queue.get())
+
+ # Demonstration using low-level setpoints.
+ p = multiprocessing.Process(target=slow, args=(queue, computation_time))
+ p.start()
+ print("second process started")
+
+ t0 = timeHelper.time()
+ while p.is_alive():
+ t = timeHelper.time() - t0
+ x = np.cos(t)
+ y = np.sin(t)
+ cf.cmdPosition([x, y, Z], yaw=0.0)
+ timeHelper.sleep(timeHelper.dt)
+
+ print("result:", queue.get())
diff --git a/real_robot/crazyswarm/scripts/chooser.py b/real_robot/crazyswarm/scripts/chooser.py
new file mode 100644
index 0000000000000000000000000000000000000000..cb8e6b5dcc471006578c63ed811d5443adaf51b7
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/chooser.py
@@ -0,0 +1,292 @@
+import argparse
+try:
+ import Tkinter
+except ImportError:
+ import tkinter as Tkinter
+import yaml
+import os
+import subprocess
+import re
+import time
+import threading
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser()
+ parser.add_argument(
+ "--configpath",
+ type=str,
+ default=os.path.join(os.path.dirname(os.path.realpath(__file__)), "../launch/"),
+ help="Path to the configuration *.yaml files")
+ parser.add_argument(
+ "--stm32Fw",
+ type=str,
+ default=os.path.join(os.path.dirname(os.path.realpath(__file__)), "../../../../crazyflie-firmware/cf2.bin"),
+ help="Path to cf2.bin")
+ parser.add_argument(
+ "--nrf51Fw",
+ type=str,
+ default=os.path.join(os.path.dirname(os.path.realpath(__file__)), "../../../../crazyflie2-nrf-firmware/cf2_nrf.bin"),
+ help="Path to cf2_nrf.bin")
+ args = parser.parse_args()
+
+ if not os.path.exists(os.path.join(args.configpath, "allCrazyflies.yaml")) or \
+ not os.path.exists(os.path.join(args.configpath, "crazyflieTypes.yaml")) or \
+ not os.path.exists(os.path.join(args.configpath, "crazyflies.yaml")):
+ print("ERROR: Could not find all yaml configuration files in configpath ({}).".format(args.configpath))
+ exit()
+
+ if not os.path.exists(args.stm32Fw):
+ print("WARNING: Could not find STM32 firmware ({}).".format(args.stm32Fw))
+
+ if not os.path.exists(args.nrf51Fw):
+ print("WARNING: Could not find NRF51 firmware ({}).".format(args.nrf51Fw))
+
+ # read a yaml file
+ def read_by_id(path):
+ by_id = {}
+ with open(path, 'r') as ymlfile:
+ root = yaml.load(ymlfile)
+ for node in root["crazyflies"]:
+ id = int(node["id"])
+ by_id[id] = node
+ return by_id
+
+ def selected_cfs():
+ nodes = [node for id, node in allCrazyflies.items() if widgets[id].checked.get()]
+ return nodes
+
+ def save():
+ nodes = selected_cfs()
+ with open(os.path.join(args.configpath, "crazyflies.yaml"), 'w') as outfile:
+ yaml.dump({"crazyflies": nodes}, outfile)
+
+ allCrazyflies = read_by_id(os.path.join(args.configpath, "allCrazyflies.yaml"))
+ enabled = read_by_id(os.path.join(args.configpath, "crazyflies.yaml")).keys()
+ with open(os.path.join(args.configpath, "crazyflieTypes.yaml"), 'r') as ymlfile:
+ data = yaml.load(ymlfile)
+ cfTypes = data["crazyflieTypes"]
+
+ # compute absolute pixel coordinates from the initial positions
+ positions = [node["initialPosition"] for node in allCrazyflies.values()]
+ DOWN_DIR = [-1, 0]
+ RIGHT_DIR = [0, -1]
+ def dot(a, b):
+ return a[0] * b[0] + a[1] * b[1]
+ pixel_x = [120 * dot(pos, RIGHT_DIR) for pos in positions]
+ pixel_y = [120 * dot(pos, DOWN_DIR) for pos in positions]
+ xmin, ymin = min(pixel_x), min(pixel_y)
+ xmax, ymax = max(pixel_x), max(pixel_y)
+
+ # construct the main window
+ top = Tkinter.Tk()
+ top.title('Crazyflie Chooser')
+
+ # construct the frame containing the absolute-positioned checkboxes
+ width = xmax - xmin + 50 # account for checkbox + text width
+ height = ymax - ymin + 50 # account for checkbox + text height
+ frame = Tkinter.Frame(top, width=width, height=height)
+
+ class CFWidget(Tkinter.Frame):
+ def __init__(self, parent, name):
+ Tkinter.Frame.__init__(self, parent)
+ self.checked = Tkinter.BooleanVar()
+ checkbox = Tkinter.Checkbutton(self, variable=self.checked, command=save,
+ padx=0, pady=0)
+ checkbox.grid(row=0, column=0, sticky='E')
+ nameLabel = Tkinter.Label(self, text=name, padx=0, pady=0)
+ nameLabel.grid(row=0, column=1, sticky='W')
+ self.batteryLabel = Tkinter.Label(self, text="", fg="#999999", padx=0, pady=0)
+ self.batteryLabel.grid(row=1, column=0, columnspan=2, sticky='E')
+ self.versionLabel = Tkinter.Label(self, text="", fg="#999999", padx=0, pady=0)
+ self.versionLabel.grid(row=2, column=0, columnspan=2, sticky='E')
+
+ # construct all the checkboxes
+ widgets = {}
+ for (id, node), x, y in zip(allCrazyflies.items(), pixel_x, pixel_y):
+ w = CFWidget(frame, str(id))
+ w.place(x = x - xmin, y = y - ymin)
+ w.checked.set(id in enabled)
+ widgets[id] = w
+
+ # dragging functionality - TODO alt-drag to deselect
+ drag_start = None
+ drag_startstate = None
+
+ def minmax(a, b):
+ return min(a, b), max(a, b)
+
+ def mouseDown(event):
+ global drag_start, drag_startstate
+ drag_start = (event.x_root, event.y_root)
+ drag_startstate = [cf.checked.get() for cf in widgets.values()]
+
+ def mouseUp(event):
+ save()
+
+ def drag(event, select):
+ x, y = event.x_root, event.y_root
+ dragx0, dragx1 = minmax(drag_start[0], x)
+ dragy0, dragy1 = minmax(drag_start[1], y)
+
+ def dragcontains(widget):
+ x0 = widget.winfo_rootx()
+ y0 = widget.winfo_rooty()
+ x1 = x0 + widget.winfo_width()
+ y1 = y0 + widget.winfo_height()
+ return not (x0 > dragx1 or x1 < dragx0 or y0 > dragy1 or y1 < dragy0)
+
+ # depending on interation over dicts being consistent
+ for initial, cf in zip(drag_startstate, widgets.values()):
+ if dragcontains(cf):
+ cf.checked.set(select)
+ else:
+ cf.checked.set(initial)
+
+ top.bind('<ButtonPress-1>', mouseDown)
+ top.bind('<ButtonPress-3>', mouseDown)
+ top.bind('<B1-Motion>', lambda event: drag(event, True))
+ top.bind('<B3-Motion>', lambda event: drag(event, False))
+ top.bind('<ButtonRelease-1>', mouseUp)
+ top.bind('<ButtonRelease-3>', mouseUp)
+
+ # buttons for clearing/filling all checkboxes
+ def clear():
+ for box in widgets.values():
+ box.checked.set(False)
+ save()
+
+ def fill():
+ for box in widgets.values():
+ box.checked.set(True)
+ save()
+
+ def mkbutton(parent, name, command):
+ button = Tkinter.Button(parent, text=name, command=command)
+ button.pack(side='left')
+
+ buttons = Tkinter.Frame(top)
+ mkbutton(buttons, "Clear", clear)
+ mkbutton(buttons, "Fill", fill)
+
+ # construct bottom buttons for utility scripts
+ def sysOff():
+ nodes = selected_cfs()
+ for crazyflie in nodes:
+ id = "{0:02X}".format(crazyflie["id"])
+ uri = "radio://0/{}/2M/E7E7E7E7{}".format(crazyflie["channel"], id)
+ subprocess.call(["rosrun crazyflie_tools reboot --uri " + uri + " --mode sysoff"], shell=True)
+
+ def reboot():
+ nodes = selected_cfs()
+ for crazyflie in nodes:
+ id = "{0:02X}".format(crazyflie["id"])
+ uri = "radio://0/{}/2M/E7E7E7E7{}".format(crazyflie["channel"], id)
+ print(crazyflie["id"])
+ subprocess.call(["rosrun crazyflie_tools reboot --uri " + uri], shell=True)
+
+ def flashSTM():
+ nodes = selected_cfs()
+ for crazyflie in nodes:
+ id = "{0:02X}".format(crazyflie["id"])
+ uri = "radio://0/{}/2M/E7E7E7E7{}".format(crazyflie["channel"], id)
+ print("Flash STM32 FW to {}".format(uri))
+ subprocess.call(["rosrun crazyflie_tools flash --uri " + uri + " --target stm32 --filename " + args.stm32Fw], shell=True)
+
+ def flashNRF():
+ nodes = selected_cfs()
+ for crazyflie in nodes:
+ id = "{0:02X}".format(crazyflie["id"])
+ uri = "radio://0/{}/2M/E7E7E7E7{}".format(crazyflie["channel"], id)
+ print("Flash NRF51 FW to {}".format(uri))
+ subprocess.call(["rosrun crazyflie_tools flash --uri " + uri + " --target nrf51 --filename " + args.nrf51Fw], shell=True)
+
+ def checkBattery():
+ # reset color
+ for id, w in widgets.items():
+ w.batteryLabel.config(foreground='#999999')
+
+ # query each CF
+ nodes = selected_cfs()
+ for crazyflie in nodes:
+ id = "{0:02X}".format(crazyflie["id"])
+ uri = "radio://0/{}/2M/E7E7E7E7{}".format(crazyflie["channel"], id)
+ cfType = crazyflie["type"]
+ bigQuad = cfTypes[cfType]["bigQuad"]
+
+ try:
+ if not bigQuad:
+ voltage = subprocess.check_output(["rosrun crazyflie_tools battery --uri " + uri], shell=True)
+ else:
+ voltage = subprocess.check_output(["rosrun crazyflie_tools battery --uri " + uri + " --external 1"], shell=True)
+ except subprocess.CalledProcessError:
+ voltage = None # CF not available
+
+ color = '#000000'
+ if voltage is not None:
+ voltage = float(voltage)
+ if voltage < cfTypes[cfType]["batteryVoltageWarning"]:
+ color = '#FF8800'
+ if voltage < cfTypes[cfType]["batteryVoltateCritical"]:
+ color = '#FF0000'
+ widgetText = "{:.2f} v".format(voltage)
+ else:
+ widgetText = "Err"
+
+ widgets[crazyflie["id"]].batteryLabel.config(foreground=color, text=widgetText)
+
+ # def checkVersion():
+ # for id, w in widgets.items():
+ # w.versionLabel.config(foreground='#999999')
+ # proc = subprocess.Popen(
+ # ['python3', SCRIPTDIR + 'version.py'], stdout=subprocess.PIPE)
+ # versions = dict()
+ # versionsCount = dict()
+ # versionForMost = None
+ # versionForMostCount = 0
+ # for line in iter(proc.stdout.readline, ''):
+ # print(line)
+ # match = re.search("(\d+): ([0-9a-fA-F]+),(\d),([0-9a-fA-F]+)", line)
+ # if match:
+ # addr = int(match.group(1))
+ # v1 = match.group(2)
+ # modified = int(match.group(3)) == 1
+ # v2 = match.group(4)
+ # v = (v1,v2)
+ # versions[addr] = v
+ # if v in versionsCount:
+ # versionsCount[v] += 1
+ # else:
+ # versionsCount[v] = 1
+ # if versionsCount[v] > versionForMostCount:
+ # versionForMostCount = versionsCount[v]
+ # versionForMost = v
+ # for addr, v in versions.items():
+ # color = '#000000'
+ # if v != versionForMost:
+ # color = '#FF0000'
+ # widgets[addr].versionLabel.config(foreground=color, text=str(v[0])[0:3] + "," + str(v[1])[0:3])
+
+ scriptButtons = Tkinter.Frame(top)
+ mkbutton(scriptButtons, "battery", checkBattery)
+ # currently not supported
+ # mkbutton(scriptButtons, "version", checkVersion)
+ mkbutton(scriptButtons, "sysOff", sysOff)
+ mkbutton(scriptButtons, "reboot", reboot)
+ mkbutton(scriptButtons, "flash (STM)", flashSTM)
+ mkbutton(scriptButtons, "flash (NRF)", flashNRF)
+
+ # start background threads
+ def checkBatteryLoop():
+ while True:
+ # rely on GIL
+ checkBattery()
+ time.sleep(10.0) # seconds
+ # checkBatteryThread = threading.Thread(target=checkBatteryLoop)
+ # checkBatteryThread.daemon = True # so it exits when the main thread exit
+ # checkBatteryThread.start()
+
+ # place the widgets in the window and start
+ buttons.pack()
+ frame.pack(padx=10, pady=10)
+ scriptButtons.pack()
+ top.mainloop()
diff --git a/real_robot/crazyswarm/scripts/cmdFullState.py b/real_robot/crazyswarm/scripts/cmdFullState.py
new file mode 100644
index 0000000000000000000000000000000000000000..59eba2638e5db937ea6e90e39dc1dd5c49b8faa6
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/cmdFullState.py
@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+from pycrazyswarm import *
+import uav_trajectory
+
+
+def executeTrajectory(timeHelper, cf, trajpath, rate=100, offset=np.zeros(3)):
+ traj = uav_trajectory.Trajectory()
+ traj.loadcsv(trajpath)
+
+ start_time = timeHelper.time()
+ while not timeHelper.isShutdown():
+ t = timeHelper.time() - start_time
+ if t > traj.duration:
+ break
+
+ e = traj.eval(t)
+ cf.cmdFullState(
+ e.pos + np.array(cf.initialPosition) + offset,
+ e.vel,
+ e.acc,
+ e.yaw,
+ e.omega)
+
+ timeHelper.sleepForRate(rate)
+
+
+if __name__ == "__main__":
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ cf = swarm.allcfs.crazyflies[0]
+
+ rate = 30.0
+ Z = 0.5
+
+ cf.takeoff(targetHeight=Z, duration=Z+1.0)
+ timeHelper.sleep(Z+2.0)
+
+ executeTrajectory(timeHelper, cf, "figure8.csv", rate, offset=np.array([0, 0, 0.5]))
+
+ cf.notifySetpointsStop()
+ cf.land(targetHeight=0.03, duration=Z+1.0)
+ timeHelper.sleep(Z+2.0)
diff --git a/real_robot/crazyswarm/scripts/cmdVelocityCircle.py b/real_robot/crazyswarm/scripts/cmdVelocityCircle.py
new file mode 100644
index 0000000000000000000000000000000000000000..1ea9bc0581b4f17420dae4a089e7cf7ce29fd5e3
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/cmdVelocityCircle.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+
+import numpy as np
+from pycrazyswarm import *
+
+
+Z = 1.0
+sleepRate = 30
+
+
+def goCircle(timeHelper, cf, totalTime, radius, kPosition):
+ startTime = timeHelper.time()
+ pos = cf.position()
+ startPos = cf.initialPosition + np.array([0, 0, Z])
+ center_circle = startPos - np.array([radius, 0, 0])
+ while True:
+ time = timeHelper.time() - startTime
+ omega = 2 * np.pi / totalTime
+ vx = -radius * omega * np.sin(omega * time)
+ vy = radius * omega * np.cos(omega * time)
+ desiredPos = center_circle + radius * np.array(
+ [np.cos(omega * time), np.sin(omega * time), 0])
+ errorX = desiredPos - cf.position()
+ cf.cmdVelocityWorld(np.array([vx, vy, 0] + kPosition * errorX), yawRate=0)
+ timeHelper.sleepForRate(sleepRate)
+
+
+if __name__ == "__main__":
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(2 + Z)
+ goCircle(timeHelper, allcfs.crazyflies[0], totalTime=4, radius=1, kPosition=1)
diff --git a/real_robot/crazyswarm/scripts/collisionAvoidance.py b/real_robot/crazyswarm/scripts/collisionAvoidance.py
new file mode 100644
index 0000000000000000000000000000000000000000..491b3f9d748fc484644c8a31252726e68d649511
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/collisionAvoidance.py
@@ -0,0 +1,123 @@
+"""Demonstration of Buffered Voronoi Cell collision avoidance algorithm."""
+
+from __future__ import print_function
+import argparse
+import time
+
+import numpy as np
+import scipy as sp
+import scipy.spatial
+import scipy.optimize
+
+import pycrazyswarm
+import pycrazyswarm.util as util
+import pycrazyswarm.cfsim.cffirmware as firm
+
+
+Z = 1.0 # Takeoff altitude.
+
+
+def main():
+
+ # Crazyswarm's inner parser must add help to get all params.
+ parser = argparse.ArgumentParser(add_help=False)
+
+ group = parser.add_argument_group("Collision avoidance", "")
+ group.add_argument(
+ "--noavoid",
+ help="Disable collision avoidance.",
+ action="store_true"
+ )
+ group.add_argument(
+ "--assign",
+ help=("Use optimal start-goal assignment instead of random assignment."),
+ action="store_true"
+ )
+ group.add_argument(
+ "--loops",
+ type=int,
+ default=1,
+ help="Repeat the experiment this many times, without resetting start positions.",
+ )
+ args, unknown = parser.parse_known_args()
+
+ if args.assign:
+ duration = 3.0
+ else:
+ duration = 10.0
+
+ # Construct the Crazyswarm objects.
+ rows, cols = 3, 5
+ N = rows * cols
+ crazyflies_yaml = util.grid_yaml(rows, cols, spacing=0.5)
+ swarm = pycrazyswarm.Crazyswarm(crazyflies_yaml=crazyflies_yaml, parent_parser=parser)
+ timeHelper = swarm.timeHelper
+ cfs = swarm.allcfs.crazyflies
+
+ # Tell the visualizer to draw collision volume ellipsoids. Ellipsoids
+ # change from green to red when a collision occurs. Make the radii slightly
+ # smaller than the radii used for the collision avoidance algorithm as a
+ # fudge factor.
+ #
+ # To show collisions on purpose, use --noavoid without --assign.
+ xy_radius = 0.125
+ radii = 1.0 * xy_radius * np.array([1.0, 1.0, 3.0])
+ timeHelper.visualizer.showEllipsoids(0.95 * radii)
+
+ swarm.allcfs.takeoff(targetHeight=Z, duration=Z+1.0)
+ timeHelper.sleep(Z + 2.0)
+
+ if not args.noavoid:
+ for i, cf in enumerate(cfs):
+ others = cfs[:i] + cfs[(i+1):]
+ cf.enableCollisionAvoidance(others, radii)
+
+ for _ in range(args.loops):
+ # Goals are randomly positioned in the XY plane, with some random
+ # perturbations in Z. If we didn't add the perturbations, we'd get purely
+ # 2D movement that doesn't take advantage of the free space in the Z axis
+ # to reduce conflicts.
+ #
+ # A more robust solution would be to add some random bias direction to each
+ # robot's reference direction, so it will have a preferred altitude when
+ # far away from the goal, giving Z separation even when the starts and
+ # goals are all coplanar.
+ #
+ # Minimum goal distance of 5*radius ensures that robots are never trying to
+ # squeeze too tightly in between two other robots.
+ #
+ goals = util.poisson_disk_sample(N, dim=2, mindist=5*xy_radius)
+ goals_z = Z * np.ones(N) + 0.2 * np.random.uniform(-1.0, 1.0, size=N)
+ goals = np.hstack([goals, goals_z[:,None]])
+
+ starts = np.stack([cf.position() for cf in cfs])
+ starts[:, 2] += Z
+
+ all_pts = np.vstack([goals, starts])
+ bbox_min = np.amin(all_pts, axis=0) - 4.0 * xy_radius
+ bbox_max = np.amax(all_pts, axis=0) + 4.0 * xy_radius
+
+ # Optimal assignment with sum of Euclidean distances objective guarantees
+ # no collisions when following straight-line trajectories IF the robots
+ # have no volume. If not, collisions may occur, but they are rare in
+ # practice if the start and goal positions are sufficiently dispersed and
+ # not collinear.
+ #
+ # Since we have our own collision avoidance, we use squared Euclidean
+ # distance instead. Minimizing sum of squared distances is closer to
+ # minimizing the # maximum distance for any robot, which generally is more
+ # important in multi-robot applications. We could use e.g. Euclidean^4 to
+ # approximate maximum distance even more closely.
+ #
+ if args.assign:
+ dists = sp.spatial.distance.cdist(starts, goals, "sqeuclidean")
+ _, assignments = sp.optimize.linear_sum_assignment(dists)
+ goals = goals[assignments,:]
+
+ for goal, cf in zip(goals, cfs):
+ cf.goTo(goal, yaw=0.0, duration=duration)
+ timeHelper.sleep(duration + 1.0)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/real_robot/crazyswarm/scripts/collisionAvoidanceHighConflict.py b/real_robot/crazyswarm/scripts/collisionAvoidanceHighConflict.py
new file mode 100644
index 0000000000000000000000000000000000000000..aa467b46d59237f71457e95f3e316fd6c3b7e038
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/collisionAvoidanceHighConflict.py
@@ -0,0 +1,125 @@
+"""Worst-case test of Buffered Voronoi Cell collision avoidance algorithm.
+
+Computes the center of all initial positions, and tells each robot to go straight across the center. Many are forced to wait. This stresses two components of the collision avoidance algorithm:
+
+- Dealing with full "deadlocks" that require a sidestep maneuver
+- Handling situations when the goals are far away from the current positions.
+
+Also will cause high-gain controllers without proper windup handling to fail.
+"""
+
+from __future__ import print_function
+import argparse
+
+import numpy as np
+
+import pycrazyswarm
+
+
+Z = 1.0 # Takeoff altitude.
+duration = 6.0 # Duration of all goTos.
+
+
+def positionGoTo(timeHelper, cfs, goals, kp=1.0, velMax=0.5):
+ while True:
+ positions = np.row_stack([cf.position() for cf in cfs])
+ errors = positions - goals
+
+ # Check stopping criterion.
+ distances = np.linalg.norm(errors, axis=1)
+ assert len(distances) == len(cfs)
+ if np.all(distances < 0.1):
+ break
+
+ # Command direct to goal.
+ # Rely on BVCA algorithm and controller to not be too aggressive.
+ for cf, goal in zip(cfs, goals):
+ cf.cmdPosition(goal, yaw=0)
+
+ timeHelper.sleepForRate(30)
+
+
+def velocityGoTo(timeHelper, cfs, goals, kp=2.0, velMax=0.5):
+ while True:
+ positions = np.row_stack([cf.position() for cf in cfs])
+ errors = positions - goals
+
+ # Check stopping criterion.
+ distances = np.linalg.norm(errors, axis=1)
+ assert len(distances) == len(cfs)
+ if np.all(distances < 0.1):
+ break
+
+ # Compute P-only linear position control law.
+ # Rely on BVCA algorithm and controller to not be too aggressive.
+ velocities = -kp * errors
+ for cf, vel in zip(cfs, velocities):
+ cf.cmdVelocityWorld(vel, yawRate=0)
+
+ timeHelper.sleepForRate(30)
+
+
+def main():
+ # Crazyswarm's inner parser must add help to get all params.
+ parser = argparse.ArgumentParser(add_help=False)
+ group = parser.add_argument_group("Collision avoidance", "")
+ group.add_argument(
+ "--mode",
+ help="Control mode to use.",
+ choices=["goto", "velocity", "position"],
+ default="goto",
+ )
+ args, unknown = parser.parse_known_args()
+
+ swarm = pycrazyswarm.Crazyswarm(parent_parser=parser)
+ timeHelper = swarm.timeHelper
+ cfs = swarm.allcfs.crazyflies
+
+ xy_radius = 0.3
+ radii = xy_radius * np.array([1.0, 1.0, 3.0])
+
+ for i, cf in enumerate(cfs):
+ others = cfs[:i] + cfs[(i+1):]
+ cf.enableCollisionAvoidance(others, radii)
+
+ # Everyone will go straight across the center, causing many conflicts.
+ initialPositions = np.row_stack([cf.initialPosition for cf in cfs])
+ initialPositions[:,2] = Z
+ center = np.mean(initialPositions, axis=0)
+ goals = center + (center - initialPositions)
+
+ swarm.allcfs.takeoff(targetHeight=Z, duration=Z+1.0)
+ timeHelper.sleep(Z + 2.0)
+
+ if args.mode == "goto":
+ for goal, cf in zip(goals, cfs):
+ cf.goTo(goal, yaw=0.0, duration=duration)
+ timeHelper.sleep(duration + 1.0)
+ for cf in cfs:
+ cf.goTo(cf.initialPosition + [0.0, 0.0, Z], yaw=0.0, duration=duration)
+ timeHelper.sleep(duration + 1.0)
+
+ elif args.mode == "velocity":
+ velocityGoTo(timeHelper, cfs, goals)
+ velocityGoTo(timeHelper, cfs, initialPositions)
+ for cf in cfs:
+ cf.notifySetpointsStop()
+
+ elif args.mode == "position":
+ positionGoTo(timeHelper, cfs, goals)
+ positionGoTo(timeHelper, cfs, initialPositions)
+ for cf in cfs:
+ cf.notifySetpointsStop()
+
+ else:
+ raise ValueError("--mode {} not understood.".format(args.mode))
+
+ for cf in cfs:
+ cf.disableCollisionAvoidance()
+
+ swarm.allcfs.land(targetHeight=0.04, duration=Z+1.0)
+ timeHelper.sleep(Z + 2.0)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/real_robot/crazyswarm/scripts/conftest.py b/real_robot/crazyswarm/scripts/conftest.py
new file mode 100644
index 0000000000000000000000000000000000000000..2a71671df1313acd43298745f825353fa152f3b8
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/conftest.py
@@ -0,0 +1,12 @@
+import subprocess
+
+import pytest
+
+
+HAVE_ROS = subprocess.call("type roslaunch", shell=True) == 0
+
+
+def pytest_runtest_setup(item):
+ markers = [mark.name for mark in item.iter_markers()]
+ if "ros" in markers and not HAVE_ROS:
+ pytest.skip("ROS is not installed.")
diff --git a/real_robot/crazyswarm/scripts/crossing2_pps/pp1.csv b/real_robot/crazyswarm/scripts/crossing2_pps/pp1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..1aec2c99583021822f9e8610112138d81c8a2f20
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/crossing2_pps/pp1.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,0.000000,0.000000,0.000000,0.000000,0.049761,-0.053341,0.024521,-0.004466,0.000000,0.000000,0.000000,0.000000,-0.047980,0.053167,-0.024798,0.004559,0.000000,0.000000,0.000000,0.000000,0.069667,-0.076972,0.035856,-0.006587,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.016474,0.048198,0.039172,-0.000273,-0.005454,0.004504,-0.001889,0.000335,-0.015051,-0.042957,-0.032434,0.003363,0.005455,-0.004616,0.001972,-0.000355,0.021964,0.062835,0.047796,-0.004475,-0.007894,0.006667,-0.002844,0.000510,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.101065,0.117430,0.029351,-0.003128,0.000437,-0.000302,0.000117,-0.000020,-0.084623,-0.089646,-0.013642,0.006053,-0.000454,0.000322,-0.000137,0.000028,0.124559,0.133272,0.021741,-0.008390,0.000650,-0.000468,0.000193,-0.000036,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.244950,0.167544,0.020896,-0.002771,-0.000028,0.000016,-0.000006,0.000001,-0.182098,-0.099602,0.003542,0.005686,0.000068,0.000014,0.000092,-0.000042,0.271520,0.152748,-0.002072,-0.007870,-0.000054,0.000058,-0.000071,0.000022,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.430603,0.200965,0.012511,-0.002802,0.000001,-0.000001,0.000001,-0.000000,-0.272340,-0.074859,0.021650,0.006472,0.000050,-0.000875,-0.000593,0.000129,0.414281,0.124796,-0.026026,-0.008149,-0.000050,-0.000115,0.000389,-0.000070,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.641278,0.217586,0.004114,-0.002794,0.000004,-0.000001,0.000000,-0.000000,-0.320367,-0.018982,0.026409,-0.009451,-0.008727,0.016363,-0.005650,0.000088,0.505056,0.049365,-0.047560,-0.004173,0.002756,-0.000776,0.000254,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.860186,0.217442,-0.004251,-0.002782,0.000004,-0.000001,-0.000000,0.000000,-0.320317,0.019106,0.026420,0.009345,-0.008589,0.001584,0.000401,-0.000152,0.504923,-0.049598,-0.047469,0.004210,0.002723,-0.000742,-0.000098,0.000069,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.070598,0.200603,-0.012586,-0.002778,-0.000001,0.000002,-0.000001,0.000001,-0.272202,0.074892,0.021595,-0.006457,0.000036,0.000335,-0.000215,0.000045,0.414017,-0.124831,-0.025944,0.008132,-0.000049,-0.000099,0.000084,-0.000022,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.255837,0.167102,-0.020902,-0.002748,0.000028,-0.000026,0.000025,-0.000020,-0.181970,0.099560,0.003519,-0.005678,0.000069,-0.000082,0.000058,-0.000028,0.271288,-0.152664,-0.002034,0.007857,-0.000053,0.000064,-0.000058,0.000036,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.399295,0.117044,-0.029288,-0.003104,-0.000435,0.000194,-0.000451,0.000333,-0.084552,0.089580,-0.013641,-0.006045,-0.000453,0.000229,-0.000509,0.000354,0.124434,-0.133153,0.021738,0.008376,0.000649,-0.000323,0.000728,-0.000510,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.483590,0.048016,-0.039034,-0.000260,0.005431,-0.000009,0.006714,-0.004447,-0.015037,0.042918,-0.032407,-0.003357,0.005450,-0.000120,0.007108,-0.004555,0.021940,-0.062767,0.047748,0.004465,-0.007885,0.000160,-0.010239,0.006579,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/crossing2_pps/pp2.csv b/real_robot/crazyswarm/scripts/crossing2_pps/pp2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..bd5e26511ede9a7ccc625f5b0e0e8cc0d167ae97
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/crossing2_pps/pp2.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,1.500000,0.000000,0.000000,0.000000,-0.049660,0.053230,-0.024469,0.004457,0.000000,0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000016,-0.000019,0.000009,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.483558,-0.048107,-0.039103,0.000267,0.005443,-0.004494,0.001885,-0.000334,0.000000,0.000000,0.000000,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000005,0.000014,0.000010,-0.000002,-0.000002,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.399114,-0.117238,-0.029319,0.003116,-0.000436,0.000301,-0.000117,0.000020,0.000000,0.000000,0.000000,-0.000000,0.000000,-0.000000,0.000000,-0.000000,0.000026,0.000026,0.000002,-0.000002,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.255442,-0.167324,-0.020899,0.002760,0.000028,-0.000016,0.000006,-0.000001,0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,-0.000000,0.000000,0.000052,0.000025,-0.000003,-0.000001,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.069996,-0.200783,-0.012548,0.002790,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,-0.000000,0.000073,0.000017,-0.000005,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.859453,-0.217513,-0.004182,0.002788,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,-0.000000,-0.000000,0.000000,-0.000000,0.000000,-0.000000,0.000084,0.000006,-0.000006,-0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.640547,-0.217513,0.004182,0.002788,-0.000000,0.000000,-0.000000,0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000084,-0.000006,-0.000006,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.430004,-0.200783,0.012548,0.002790,0.000001,-0.000002,0.000001,-0.000001,0.000000,-0.000000,-0.000000,0.000000,0.000000,-0.000000,0.000000,-0.000000,0.000072,-0.000017,-0.000005,0.000001,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.244558,-0.167324,0.020899,0.002760,-0.000028,0.000026,-0.000025,0.000020,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000051,-0.000024,-0.000002,0.000001,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.100886,-0.117238,0.029319,0.003116,0.000436,-0.000195,0.000452,-0.000334,0.000000,-0.000000,0.000000,0.000000,0.000000,-0.000000,0.000000,-0.000000,0.000025,-0.000025,0.000002,0.000002,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.016442,-0.048107,0.039103,0.000267,-0.005443,0.000009,-0.006729,0.004457,0.000000,-0.000000,0.000000,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000005,-0.000013,0.000009,0.000002,-0.000002,0.000000,-0.000002,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/crossing4_pps/pp1.csv b/real_robot/crazyswarm/scripts/crossing4_pps/pp1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..f4235a82a2ffd8956b3ff2626e6f9710d5eb0983
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/crossing4_pps/pp1.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,0.000000,0.000000,0.000000,0.000000,0.074358,-0.080059,0.036875,-0.006725,1.000000,0.000000,0.000000,0.000000,0.008664,-0.009525,0.004428,-0.000812,0.000000,0.000000,0.000000,0.000000,-0.002250,0.002432,-0.001121,0.000205,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.024449,0.071312,0.057458,-0.001035,-0.008190,0.006785,-0.002853,0.000506,1.002754,0.007908,0.006087,-0.000474,-0.000977,0.000822,-0.000349,0.000063,-0.000734,-0.002134,-0.001699,0.000059,0.000255,-0.000206,0.000087,-0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.148433,0.170714,0.040898,-0.005289,0.000657,-0.000457,0.000177,-0.000031,1.015833,0.017204,0.003094,-0.000964,0.000079,-0.000056,0.000022,-0.000004,-0.004388,-0.004952,-0.001072,0.000218,-0.000011,0.000014,-0.000005,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.355103,0.237838,0.026423,-0.004755,-0.000042,0.000021,-0.000004,-0.000000,1.035207,0.020639,0.000363,-0.000904,-0.000006,0.000000,0.000002,-0.000001,-0.010196,-0.006442,-0.000402,0.000244,0.000014,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.614583,0.276330,0.012050,-0.004802,-0.000006,0.000018,-0.000035,0.000012,1.055300,0.018635,-0.002373,-0.000922,-0.000014,0.000016,-0.000030,0.000012,-0.016782,-0.006455,0.000422,0.000310,0.000023,-0.000003,0.000010,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.898151,0.285966,-0.002481,-0.004923,-0.000019,-0.000075,0.000229,-0.000047,1.070624,0.011048,-0.005271,-0.001012,0.000025,-0.000068,0.000233,-0.000071,-0.022479,-0.004567,0.001544,0.000466,0.000055,0.000032,-0.000055,0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.176801,0.266828,-0.015668,-0.002815,0.001396,-0.000316,-0.000097,0.000046,1.075508,-0.001869,-0.006837,0.000571,0.000681,-0.000534,0.000198,-0.000030,-0.025000,-0.000002,0.002850,0.000043,-0.000473,0.000100,0.000060,-0.000024,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.426175,0.230793,-0.019379,-0.000710,-0.000018,-0.000060,0.000048,-0.000012,1.067689,-0.012797,-0.004037,0.000863,-0.000073,0.000035,-0.000009,0.000001,-0.022446,0.004625,0.001530,-0.000503,0.000072,0.000015,-0.000022,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.636839,0.189744,-0.021735,-0.000825,-0.000006,0.000013,-0.000008,0.000002,1.051672,-0.018444,-0.001647,0.000777,-0.000001,-0.000000,-0.000000,0.000000,-0.016723,0.006449,0.000397,-0.000299,0.000023,-0.000009,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.804023,0.143807,-0.024188,-0.000797,0.000021,-0.000018,0.000016,-0.000013,1.032357,-0.019413,0.000674,0.000767,-0.000006,0.000005,-0.000005,0.000003,-0.010157,0.006415,-0.000403,-0.000240,0.000012,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.922850,0.093034,-0.026678,-0.001046,-0.000300,0.000126,-0.000296,0.000227,1.014382,-0.015768,0.002989,0.000819,0.000071,-0.000034,0.000078,-0.000056,-0.004373,0.004933,-0.001066,-0.000217,-0.000012,0.000009,-0.000021,0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.987916,0.035776,-0.030046,0.001017,0.003816,0.000037,0.004573,-0.003089,1.002481,-0.007141,0.005535,0.000379,-0.000873,0.000014,-0.001119,0.000725,-0.000732,0.002127,-0.001694,-0.000060,0.000254,-0.000002,0.000310,-0.000204,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/crossing4_pps/pp2.csv b/real_robot/crazyswarm/scripts/crossing4_pps/pp2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..6af62b4562f1e6d2d953b74f3ebb945115c6b800
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/crossing4_pps/pp2.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,2.000000,0.000000,0.000000,0.000000,-0.085060,0.091827,-0.042346,0.007729,1.000000,0.000000,0.000000,0.000000,-0.011426,0.012606,-0.005869,0.001078,0.000000,0.000000,0.000000,0.000000,0.117649,-0.129952,0.060532,-0.011120,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.972150,-0.081078,-0.064972,0.001623,0.009397,-0.007800,0.003285,-0.000584,0.996389,-0.010343,-0.007897,0.000701,0.001295,-0.001099,0.000475,-0.000087,0.037110,0.106192,0.080846,-0.007468,-0.013318,0.011253,-0.004802,0.000862,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.832020,-0.191942,-0.044705,0.006483,-0.000755,0.000524,-0.000202,0.000035,0.979435,-0.022104,-0.003710,0.001352,-0.000116,0.000142,-0.000109,0.000031,0.210676,0.225702,0.037151,-0.014062,0.001100,-0.000796,0.000334,-0.000064,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.601458,-0.263273,-0.026848,0.005877,0.000050,-0.000002,-0.000015,0.000005,0.954921,-0.025655,0.000093,0.001218,0.000046,-0.000493,0.000552,-0.000151,0.460042,0.259795,-0.002727,-0.013177,-0.000104,0.000136,-0.000187,0.000061,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.317250,-0.299209,-0.009069,0.005918,-0.000020,-0.000226,0.000179,-0.000040,0.930531,-0.021843,0.004198,0.002223,0.000573,0.004004,-0.003664,0.000785,0.703838,0.214378,-0.043047,-0.013837,-0.000090,-0.000364,0.001110,-0.000225,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.014784,-0.300010,0.008143,0.005743,0.000120,0.000148,-0.000302,0.000040,0.916807,-0.000954,0.015873,-0.001243,-0.006882,0.001496,0.002684,-0.001162,0.861762,0.089675,-0.076816,-0.003512,0.006868,-0.001569,-0.000392,0.000211,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.728666,-0.266808,0.023876,0.003050,-0.002283,0.000524,0.000143,-0.000070,0.926617,0.014976,0.001651,-0.000825,0.000169,0.000759,-0.000789,0.000227,0.876227,-0.055746,-0.063297,0.007797,0.000513,-0.000436,0.000200,-0.000039,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.487099,-0.216048,0.025245,-0.000431,0.000030,0.000094,-0.000076,0.000018,0.942785,0.017131,0.000717,-0.000387,0.000081,-0.000186,0.000141,-0.000036,0.765219,-0.158151,-0.039012,0.008115,-0.000041,0.000034,-0.000016,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.295932,-0.166585,0.024325,-0.000239,0.000010,-0.000019,0.000012,-0.000003,0.960245,0.017388,-0.000469,-0.000378,-0.000008,0.000030,-0.000021,0.000005,0.576152,-0.211896,-0.014742,0.008088,0.000006,-0.000008,0.000004,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.153432,-0.118659,0.023588,-0.000262,-0.000015,0.000012,-0.000010,0.000009,0.976791,0.015341,-0.001562,-0.000351,0.000009,-0.000008,0.000006,-0.000003,0.357601,-0.217123,0.009494,0.008038,-0.000051,0.000051,-0.000051,0.000038,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.058094,-0.072265,0.022873,-0.000088,0.000205,-0.000079,0.000190,-0.000152,0.990224,0.011178,-0.002610,-0.000376,-0.000043,0.000020,-0.000047,0.000034,0.157998,-0.174009,0.033847,0.008659,0.000776,-0.000373,0.000853,-0.000606,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.008779,-0.026278,0.022718,-0.001563,-0.002648,-0.000056,-0.003070,0.002117,0.998380,0.004716,-0.003782,-0.000091,0.000546,-0.000003,0.000683,-0.000449,0.027145,-0.078221,0.060826,0.003892,-0.009498,0.000145,-0.012184,0.007895,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/crossing4_pps/pp3.csv b/real_robot/crazyswarm/scripts/crossing4_pps/pp3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..e67b9b48dc4669be7beeaeb70936440ae9b2c1af
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/crossing4_pps/pp3.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,1.000000,0.000000,0.000000,0.000000,0.016259,-0.018257,0.008563,-0.001580,0.000000,0.000000,0.000000,0.000000,-0.014991,0.019018,-0.009348,0.001774,0.000000,0.000000,0.000000,0.000000,0.148973,-0.165605,0.077344,-0.014231,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.004984,0.014067,0.010248,-0.001570,-0.001873,0.001595,-0.000679,0.000121,-0.003547,-0.008546,-0.002733,0.005348,0.001973,-0.001813,0.000811,-0.000150,0.046480,0.132309,0.099085,-0.011379,-0.016991,0.014403,-0.006148,0.001102,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.026892,0.027105,0.002598,-0.002469,0.000140,-0.000071,-0.000030,0.000019,-0.008657,0.004676,0.016041,0.006095,-0.000161,0.000091,0.000033,-0.000021,0.258861,0.271218,0.037953,-0.019703,0.001375,-0.000931,0.000260,-0.000023,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.054185,0.025050,-0.004740,-0.002572,-0.000017,-0.000177,0.000515,-0.000125,0.018098,0.054910,0.034331,0.006295,0.000063,0.000198,-0.000616,0.000128,0.549010,0.290259,-0.018802,-0.019125,-0.000193,-0.000231,0.000901,-0.000174,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.072118,0.009115,-0.009226,0.001522,0.002458,-0.000489,-0.000884,0.000354,0.113407,0.140895,0.049012,0.000674,-0.003717,0.000805,0.000344,-0.000143,0.801646,0.197544,-0.069776,-0.010267,0.006084,-0.001551,0.000088,0.000099,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.074968,-0.000208,-0.000623,0.001182,-0.000849,0.000500,-0.000182,0.000016,0.301277,0.231159,0.038932,-0.004282,0.000448,0.000109,-0.000271,0.000067,0.923867,0.044989,-0.076195,0.003768,0.003099,-0.000982,-0.000007,0.000053,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.074804,0.000215,0.000429,-0.000302,-0.000525,-0.000376,-0.000266,0.000250,0.567438,0.297354,0.027198,-0.004488,-0.000740,0.000144,0.000020,-0.000020,0.898592,-0.088280,-0.055107,0.008055,-0.000070,-0.000075,0.000049,-0.000014,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.074228,-0.003664,-0.006140,-0.002751,0.002339,-0.000247,-0.000293,0.000094,0.886905,0.336028,0.010624,-0.006291,-0.000404,0.000132,-0.000002,-0.000006,0.763150,-0.174786,-0.031666,0.007522,-0.000193,0.000062,-0.000000,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.063566,-0.017176,-0.005248,0.001569,0.000004,-0.000133,0.000091,-0.000021,1.226986,0.337392,-0.009513,-0.006845,0.000007,0.000015,-0.000012,0.000004,0.564085,-0.216037,-0.009705,0.007259,0.000007,0.000001,-0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.042652,-0.023213,-0.000917,0.001349,-0.000022,0.000029,-0.000020,0.000008,1.548034,0.297893,-0.029943,-0.006752,0.000057,-0.000055,0.000053,-0.000041,0.345607,-0.213656,0.012073,0.007225,-0.000047,0.000045,-0.000046,0.000036,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.019865,-0.021008,0.003157,0.001435,0.000108,-0.000055,0.000121,-0.000084,1.809246,0.217734,-0.050474,-0.007451,-0.000861,0.000396,-0.000913,0.000665,0.151236,-0.167826,0.033971,0.007808,0.000729,-0.000347,0.000795,-0.000568,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.003538,-0.010095,0.007612,0.000802,-0.001284,0.000029,-0.001677,0.001074,1.968340,0.092141,-0.073771,-0.001927,0.010694,-0.000068,0.013390,-0.008799,0.025797,-0.074491,0.058285,0.003262,-0.008970,0.000116,-0.011429,0.007430,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/crossing4_pps/pp4.csv b/real_robot/crazyswarm/scripts/crossing4_pps/pp4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..87dddf1c940f51526843c63512a78ec1dacef062
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/crossing4_pps/pp4.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,1.000000,0.000000,0.000000,0.000000,0.011715,-0.013364,0.006309,-0.001168,2.000000,0.000000,0.000000,0.000000,-0.206797,0.227923,-0.106059,0.019470,0.000000,0.000000,0.000000,0.000000,-0.003633,0.004009,-0.001867,0.000343,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.003491,0.009713,0.006742,-0.001504,-0.001373,0.001181,-0.000505,0.000090,1.934536,-0.187641,-0.143579,0.012292,0.023365,-0.019682,0.008362,-0.001493,-0.001148,-0.003286,-0.002506,0.000227,0.000413,-0.000347,0.000148,-0.000027,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.017836,0.016698,0.000114,-0.002142,0.000102,-0.000046,-0.000037,0.000018,1.626160,-0.403152,-0.069257,0.023915,-0.001873,0.001246,-0.000325,0.000022,-0.006526,-0.006999,-0.001155,0.000442,-0.000029,0.000029,-0.000014,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.032543,0.010586,-0.006325,-0.002286,-0.000036,-0.000162,0.000473,-0.000103,1.176737,-0.472976,-0.000700,0.023154,0.000250,0.000388,-0.001388,0.000283,-0.014249,-0.008012,0.000155,0.000465,0.000033,-0.000017,0.000039,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.034691,-0.007761,-0.010099,0.001783,0.002622,-0.000478,-0.000504,0.000152,0.725748,-0.408319,0.059269,0.010184,-0.008723,0.001994,0.000529,-0.000274,-0.021599,-0.006114,0.001903,0.000774,0.000101,0.000119,-0.000241,0.000060,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.020407,-0.016465,0.001850,0.002755,-0.001987,0.000327,0.000229,-0.000088,0.380408,-0.282896,0.059599,-0.003788,-0.000418,0.000497,-0.000268,0.000057,-0.024997,-0.000014,0.003665,-0.000352,-0.000818,0.000517,-0.000118,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.007029,-0.010051,0.003059,-0.000410,0.000015,0.000106,-0.000082,0.000020,0.153191,-0.175455,0.047881,-0.003846,0.000048,-0.000052,0.000028,-0.000006,-0.022112,0.004899,0.001206,-0.000634,0.000180,-0.000027,-0.000011,0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.999686,-0.004924,0.002169,-0.000233,0.000011,-0.000020,0.000012,-0.000003,0.021790,-0.091172,0.036406,-0.003825,-0.000005,0.000005,-0.000003,0.000001,-0.016494,0.005963,0.000051,-0.000237,0.000044,-0.000011,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.996700,-0.001283,0.001467,-0.000238,-0.000002,0.000003,-0.000002,0.000000,-0.036803,-0.029842,0.024923,-0.003829,-0.000000,0.000001,-0.000001,0.000001,-0.010681,0.005492,-0.000465,-0.000120,0.000021,-0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.996646,0.000937,0.000754,-0.000236,0.000001,-0.000001,0.000001,-0.000001,-0.045550,0.008521,0.013446,-0.003815,0.000013,-0.000015,0.000017,-0.000011,-0.005754,0.004279,-0.000715,-0.000053,0.000013,-0.000002,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.998100,0.001736,0.000042,-0.000246,-0.000013,0.000007,-0.000015,0.000010,-0.027395,0.023968,0.001950,-0.003961,-0.000193,0.000108,-0.000239,0.000158,-0.002232,0.002732,-0.000816,-0.000024,-0.000003,0.000002,-0.000008,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.999622,0.001047,-0.000716,-0.000175,0.000151,-0.000006,0.000207,-0.000128,-0.005605,0.015420,-0.010287,-0.002913,0.002283,-0.000104,0.003159,-0.001953,-0.000343,0.001022,-0.000872,0.000046,0.000107,0.000001,0.000124,-0.000085,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/csv_sequence.py b/real_robot/crazyswarm/scripts/csv_sequence.py
new file mode 100644
index 0000000000000000000000000000000000000000..93daae5e6e5fbcdd67f267c073f13ce0df2690f2
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/csv_sequence.py
@@ -0,0 +1,253 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+import argparse
+import os
+import os.path
+
+import numpy as np
+import csv
+
+from pycrazyswarm import *
+import uav_trajectory
+
+import pycrazyswarm.cfsim.cffirmware as firm
+
+
+def main():
+
+ parser = argparse.ArgumentParser(add_help=False)
+ parser.add_argument("path",
+ type=str,
+ help="directory containing numbered subdirectories for each robot," +
+ "each of which contains numbered <n>.csv files for each formation change")
+ swarm = Crazyswarm(parent_parser=parser)
+ args, unknown = parser.parse_known_args()
+
+ #
+ # DATA LOADING
+ #
+ folder_path = os.path.split(args.path)[0]
+ print("folder_path:", folder_path)
+
+ # ...capability matrices...
+ with open(os.path.join(folder_path, "Capability_matrices.csv")) as f:
+ read_data = csv.reader(f, delimiter=",")
+ data = list(read_data)
+ C_matrices = np.array(data).astype("int")
+
+ # ...simulation parameters...
+ with open(os.path.join(folder_path, "sim_parameter.txt")) as f:
+ lines = [line.rstrip('\n') for line in f]
+ n = int(lines[0])
+ r = int(lines[1])
+
+ # ...trajectory sequences...
+ root = args.path
+ robot_dirs = sorted(os.listdir(root), key=int)
+ seqs = [load_all_csvs(os.path.join(root, d)) for d in robot_dirs]
+ N = len(robot_dirs)
+ steps = len(seqs[0])
+ assert C_matrices.shape[0] == steps + 2, "capabilities / trajs mismatch"
+
+ print("loading complete")
+
+ #
+ # DATA VALIDATION / PROCESSING
+ #
+
+ # transpose / reshape capabilities to (time, robot, capability)
+ C_matrices = C_matrices.reshape((r, n, -1)).transpose([2, 1, 0])
+ # lower brightness
+ C_matrices = 0.6 * C_matrices
+
+ # validate sequences w.r.t. each other
+ assert all(len(seq) == steps for seq in seqs)
+ for i in range(steps):
+ agent_lens = [seq[i].duration for seq in seqs]
+ assert all(agent_lens == agent_lens[0])
+ step_lens = [t.duration for t in seqs[0]]
+
+ print("validation complete")
+
+ #
+ # CRAZYSWARM INITIALIZATION
+ #
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+ crazyflies = allcfs.crazyflies
+
+ # support trials on <N robots
+ if len(crazyflies) < N:
+ N = len(crazyflies)
+ seqs = seqs[:N]
+ C_matrices = C_matrices[:,:N,:]
+ print("using", N, "crazyflies")
+
+ # transposed copy - by timestep instead of robot
+ seqs_t = zip(*seqs)
+
+ # check that crazyflies.yaml initial positions match sequences.
+ # only compare xy positions.
+ # assume that cf id numerical order matches sequence order,
+ # but id's don't need to be 1..N.
+ crazyflies = sorted(crazyflies, key=lambda cf: cf.id)
+ init_positions = np.stack([cf.initialPosition for cf in crazyflies])
+ evals = [seq[0].eval(0.0).pos for seq in seqs]
+ traj_starts = np.stack(evals)
+ errs = init_positions - traj_starts
+ errnorms = np.linalg.norm(errs[:,:2], axis=1)
+ assert not np.any(np.abs(errnorms) > 0.1)
+
+ # planners for takeoff and landing
+ planners = [firm.planner() for cf in crazyflies]
+ for p in planners:
+ firm.plan_init(p)
+
+ #
+ # ASSORTED OTHER SETUP
+ #
+
+ # local helper fn to set colors
+ def set_colors(i):
+ for cf, color in zip(crazyflies, C_matrices[i]):
+ cf.setLEDColor(*color)
+
+ # timing parameters
+ timescale = 1.0
+ pause_between = 1.5
+ takeoff_time = 3.0
+ land_time = 4.0
+
+ #
+ # RUN DEMO
+ #
+
+ print("validation complete")
+
+ # takeoff
+ print("takeoff")
+ z_init = traj_starts[0,2]
+
+ for cf, p in zip(crazyflies, planners):
+ p.lastKnownPosition = cf.position()
+ vposition = firm.mkvec(*p.lastKnownPosition)
+ firm.plan_takeoff(p, vposition, 0.0, z_init, takeoff_time, 0.0)
+
+ poll_planners(crazyflies, timeHelper, planners, takeoff_time)
+ end_pos = np.stack([cf.position() for cf in crazyflies])
+
+ # set to full capability colors
+ set_colors(0)
+
+ # pause - all is well...
+ hover(crazyflies, timeHelper, end_pos, pause_between)
+
+ # set colors first capability loss
+ set_colors(1)
+
+ # pause - reacting to capability loss
+ hover(crazyflies, timeHelper, end_pos, pause_between)
+
+ # main loop!
+ for step in range(steps):
+
+ # move - new configuration after capability loss
+ print("executing trajectory", step, "/", steps)
+ poll_trajs(crazyflies, timeHelper, seqs_t[step], timescale)
+ end_pos = np.stack([cf.position() for cf in crazyflies])
+
+ # done with this step's trajs - hover for a few sec
+ hover(crazyflies, timeHelper, end_pos, pause_between)
+
+ # change the LEDs - another capability loss
+ if step < steps - 1:
+ set_colors(step + 2)
+
+ # hover some more
+ hover(crazyflies, timeHelper, end_pos, pause_between)
+
+ # land
+ print("landing")
+
+ end_pos = np.stack([cf.position() for cf in crazyflies])
+ for cf, p, pos in zip(crazyflies, planners, end_pos):
+ vposition = firm.mkvec(*pos)
+ firm.plan_land(p, vposition, 0.0, 0.06, land_time, 0.0)
+
+ poll_planners(crazyflies, timeHelper, planners, land_time)
+
+ # cut power
+ print("sequence complete.")
+ allcfs.emergency()
+
+POLL_RATE = 100 # Hz
+
+def poll_trajs(crazyflies, timeHelper, trajs, timescale):
+ duration = trajs[0].duration
+ start_time = timeHelper.time()
+ while not timeHelper.isShutdown():
+ t = (timeHelper.time() - start_time) / timescale
+ if t > duration:
+ break
+ for cf, traj in zip(crazyflies, trajs):
+ ev = traj.eval(t)
+ cf.cmdFullState(
+ ev.pos,
+ ev.vel,
+ ev.acc,
+ ev.yaw,
+ ev.omega)
+ timeHelper.sleepForRate(POLL_RATE)
+
+
+def poll_planners(crazyflies, timeHelper, planners, duration):
+ start_time = timeHelper.time()
+ while not timeHelper.isShutdown():
+ t = timeHelper.time() - start_time
+ if t > duration:
+ break
+ for cf, planner in zip(crazyflies, planners):
+ ev = firm.plan_current_goal(planner, t)
+ cf.cmdFullState(
+ ev.pos,
+ ev.vel,
+ ev.acc,
+ ev.yaw,
+ ev.omega)
+ timeHelper.sleepForRate(POLL_RATE)
+
+
+def hover(crazyflies, timeHelper, positions, duration):
+ start_time = timeHelper.time()
+ zero = np.zeros(3)
+ while not timeHelper.isShutdown():
+ t = timeHelper.time() - start_time
+ if t > duration:
+ break
+ for cf, pos in zip(crazyflies, positions):
+ cf.cmdFullState(
+ pos,
+ zero,
+ zero,
+ 0.0,
+ zero)
+ timeHelper.sleepForRate(POLL_RATE)
+
+
+def load_all_csvs(path):
+ csvs = os.listdir(path)
+ csvs = sorted(csvs, key=lambda s: int(os.path.splitext(s)[0])) # numerical order
+ names, exts = zip(*[os.path.splitext(os.path.basename(f)) for f in csvs])
+ assert all(e == ".csv" for e in exts)
+ steps = len(names)
+ assert set(names) == set([str(i) for i in range(1, steps + 1)])
+ trajs = [uav_trajectory.Trajectory() for _ in range(steps)]
+ for t, csv in zip(trajs, csvs):
+ t.loadcsv(os.path.join(path, csv))
+ return trajs
+
+
+if __name__ == "__main__":
+ main()
diff --git a/real_robot/crazyswarm/scripts/example_cmd_pos.py b/real_robot/crazyswarm/scripts/example_cmd_pos.py
new file mode 100644
index 0000000000000000000000000000000000000000..35ec56fb133829587ab0d73804a3111ad205428d
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/example_cmd_pos.py
@@ -0,0 +1,27 @@
+from pycrazyswarm import *
+import numpy as np
+
+swarm = Crazyswarm()
+timeHelper = swarm.timeHelper
+allcfs = swarm.allcfs
+
+Z = 0
+# takeoff
+while Z < 1.0:
+ for cf in allcfs.crazyflies:
+ pos = np.array(cf.initialPosition) + np.array([0, 0, Z])
+ cf.cmdPosition(pos)
+ timeHelper.sleep(0.1)
+ Z += 0.05
+
+# land
+while Z > 0.0:
+ for cf in allcfs.crazyflies:
+ pos = np.array(cf.initialPosition) + np.array([0, 0, Z])
+ cf.cmdPosition(pos)
+ timeHelper.sleep(0.1)
+ Z -= 0.05
+
+# turn-off motors
+for cf in allcfs.crazyflies:
+ cf.cmdStop()
diff --git a/real_robot/crazyswarm/scripts/figure8.csv b/real_robot/crazyswarm/scripts/figure8.csv
new file mode 100644
index 0000000000000000000000000000000000000000..4b25059b3735c8846a276f517a647c04faf51264
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/figure8.csv
@@ -0,0 +1,11 @@
+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,
+1.050000,0.000000,-0.000000,0.000000,-0.000000,0.830443,-0.276140,-0.384219,0.180493,-0.000000,0.000000,-0.000000,0.000000,-1.356107,0.688430,0.587426,-0.329106,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.710000,0.396058,0.918033,0.128965,-0.773546,0.339704,0.034310,-0.026417,-0.030049,-0.445604,-0.684403,0.888433,1.493630,-1.361618,-0.139316,0.158875,0.095799,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.620000,0.922409,0.405715,-0.582968,-0.092188,-0.114670,0.101046,0.075834,-0.037926,-0.291165,0.967514,0.421451,-1.086348,0.545211,0.030109,-0.050046,-0.068177,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.700000,0.923174,-0.431533,-0.682975,0.177173,0.319468,-0.043852,-0.111269,0.023166,0.289869,0.724722,-0.512011,-0.209623,-0.218710,0.108797,0.128756,-0.055461,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.560000,0.405364,-0.834716,0.158939,0.288175,-0.373738,-0.054995,0.036090,0.078627,0.450742,-0.385534,-0.954089,0.128288,0.442620,0.055630,-0.060142,-0.076163,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.560000,0.001062,-0.646270,-0.012560,-0.324065,0.125327,0.119738,0.034567,-0.063130,0.001593,-1.031457,0.015159,0.820816,-0.152665,-0.130729,-0.045679,0.080444,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.700000,-0.402804,-0.820508,-0.132914,0.236278,0.235164,-0.053551,-0.088687,0.031253,-0.449354,-0.411507,0.902946,0.185335,-0.239125,-0.041696,0.016857,0.016709,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.620000,-0.921641,-0.464596,0.661875,0.286582,-0.228921,-0.051987,0.004669,0.038463,-0.292459,0.777682,0.565788,-0.432472,-0.060568,-0.082048,-0.009439,0.041158,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.710000,-0.923935,0.447832,0.627381,-0.259808,-0.042325,-0.032258,0.001420,0.005294,0.288570,0.873350,-0.515586,-0.730207,-0.026023,0.288755,0.215678,-0.148061,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.053185,-0.398611,0.850510,-0.144007,-0.485368,-0.079781,0.176330,0.234482,-0.153567,0.447039,-0.532729,-0.855023,0.878509,0.775168,-0.391051,-0.713519,0.391628,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/figure8_csv.py b/real_robot/crazyswarm/scripts/figure8_csv.py
new file mode 100644
index 0000000000000000000000000000000000000000..27ed3fe980977779479889f3215125c9104f174b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/figure8_csv.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+from pycrazyswarm import *
+import uav_trajectory
+
+if __name__ == "__main__":
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+
+ traj1 = uav_trajectory.Trajectory()
+ traj1.loadcsv("figure8.csv")
+
+ TRIALS = 1
+ TIMESCALE = 1.0
+ for i in range(TRIALS):
+ for cf in allcfs.crazyflies:
+ cf.uploadTrajectory(0, 0, traj1)
+
+ allcfs.takeoff(targetHeight=1.0, duration=2.0)
+ timeHelper.sleep(2.5)
+ for cf in allcfs.crazyflies:
+ pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0])
+ cf.goTo(pos, 0, 2.0)
+ timeHelper.sleep(2.5)
+
+ allcfs.startTrajectory(0, timescale=TIMESCALE)
+ timeHelper.sleep(traj1.duration * TIMESCALE + 2.0)
+ allcfs.startTrajectory(0, timescale=TIMESCALE, reverse=True)
+ timeHelper.sleep(traj1.duration * TIMESCALE + 2.0)
+
+ allcfs.land(targetHeight=0.06, duration=2.0)
+ timeHelper.sleep(3.0)
diff --git a/real_robot/crazyswarm/scripts/graphVisualization.py b/real_robot/crazyswarm/scripts/graphVisualization.py
new file mode 100644
index 0000000000000000000000000000000000000000..1529c07724d2a756f96c66829101898bb683acf3
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/graphVisualization.py
@@ -0,0 +1,73 @@
+#!/usr/bin/env python
+
+"""Demonstrates the graph visualization feature of the 3D visualizer."""
+
+import numpy as np
+
+from pycrazyswarm import *
+
+crazyflies_yaml = """
+crazyflies:
+- id: 1
+ channel: 110
+ initialPosition: [-1.0, 0.0, 0.0]
+- id: 2
+ channel: 120
+ initialPosition: [-0.5, 0.0, 0.0]
+- id: 3
+ channel: 100
+ initialPosition: [0.0, 0.0, 0.0]
+- id: 4
+ channel: 110
+ initialPosition: [0.5, 0.0, 0.0]
+- id: 5
+ channel: 120
+ initialPosition: [1.0, 0.0, 0.0]
+"""
+
+# Cycle between pentagon and star to show graph visualization.
+t = np.linspace(0, 2 * np.pi, 6)[:-1]
+positions = np.column_stack([np.cos(t), np.sin(t), np.ones_like(t)])
+permutations = np.stack([np.arange(5), [0, 2, 4, 1, 3]])
+graph_edges_pentagon = (
+ (0, 1),
+ (1, 2),
+ (2, 3),
+ (3, 4),
+ (4, 0),
+)
+graph_edges_star = (
+ (0, 2),
+ (0, 3),
+ (1, 3),
+ (1, 4),
+ (2, 4),
+)
+
+if __name__ == "__main__":
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml)
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+ visualizer = timeHelper.visualizer
+ visualizer.setGraph(graph_edges_star)
+
+ cfs = allcfs.crazyflies
+
+ allcfs.takeoff(targetHeight=1.0, duration=2.0)
+ timeHelper.sleep(2.5)
+
+ # First, cycle between pentagon and star by moving.
+ n_cycles = 4
+ for cycle in range(n_cycles):
+ for i in range(5):
+ index = permutations[cycle % 2][i]
+ pos = positions[index]
+ cfs[i].goTo(pos, 0, 3.0)
+
+ timeHelper.sleep(6.0)
+
+ # Then, cycle between pentagon and star by changing the graph.
+ edges = (graph_edges_pentagon, graph_edges_star)
+ for cycle in range(n_cycles):
+ visualizer.setGraph(edges[cycle % 2])
+ timeHelper.sleep(6.0)
diff --git a/real_robot/crazyswarm/scripts/hello_world.py b/real_robot/crazyswarm/scripts/hello_world.py
new file mode 100755
index 0000000000000000000000000000000000000000..629e85eeac448676abe04e92a7d37cfa7b62815f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/hello_world.py
@@ -0,0 +1,22 @@
+"""Takeoff-hover-land for one CF. Useful to validate hardware config."""
+
+from pycrazyswarm import Crazyswarm
+
+
+TAKEOFF_DURATION = 2.5
+HOVER_DURATION = 5.0
+
+
+def main():
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ cf = swarm.allcfs.crazyflies[0]
+
+ cf.takeoff(targetHeight=1.0, duration=TAKEOFF_DURATION)
+ timeHelper.sleep(TAKEOFF_DURATION + HOVER_DURATION)
+ cf.land(targetHeight=0.04, duration=2.5)
+ timeHelper.sleep(TAKEOFF_DURATION)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/real_robot/crazyswarm/scripts/individual_hover.py b/real_robot/crazyswarm/scripts/individual_hover.py
new file mode 100644
index 0000000000000000000000000000000000000000..b2fb573c27ea4d9a0b5237bf1ec36f0a8e764aaa
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/individual_hover.py
@@ -0,0 +1,21 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+from pycrazyswarm import *
+
+def main():
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+
+ for cf in allcfs.crazyflies:
+ print(cf.id)
+ cf.takeoff(1.0, 2.5)
+ print("press button to continue")
+ swarm.input.waitUntilButtonPressed()
+ cf.land(0.04, 2.5)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/real_robot/crazyswarm/scripts/led_colors.py b/real_robot/crazyswarm/scripts/led_colors.py
new file mode 100644
index 0000000000000000000000000000000000000000..2644cca2c2ac6d37de12ac8effd9a5583a954498
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/led_colors.py
@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+from pycrazyswarm import *
+import uav_trajectory
+
+if __name__ == "__main__":
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+
+ # Configure the CFs so that the LED ring displays the solid color.
+ # Overrides the launch file and the firmware default.
+ for cf in allcfs.crazyflies:
+ cf.setParam("ring/effect", 7)
+
+ # Generate all possible on/off rgb values.
+ rgb_bits = [tuple((x >> k) & 0x1 for k in range(3)) for x in range(8)]
+
+ allcfs.takeoff(targetHeight=1.0, duration=2.0)
+ timeHelper.sleep(2.5)
+
+ TRIALS = 1
+ for i in range(TRIALS):
+ for rgb in rgb_bits:
+ for cf in allcfs.crazyflies:
+ cf.setLEDColor(*rgb)
+ timeHelper.sleep(1.0)
+
diff --git a/real_robot/crazyswarm/scripts/niceHover.py b/real_robot/crazyswarm/scripts/niceHover.py
new file mode 100644
index 0000000000000000000000000000000000000000..2b68f6a34ac75aca19f2560c56083c7796c2fb9f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/niceHover.py
@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+
+import numpy as np
+from pycrazyswarm import *
+
+Z = 1.0
+
+if __name__ == "__main__":
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ for cf in allcfs.crazyflies:
+ pos = np.array(cf.initialPosition) + np.array([0, 0, Z])
+ cf.goTo(pos, 0, 1.0)
+
+ print("press button to continue...")
+ swarm.input.waitUntilButtonPressed()
+
+ allcfs.land(targetHeight=0.02, duration=1.0+Z)
+ timeHelper.sleep(1.0+Z)
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/__init__.py b/real_robot/crazyswarm/scripts/pycrazyswarm/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..a33abaa708b8c99223806e1ed7f1ebdb4eb6feae
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/__init__.py
@@ -0,0 +1,3 @@
+from .crazyswarm_py import *
+
+__all__ = ["Crazyswarm", "cfsim"]
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/__init__.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..38d69ed964b5465164fa1f73b01683e95f713bfa
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/__init__.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/__init__.cpython-37.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/__init__.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..7a4b682737bd323a2fc8b74ce1f163fb6cb008fc
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/__init__.cpython-37.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/crazyflie.cpython-37.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/crazyflie.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..68786bf906b01f7b0aca63243a3d5d393553a4ea
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/crazyflie.cpython-37.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/crazyswarm_py.cpython-37.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/crazyswarm_py.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b6b9fa2042878fa217a9a296581450cde7e1ba71
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/crazyswarm_py.cpython-37.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/genericJoystick.cpython-37.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/genericJoystick.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..242c0e92bac21219a19a62502784aa2744794d31
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/genericJoystick.cpython-37.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/keyboard.cpython-37.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/keyboard.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..14374d34c2a2c10d6d01a609160f05551348fae4
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/__pycache__/keyboard.cpython-37.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/.gitignore b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..417c588e46254514616615cdb27e8560fd9be65e
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/.gitignore
@@ -0,0 +1,4 @@
+cffirmware.py
+cffirmware_wrap.c
+*.so
+build
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/Makefile b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/Makefile
new file mode 100644
index 0000000000000000000000000000000000000000..c367a63f022816a8677cc5a83fb81eb71b4e1c14
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/Makefile
@@ -0,0 +1,16 @@
+firmdir = ../../../../../../crazyflie-firmware
+modinc = $(firmdir)/src/modules/interface
+modsrc = $(firmdir)/src/modules/src
+
+swig: setup.py cffirmware_wrap.c $(modsrc)/*.c
+ifeq ($(CSW_PYTHON),)
+ $(error Environment variable CSW_PYTHON must be set to "python2" or "python3")
+endif
+ $(CSW_PYTHON) setup.py build_ext --inplace
+
+cffirmware_wrap.c: cffirmware.i $(modinc)/*.h
+ swig -python -I$(modinc) cffirmware.i
+
+clean:
+ rm -f cffirmware.py _cffirmware.so *.pyc cffirmware_wrap.c
+ rm -rf build
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/__init__.py b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/__init__.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..780f95e390aa9fd045267b9385b17c26759bd08b
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/__init__.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/cffirmware.i b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/cffirmware.i
new file mode 100644
index 0000000000000000000000000000000000000000..eaab0b25a7f0937bba9099c09cc9f77358ff1c02
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/cffirmware.i
@@ -0,0 +1,139 @@
+%module cffirmware
+
+// ignore GNU specific compiler attributes
+#define __attribute__(x)
+
+%{
+#define SWIG_FILE_WITH_INIT
+#include "collision_avoidance.h"
+#include "math3d.h"
+#include "pptraj.h"
+#include "planner.h"
+#include "stabilizer_types.h"
+%}
+
+%include "collision_avoidance.h"
+%include "math3d.h"
+%include "pptraj.h"
+%include "planner.h"
+%include "stabilizer_types.h"
+
+%include "numpy.i"
+
+
+%init %{
+ import_array()
+%}
+
+%apply (int DIM1, float* IN_ARRAY1) {(int nOthers, float const *otherPositions)}
+
+%inline %{
+void poly4d_set(struct poly4d *poly, int dim, int coef, float val)
+{
+ poly->p[dim][coef] = val;
+}
+float poly4d_get(struct poly4d *poly, int dim, int coef)
+{
+ return poly->p[dim][coef];
+}
+struct poly4d* pp_get_piece(struct piecewise_traj *pp, int i)
+{
+ return &pp->pieces[i];
+}
+struct poly4d* malloc_poly4d(int size)
+{
+ return (struct poly4d*)malloc(sizeof(struct poly4d) * size);
+}
+
+struct vec vec2svec(struct vec3_s v)
+{
+ return mkvec(v.x, v.y, v.z);
+}
+
+struct vec3_s svec2vec(struct vec v)
+{
+ struct vec3_s vv = { .x = v.x, .y = v.y, .z = v.z, };
+ return vv;
+}
+
+void collisionAvoidanceUpdateSetpointWrap(
+ collision_avoidance_params_t const *params,
+ collision_avoidance_state_t *collisionState,
+ int nOthers,
+ float const *otherPositions,
+ setpoint_t *setpoint, sensorData_t const *sensorData, state_t const *state)
+{
+ nOthers /= 3;
+ float *workspace = malloc(sizeof(float) * 7 * (nOthers + 6));
+ collisionAvoidanceUpdateSetpointCore(
+ params,
+ collisionState,
+ nOthers,
+ otherPositions,
+ workspace,
+ setpoint, sensorData, state
+ );
+ free(workspace);
+}
+%}
+
+
+%pythoncode %{
+import numpy as np
+%}
+
+#define COPY_CTOR(structname) \
+structname(struct structname const *x) { \
+ struct structname *y = malloc(sizeof(struct structname)); \
+ *y = *x; \
+ return y; \
+} \
+~structname() { \
+ free($self); \
+} \
+
+%extend vec {
+ COPY_CTOR(vec)
+
+ %pythoncode %{
+ def __repr__(self):
+ return "({}, {}, {})".format(self.x, self.y, self.z)
+
+ def __array__(self):
+ return np.array([self.x, self.y, self.z])
+
+ def __len__(self):
+ return 3
+
+ def __getitem__(self, i):
+ if 0 <= i and i < 3:
+ return _cffirmware.vindex(self, i)
+ else:
+ raise IndexError("vec index must be in {0, 1, 2}.")
+
+ # Unary operator overloads.
+ def __neg__(self):
+ return _cffirmware.vneg(self)
+
+ # Vector-scalar binary operator overloads.
+ def __rmul__(self, s):
+ return _cffirmware.vscl(s, self)
+
+ def __div__(self, s):
+ return self.__truediv__(s)
+
+ def __truediv__(self, s):
+ return _cffirmware.vdiv(self, s)
+
+ # Vector-vector binary operator overloads.
+ def __add__(self, other):
+ return _cffirmware.vadd(self, other)
+
+ def __sub__(self, other):
+ return _cffirmware.vsub(self, other)
+ %}
+};
+
+%extend traj_eval {
+ COPY_CTOR(traj_eval)
+};
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/numpy.i b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/numpy.i
new file mode 100644
index 0000000000000000000000000000000000000000..8416e82f3a9017123a931eb53339617a2f4bdcbd
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/numpy.i
@@ -0,0 +1,3178 @@
+/* -*- C -*- (not really, but good for syntax highlighting) */
+
+/*
+ * Copyright (c) 2005-2015, NumPy Developers.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are
+ * met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ *
+ * * Neither the name of the NumPy Developers nor the names of any
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifdef SWIGPYTHON
+
+%{
+#ifndef SWIG_FILE_WITH_INIT
+#define NO_IMPORT_ARRAY
+#endif
+#include "stdio.h"
+#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION
+#include <numpy/arrayobject.h>
+%}
+
+/**********************************************************************/
+
+%fragment("NumPy_Backward_Compatibility", "header")
+{
+%#if NPY_API_VERSION < 0x00000007
+%#define NPY_ARRAY_DEFAULT NPY_DEFAULT
+%#define NPY_ARRAY_FARRAY NPY_FARRAY
+%#define NPY_FORTRANORDER NPY_FORTRAN
+%#endif
+}
+
+/**********************************************************************/
+
+/* The following code originally appeared in
+ * enthought/kiva/agg/src/numeric.i written by Eric Jones. It was
+ * translated from C++ to C by John Hunter. Bill Spotz has modified
+ * it to fix some minor bugs, upgrade from Numeric to numpy (all
+ * versions), add some comments and functionality, and convert from
+ * direct code insertion to SWIG fragments.
+ */
+
+%fragment("NumPy_Macros", "header")
+{
+/* Macros to extract array attributes.
+ */
+%#if NPY_API_VERSION < 0x00000007
+%#define is_array(a) ((a) && PyArray_Check((PyArrayObject*)a))
+%#define array_type(a) (int)(PyArray_TYPE((PyArrayObject*)a))
+%#define array_numdims(a) (((PyArrayObject*)a)->nd)
+%#define array_dimensions(a) (((PyArrayObject*)a)->dimensions)
+%#define array_size(a,i) (((PyArrayObject*)a)->dimensions[i])
+%#define array_strides(a) (((PyArrayObject*)a)->strides)
+%#define array_stride(a,i) (((PyArrayObject*)a)->strides[i])
+%#define array_data(a) (((PyArrayObject*)a)->data)
+%#define array_descr(a) (((PyArrayObject*)a)->descr)
+%#define array_flags(a) (((PyArrayObject*)a)->flags)
+%#define array_clearflags(a,f) (((PyArrayObject*)a)->flags) &= ~f
+%#define array_enableflags(a,f) (((PyArrayObject*)a)->flags) = f
+%#define array_is_fortran(a) (PyArray_ISFORTRAN((PyArrayObject*)a))
+%#else
+%#define is_array(a) ((a) && PyArray_Check(a))
+%#define array_type(a) PyArray_TYPE((PyArrayObject*)a)
+%#define array_numdims(a) PyArray_NDIM((PyArrayObject*)a)
+%#define array_dimensions(a) PyArray_DIMS((PyArrayObject*)a)
+%#define array_strides(a) PyArray_STRIDES((PyArrayObject*)a)
+%#define array_stride(a,i) PyArray_STRIDE((PyArrayObject*)a,i)
+%#define array_size(a,i) PyArray_DIM((PyArrayObject*)a,i)
+%#define array_data(a) PyArray_DATA((PyArrayObject*)a)
+%#define array_descr(a) PyArray_DESCR((PyArrayObject*)a)
+%#define array_flags(a) PyArray_FLAGS((PyArrayObject*)a)
+%#define array_enableflags(a,f) PyArray_ENABLEFLAGS((PyArrayObject*)a,f)
+%#define array_clearflags(a,f) PyArray_CLEARFLAGS((PyArrayObject*)a,f)
+%#define array_is_fortran(a) (PyArray_IS_F_CONTIGUOUS((PyArrayObject*)a))
+%#endif
+%#define array_is_contiguous(a) (PyArray_ISCONTIGUOUS((PyArrayObject*)a))
+%#define array_is_native(a) (PyArray_ISNOTSWAPPED((PyArrayObject*)a))
+}
+
+/**********************************************************************/
+
+%fragment("NumPy_Utilities",
+ "header")
+{
+ /* Given a PyObject, return a string describing its type.
+ */
+ const char* pytype_string(PyObject* py_obj)
+ {
+ if (py_obj == NULL ) return "C NULL value";
+ if (py_obj == Py_None ) return "Python None" ;
+ if (PyCallable_Check(py_obj)) return "callable" ;
+ if (PyString_Check( py_obj)) return "string" ;
+ if (PyInt_Check( py_obj)) return "int" ;
+ if (PyFloat_Check( py_obj)) return "float" ;
+ if (PyDict_Check( py_obj)) return "dict" ;
+ if (PyList_Check( py_obj)) return "list" ;
+ if (PyTuple_Check( py_obj)) return "tuple" ;
+
+ return "unknown type";
+ }
+
+ /* Given a NumPy typecode, return a string describing the type.
+ */
+ const char* typecode_string(int typecode)
+ {
+ static const char* type_names[25] = {"bool",
+ "byte",
+ "unsigned byte",
+ "short",
+ "unsigned short",
+ "int",
+ "unsigned int",
+ "long",
+ "unsigned long",
+ "long long",
+ "unsigned long long",
+ "float",
+ "double",
+ "long double",
+ "complex float",
+ "complex double",
+ "complex long double",
+ "object",
+ "string",
+ "unicode",
+ "void",
+ "ntypes",
+ "notype",
+ "char",
+ "unknown"};
+ return typecode < 24 ? type_names[typecode] : type_names[24];
+ }
+
+ /* Make sure input has correct numpy type. This now just calls
+ PyArray_EquivTypenums().
+ */
+ int type_match(int actual_type,
+ int desired_type)
+ {
+ return PyArray_EquivTypenums(actual_type, desired_type);
+ }
+
+%#ifdef SWIGPY_USE_CAPSULE
+ void free_cap(PyObject * cap)
+ {
+ void* array = (void*) PyCapsule_GetPointer(cap,SWIGPY_CAPSULE_NAME);
+ if (array != NULL) free(array);
+ }
+%#endif
+
+
+}
+
+/**********************************************************************/
+
+%fragment("NumPy_Object_to_Array",
+ "header",
+ fragment="NumPy_Backward_Compatibility",
+ fragment="NumPy_Macros",
+ fragment="NumPy_Utilities")
+{
+ /* Given a PyObject pointer, cast it to a PyArrayObject pointer if
+ * legal. If not, set the python error string appropriately and
+ * return NULL.
+ */
+ PyArrayObject* obj_to_array_no_conversion(PyObject* input,
+ int typecode)
+ {
+ PyArrayObject* ary = NULL;
+ if (is_array(input) && (typecode == NPY_NOTYPE ||
+ PyArray_EquivTypenums(array_type(input), typecode)))
+ {
+ ary = (PyArrayObject*) input;
+ }
+ else if is_array(input)
+ {
+ const char* desired_type = typecode_string(typecode);
+ const char* actual_type = typecode_string(array_type(input));
+ PyErr_Format(PyExc_TypeError,
+ "Array of type '%s' required. Array of type '%s' given",
+ desired_type, actual_type);
+ ary = NULL;
+ }
+ else
+ {
+ const char* desired_type = typecode_string(typecode);
+ const char* actual_type = pytype_string(input);
+ PyErr_Format(PyExc_TypeError,
+ "Array of type '%s' required. A '%s' was given",
+ desired_type,
+ actual_type);
+ ary = NULL;
+ }
+ return ary;
+ }
+
+ /* Convert the given PyObject to a NumPy array with the given
+ * typecode. On success, return a valid PyArrayObject* with the
+ * correct type. On failure, the python error string will be set and
+ * the routine returns NULL.
+ */
+ PyArrayObject* obj_to_array_allow_conversion(PyObject* input,
+ int typecode,
+ int* is_new_object)
+ {
+ PyArrayObject* ary = NULL;
+ PyObject* py_obj;
+ if (is_array(input) && (typecode == NPY_NOTYPE ||
+ PyArray_EquivTypenums(array_type(input),typecode)))
+ {
+ ary = (PyArrayObject*) input;
+ *is_new_object = 0;
+ }
+ else
+ {
+ py_obj = PyArray_FROMANY(input, typecode, 0, 0, NPY_ARRAY_DEFAULT);
+ /* If NULL, PyArray_FromObject will have set python error value.*/
+ ary = (PyArrayObject*) py_obj;
+ *is_new_object = 1;
+ }
+ return ary;
+ }
+
+ /* Given a PyArrayObject, check to see if it is contiguous. If so,
+ * return the input pointer and flag it as not a new object. If it is
+ * not contiguous, create a new PyArrayObject using the original data,
+ * flag it as a new object and return the pointer.
+ */
+ PyArrayObject* make_contiguous(PyArrayObject* ary,
+ int* is_new_object,
+ int min_dims,
+ int max_dims)
+ {
+ PyArrayObject* result;
+ if (array_is_contiguous(ary))
+ {
+ result = ary;
+ *is_new_object = 0;
+ }
+ else
+ {
+ result = (PyArrayObject*) PyArray_ContiguousFromObject((PyObject*)ary,
+ array_type(ary),
+ min_dims,
+ max_dims);
+ *is_new_object = 1;
+ }
+ return result;
+ }
+
+ /* Given a PyArrayObject, check to see if it is Fortran-contiguous.
+ * If so, return the input pointer, but do not flag it as not a new
+ * object. If it is not Fortran-contiguous, create a new
+ * PyArrayObject using the original data, flag it as a new object
+ * and return the pointer.
+ */
+ PyArrayObject* make_fortran(PyArrayObject* ary,
+ int* is_new_object)
+ {
+ PyArrayObject* result;
+ if (array_is_fortran(ary))
+ {
+ result = ary;
+ *is_new_object = 0;
+ }
+ else
+ {
+ Py_INCREF(array_descr(ary));
+ result = (PyArrayObject*) PyArray_FromArray(ary,
+ array_descr(ary),
+%#if NPY_API_VERSION < 0x00000007
+ NPY_FORTRANORDER);
+%#else
+ NPY_ARRAY_F_CONTIGUOUS);
+%#endif
+ *is_new_object = 1;
+ }
+ return result;
+ }
+
+ /* Convert a given PyObject to a contiguous PyArrayObject of the
+ * specified type. If the input object is not a contiguous
+ * PyArrayObject, a new one will be created and the new object flag
+ * will be set.
+ */
+ PyArrayObject* obj_to_array_contiguous_allow_conversion(PyObject* input,
+ int typecode,
+ int* is_new_object)
+ {
+ int is_new1 = 0;
+ int is_new2 = 0;
+ PyArrayObject* ary2;
+ PyArrayObject* ary1 = obj_to_array_allow_conversion(input,
+ typecode,
+ &is_new1);
+ if (ary1)
+ {
+ ary2 = make_contiguous(ary1, &is_new2, 0, 0);
+ if ( is_new1 && is_new2)
+ {
+ Py_DECREF(ary1);
+ }
+ ary1 = ary2;
+ }
+ *is_new_object = is_new1 || is_new2;
+ return ary1;
+ }
+
+ /* Convert a given PyObject to a Fortran-ordered PyArrayObject of the
+ * specified type. If the input object is not a Fortran-ordered
+ * PyArrayObject, a new one will be created and the new object flag
+ * will be set.
+ */
+ PyArrayObject* obj_to_array_fortran_allow_conversion(PyObject* input,
+ int typecode,
+ int* is_new_object)
+ {
+ int is_new1 = 0;
+ int is_new2 = 0;
+ PyArrayObject* ary2;
+ PyArrayObject* ary1 = obj_to_array_allow_conversion(input,
+ typecode,
+ &is_new1);
+ if (ary1)
+ {
+ ary2 = make_fortran(ary1, &is_new2);
+ if (is_new1 && is_new2)
+ {
+ Py_DECREF(ary1);
+ }
+ ary1 = ary2;
+ }
+ *is_new_object = is_new1 || is_new2;
+ return ary1;
+ }
+} /* end fragment */
+
+/**********************************************************************/
+
+%fragment("NumPy_Array_Requirements",
+ "header",
+ fragment="NumPy_Backward_Compatibility",
+ fragment="NumPy_Macros")
+{
+ /* Test whether a python object is contiguous. If array is
+ * contiguous, return 1. Otherwise, set the python error string and
+ * return 0.
+ */
+ int require_contiguous(PyArrayObject* ary)
+ {
+ int contiguous = 1;
+ if (!array_is_contiguous(ary))
+ {
+ PyErr_SetString(PyExc_TypeError,
+ "Array must be contiguous. A non-contiguous array was given");
+ contiguous = 0;
+ }
+ return contiguous;
+ }
+
+ /* Test whether a python object is (C_ or F_) contiguous. If array is
+ * contiguous, return 1. Otherwise, set the python error string and
+ * return 0.
+ */
+ int require_c_or_f_contiguous(PyArrayObject* ary)
+ {
+ int contiguous = 1;
+ if (!(array_is_contiguous(ary) || array_is_fortran(ary)))
+ {
+ PyErr_SetString(PyExc_TypeError,
+ "Array must be contiguous (C_ or F_). A non-contiguous array was given");
+ contiguous = 0;
+ }
+ return contiguous;
+ }
+
+ /* Require that a numpy array is not byte-swapped. If the array is
+ * not byte-swapped, return 1. Otherwise, set the python error string
+ * and return 0.
+ */
+ int require_native(PyArrayObject* ary)
+ {
+ int native = 1;
+ if (!array_is_native(ary))
+ {
+ PyErr_SetString(PyExc_TypeError,
+ "Array must have native byteorder. "
+ "A byte-swapped array was given");
+ native = 0;
+ }
+ return native;
+ }
+
+ /* Require the given PyArrayObject to have a specified number of
+ * dimensions. If the array has the specified number of dimensions,
+ * return 1. Otherwise, set the python error string and return 0.
+ */
+ int require_dimensions(PyArrayObject* ary,
+ int exact_dimensions)
+ {
+ int success = 1;
+ if (array_numdims(ary) != exact_dimensions)
+ {
+ PyErr_Format(PyExc_TypeError,
+ "Array must have %d dimensions. Given array has %d dimensions",
+ exact_dimensions,
+ array_numdims(ary));
+ success = 0;
+ }
+ return success;
+ }
+
+ /* Require the given PyArrayObject to have one of a list of specified
+ * number of dimensions. If the array has one of the specified number
+ * of dimensions, return 1. Otherwise, set the python error string
+ * and return 0.
+ */
+ int require_dimensions_n(PyArrayObject* ary,
+ int* exact_dimensions,
+ int n)
+ {
+ int success = 0;
+ int i;
+ char dims_str[255] = "";
+ char s[255];
+ for (i = 0; i < n && !success; i++)
+ {
+ if (array_numdims(ary) == exact_dimensions[i])
+ {
+ success = 1;
+ }
+ }
+ if (!success)
+ {
+ for (i = 0; i < n-1; i++)
+ {
+ sprintf(s, "%d, ", exact_dimensions[i]);
+ strcat(dims_str,s);
+ }
+ sprintf(s, " or %d", exact_dimensions[n-1]);
+ strcat(dims_str,s);
+ PyErr_Format(PyExc_TypeError,
+ "Array must have %s dimensions. Given array has %d dimensions",
+ dims_str,
+ array_numdims(ary));
+ }
+ return success;
+ }
+
+ /* Require the given PyArrayObject to have a specified shape. If the
+ * array has the specified shape, return 1. Otherwise, set the python
+ * error string and return 0.
+ */
+ int require_size(PyArrayObject* ary,
+ npy_intp* size,
+ int n)
+ {
+ int i;
+ int success = 1;
+ size_t len;
+ char desired_dims[255] = "[";
+ char s[255];
+ char actual_dims[255] = "[";
+ for(i=0; i < n;i++)
+ {
+ if (size[i] != -1 && size[i] != array_size(ary,i))
+ {
+ success = 0;
+ }
+ }
+ if (!success)
+ {
+ for (i = 0; i < n; i++)
+ {
+ if (size[i] == -1)
+ {
+ sprintf(s, "*,");
+ }
+ else
+ {
+ sprintf(s, "%ld,", (long int)size[i]);
+ }
+ strcat(desired_dims,s);
+ }
+ len = strlen(desired_dims);
+ desired_dims[len-1] = ']';
+ for (i = 0; i < n; i++)
+ {
+ sprintf(s, "%ld,", (long int)array_size(ary,i));
+ strcat(actual_dims,s);
+ }
+ len = strlen(actual_dims);
+ actual_dims[len-1] = ']';
+ PyErr_Format(PyExc_TypeError,
+ "Array must have shape of %s. Given array has shape of %s",
+ desired_dims,
+ actual_dims);
+ }
+ return success;
+ }
+
+ /* Require the given PyArrayObject to to be Fortran ordered. If the
+ * the PyArrayObject is already Fortran ordered, do nothing. Else,
+ * set the Fortran ordering flag and recompute the strides.
+ */
+ int require_fortran(PyArrayObject* ary)
+ {
+ int success = 1;
+ int nd = array_numdims(ary);
+ int i;
+ npy_intp * strides = array_strides(ary);
+ if (array_is_fortran(ary)) return success;
+ int n_non_one = 0;
+ /* Set the Fortran ordered flag */
+ const npy_intp *dims = array_dimensions(ary);
+ for (i=0; i < nd; ++i)
+ n_non_one += (dims[i] != 1) ? 1 : 0;
+ if (n_non_one > 1)
+ array_clearflags(ary,NPY_ARRAY_CARRAY);
+ array_enableflags(ary,NPY_ARRAY_FARRAY);
+ /* Recompute the strides */
+ strides[0] = strides[nd-1];
+ for (i=1; i < nd; ++i)
+ strides[i] = strides[i-1] * array_size(ary,i-1);
+ return success;
+ }
+}
+
+/* Combine all NumPy fragments into one for convenience */
+%fragment("NumPy_Fragments",
+ "header",
+ fragment="NumPy_Backward_Compatibility",
+ fragment="NumPy_Macros",
+ fragment="NumPy_Utilities",
+ fragment="NumPy_Object_to_Array",
+ fragment="NumPy_Array_Requirements")
+{
+}
+
+/* End John Hunter translation (with modifications by Bill Spotz)
+ */
+
+/* %numpy_typemaps() macro
+ *
+ * This macro defines a family of 75 typemaps that allow C arguments
+ * of the form
+ *
+ * 1. (DATA_TYPE IN_ARRAY1[ANY])
+ * 2. (DATA_TYPE* IN_ARRAY1, DIM_TYPE DIM1)
+ * 3. (DIM_TYPE DIM1, DATA_TYPE* IN_ARRAY1)
+ *
+ * 4. (DATA_TYPE IN_ARRAY2[ANY][ANY])
+ * 5. (DATA_TYPE* IN_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ * 6. (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_ARRAY2)
+ * 7. (DATA_TYPE* IN_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ * 8. (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_FARRAY2)
+ *
+ * 9. (DATA_TYPE IN_ARRAY3[ANY][ANY][ANY])
+ * 10. (DATA_TYPE* IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ * 11. (DATA_TYPE** IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ * 12. (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* IN_ARRAY3)
+ * 13. (DATA_TYPE* IN_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ * 14. (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* IN_FARRAY3)
+ *
+ * 15. (DATA_TYPE IN_ARRAY4[ANY][ANY][ANY][ANY])
+ * 16. (DATA_TYPE* IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ * 17. (DATA_TYPE** IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ * 18. (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, , DIM_TYPE DIM4, DATA_TYPE* IN_ARRAY4)
+ * 19. (DATA_TYPE* IN_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ * 20. (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* IN_FARRAY4)
+ *
+ * 21. (DATA_TYPE INPLACE_ARRAY1[ANY])
+ * 22. (DATA_TYPE* INPLACE_ARRAY1, DIM_TYPE DIM1)
+ * 23. (DIM_TYPE DIM1, DATA_TYPE* INPLACE_ARRAY1)
+ *
+ * 24. (DATA_TYPE INPLACE_ARRAY2[ANY][ANY])
+ * 25. (DATA_TYPE* INPLACE_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ * 26. (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* INPLACE_ARRAY2)
+ * 27. (DATA_TYPE* INPLACE_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ * 28. (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* INPLACE_FARRAY2)
+ *
+ * 29. (DATA_TYPE INPLACE_ARRAY3[ANY][ANY][ANY])
+ * 30. (DATA_TYPE* INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ * 31. (DATA_TYPE** INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ * 32. (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* INPLACE_ARRAY3)
+ * 33. (DATA_TYPE* INPLACE_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ * 34. (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* INPLACE_FARRAY3)
+ *
+ * 35. (DATA_TYPE INPLACE_ARRAY4[ANY][ANY][ANY][ANY])
+ * 36. (DATA_TYPE* INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ * 37. (DATA_TYPE** INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ * 38. (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* INPLACE_ARRAY4)
+ * 39. (DATA_TYPE* INPLACE_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ * 40. (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* INPLACE_FARRAY4)
+ *
+ * 41. (DATA_TYPE ARGOUT_ARRAY1[ANY])
+ * 42. (DATA_TYPE* ARGOUT_ARRAY1, DIM_TYPE DIM1)
+ * 43. (DIM_TYPE DIM1, DATA_TYPE* ARGOUT_ARRAY1)
+ *
+ * 44. (DATA_TYPE ARGOUT_ARRAY2[ANY][ANY])
+ *
+ * 45. (DATA_TYPE ARGOUT_ARRAY3[ANY][ANY][ANY])
+ *
+ * 46. (DATA_TYPE ARGOUT_ARRAY4[ANY][ANY][ANY][ANY])
+ *
+ * 47. (DATA_TYPE** ARGOUTVIEW_ARRAY1, DIM_TYPE* DIM1)
+ * 48. (DIM_TYPE* DIM1, DATA_TYPE** ARGOUTVIEW_ARRAY1)
+ *
+ * 49. (DATA_TYPE** ARGOUTVIEW_ARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+ * 50. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEW_ARRAY2)
+ * 51. (DATA_TYPE** ARGOUTVIEW_FARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+ * 52. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEW_FARRAY2)
+ *
+ * 53. (DATA_TYPE** ARGOUTVIEW_ARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+ * 54. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEW_ARRAY3)
+ * 55. (DATA_TYPE** ARGOUTVIEW_FARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+ * 56. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEW_FARRAY3)
+ *
+ * 57. (DATA_TYPE** ARGOUTVIEW_ARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ * 58. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEW_ARRAY4)
+ * 59. (DATA_TYPE** ARGOUTVIEW_FARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ * 60. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEW_FARRAY4)
+ *
+ * 61. (DATA_TYPE** ARGOUTVIEWM_ARRAY1, DIM_TYPE* DIM1)
+ * 62. (DIM_TYPE* DIM1, DATA_TYPE** ARGOUTVIEWM_ARRAY1)
+ *
+ * 63. (DATA_TYPE** ARGOUTVIEWM_ARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+ * 64. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEWM_ARRAY2)
+ * 65. (DATA_TYPE** ARGOUTVIEWM_FARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+ * 66. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEWM_FARRAY2)
+ *
+ * 67. (DATA_TYPE** ARGOUTVIEWM_ARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+ * 68. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEWM_ARRAY3)
+ * 69. (DATA_TYPE** ARGOUTVIEWM_FARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+ * 70. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEWM_FARRAY3)
+ *
+ * 71. (DATA_TYPE** ARGOUTVIEWM_ARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ * 72. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEWM_ARRAY4)
+ * 73. (DATA_TYPE** ARGOUTVIEWM_FARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ * 74. (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEWM_FARRAY4)
+ *
+ * 75. (DATA_TYPE* INPLACE_ARRAY_FLAT, DIM_TYPE DIM_FLAT)
+ *
+ * where "DATA_TYPE" is any type supported by the NumPy module, and
+ * "DIM_TYPE" is any int-like type suitable for specifying dimensions.
+ * The difference between "ARRAY" typemaps and "FARRAY" typemaps is
+ * that the "FARRAY" typemaps expect Fortran ordering of
+ * multidimensional arrays. In python, the dimensions will not need
+ * to be specified (except for the "DATA_TYPE* ARGOUT_ARRAY1"
+ * typemaps). The IN_ARRAYs can be a numpy array or any sequence that
+ * can be converted to a numpy array of the specified type. The
+ * INPLACE_ARRAYs must be numpy arrays of the appropriate type. The
+ * ARGOUT_ARRAYs will be returned as new numpy arrays of the
+ * appropriate type.
+ *
+ * These typemaps can be applied to existing functions using the
+ * %apply directive. For example:
+ *
+ * %apply (double* IN_ARRAY1, int DIM1) {(double* series, int length)};
+ * double prod(double* series, int length);
+ *
+ * %apply (int DIM1, int DIM2, double* INPLACE_ARRAY2)
+ * {(int rows, int cols, double* matrix )};
+ * void floor(int rows, int cols, double* matrix, double f);
+ *
+ * %apply (double IN_ARRAY3[ANY][ANY][ANY])
+ * {(double tensor[2][2][2] )};
+ * %apply (double ARGOUT_ARRAY3[ANY][ANY][ANY])
+ * {(double low[2][2][2] )};
+ * %apply (double ARGOUT_ARRAY3[ANY][ANY][ANY])
+ * {(double upp[2][2][2] )};
+ * void luSplit(double tensor[2][2][2],
+ * double low[2][2][2],
+ * double upp[2][2][2] );
+ *
+ * or directly with
+ *
+ * double prod(double* IN_ARRAY1, int DIM1);
+ *
+ * void floor(int DIM1, int DIM2, double* INPLACE_ARRAY2, double f);
+ *
+ * void luSplit(double IN_ARRAY3[ANY][ANY][ANY],
+ * double ARGOUT_ARRAY3[ANY][ANY][ANY],
+ * double ARGOUT_ARRAY3[ANY][ANY][ANY]);
+ */
+
+%define %numpy_typemaps(DATA_TYPE, DATA_TYPECODE, DIM_TYPE)
+
+/************************/
+/* Input Array Typemaps */
+/************************/
+
+/* Typemap suite for (DATA_TYPE IN_ARRAY1[ANY])
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE IN_ARRAY1[ANY])
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE IN_ARRAY1[ANY])
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[1] = { $1_dim0 };
+ array = obj_to_array_contiguous_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 1) ||
+ !require_size(array, size, 1)) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+%typemap(freearg)
+ (DATA_TYPE IN_ARRAY1[ANY])
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE* IN_ARRAY1, DIM_TYPE DIM1)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* IN_ARRAY1, DIM_TYPE DIM1)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* IN_ARRAY1, DIM_TYPE DIM1)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[1] = { -1 };
+ array = obj_to_array_contiguous_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 1) ||
+ !require_size(array, size, 1)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+}
+%typemap(freearg)
+ (DATA_TYPE* IN_ARRAY1, DIM_TYPE DIM1)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DATA_TYPE* IN_ARRAY1)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DATA_TYPE* IN_ARRAY1)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DATA_TYPE* IN_ARRAY1)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[1] = {-1};
+ array = obj_to_array_contiguous_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 1) ||
+ !require_size(array, size, 1)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DATA_TYPE*) array_data(array);
+}
+%typemap(freearg)
+ (DIM_TYPE DIM1, DATA_TYPE* IN_ARRAY1)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE IN_ARRAY2[ANY][ANY])
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE IN_ARRAY2[ANY][ANY])
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE IN_ARRAY2[ANY][ANY])
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[2] = { $1_dim0, $1_dim1 };
+ array = obj_to_array_contiguous_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 2) ||
+ !require_size(array, size, 2)) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+%typemap(freearg)
+ (DATA_TYPE IN_ARRAY2[ANY][ANY])
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE* IN_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* IN_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* IN_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[2] = { -1, -1 };
+ array = obj_to_array_contiguous_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 2) ||
+ !require_size(array, size, 2)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+}
+%typemap(freearg)
+ (DATA_TYPE* IN_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_ARRAY2)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_ARRAY2)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_ARRAY2)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[2] = { -1, -1 };
+ array = obj_to_array_contiguous_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 2) ||
+ !require_size(array, size, 2)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DATA_TYPE*) array_data(array);
+}
+%typemap(freearg)
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_ARRAY2)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE* IN_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* IN_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* IN_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[2] = { -1, -1 };
+ array = obj_to_array_fortran_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 2) ||
+ !require_size(array, size, 2) || !require_fortran(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+}
+%typemap(freearg)
+ (DATA_TYPE* IN_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_FARRAY2)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_FARRAY2)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_FARRAY2)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[2] = { -1, -1 };
+ array = obj_to_array_fortran_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 2) ||
+ !require_size(array, size, 2) || !require_fortran(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DATA_TYPE*) array_data(array);
+}
+%typemap(freearg)
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* IN_FARRAY2)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE IN_ARRAY3[ANY][ANY][ANY])
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE IN_ARRAY3[ANY][ANY][ANY])
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE IN_ARRAY3[ANY][ANY][ANY])
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[3] = { $1_dim0, $1_dim1, $1_dim2 };
+ array = obj_to_array_contiguous_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 3) ||
+ !require_size(array, size, 3)) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+%typemap(freearg)
+ (DATA_TYPE IN_ARRAY3[ANY][ANY][ANY])
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE* IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[3] = { -1, -1, -1 };
+ array = obj_to_array_contiguous_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 3) ||
+ !require_size(array, size, 3)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+ $4 = (DIM_TYPE) array_size(array,2);
+}
+%typemap(freearg)
+ (DATA_TYPE* IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE** IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE** IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ /* for now, only concerned with lists */
+ $1 = PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE** IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ (DATA_TYPE** array=NULL, PyArrayObject** object_array=NULL, int* is_new_object_array=NULL)
+{
+ npy_intp size[2] = { -1, -1 };
+ PyArrayObject* temp_array;
+ Py_ssize_t i;
+ int is_new_object;
+
+ /* length of the list */
+ $2 = PyList_Size($input);
+
+ /* the arrays */
+ array = (DATA_TYPE **)malloc($2*sizeof(DATA_TYPE *));
+ object_array = (PyArrayObject **)calloc($2,sizeof(PyArrayObject *));
+ is_new_object_array = (int *)calloc($2,sizeof(int));
+
+ if (array == NULL || object_array == NULL || is_new_object_array == NULL)
+ {
+ SWIG_fail;
+ }
+
+ for (i=0; i<$2; i++)
+ {
+ temp_array = obj_to_array_contiguous_allow_conversion(PySequence_GetItem($input,i), DATA_TYPECODE, &is_new_object);
+
+ /* the new array must be stored so that it can be destroyed in freearg */
+ object_array[i] = temp_array;
+ is_new_object_array[i] = is_new_object;
+
+ if (!temp_array || !require_dimensions(temp_array, 2)) SWIG_fail;
+
+ /* store the size of the first array in the list, then use that for comparison. */
+ if (i == 0)
+ {
+ size[0] = array_size(temp_array,0);
+ size[1] = array_size(temp_array,1);
+ }
+
+ if (!require_size(temp_array, size, 2)) SWIG_fail;
+
+ array[i] = (DATA_TYPE*) array_data(temp_array);
+ }
+
+ $1 = (DATA_TYPE**) array;
+ $3 = (DIM_TYPE) size[0];
+ $4 = (DIM_TYPE) size[1];
+}
+%typemap(freearg)
+ (DATA_TYPE** IN_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ Py_ssize_t i;
+
+ if (array$argnum!=NULL) free(array$argnum);
+
+ /*freeing the individual arrays if needed */
+ if (object_array$argnum!=NULL)
+ {
+ if (is_new_object_array$argnum!=NULL)
+ {
+ for (i=0; i<$2; i++)
+ {
+ if (object_array$argnum[i] != NULL && is_new_object_array$argnum[i])
+ { Py_DECREF(object_array$argnum[i]); }
+ }
+ free(is_new_object_array$argnum);
+ }
+ free(object_array$argnum);
+ }
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3,
+ * DATA_TYPE* IN_ARRAY3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* IN_ARRAY3)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* IN_ARRAY3)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[3] = { -1, -1, -1 };
+ array = obj_to_array_contiguous_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 3) ||
+ !require_size(array, size, 3)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DIM_TYPE) array_size(array,2);
+ $4 = (DATA_TYPE*) array_data(array);
+}
+%typemap(freearg)
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* IN_ARRAY3)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE* IN_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* IN_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* IN_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[3] = { -1, -1, -1 };
+ array = obj_to_array_fortran_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 3) ||
+ !require_size(array, size, 3) | !require_fortran(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+ $4 = (DIM_TYPE) array_size(array,2);
+}
+%typemap(freearg)
+ (DATA_TYPE* IN_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3,
+ * DATA_TYPE* IN_FARRAY3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* IN_FARRAY3)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* IN_FARRAY3)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[3] = { -1, -1, -1 };
+ array = obj_to_array_fortran_allow_conversion($input,
+ DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 3) ||
+ !require_size(array, size, 3) || !require_fortran(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DIM_TYPE) array_size(array,2);
+ $4 = (DATA_TYPE*) array_data(array);
+}
+%typemap(freearg)
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* IN_FARRAY3)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE IN_ARRAY4[ANY][ANY][ANY][ANY])
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE IN_ARRAY4[ANY][ANY][ANY][ANY])
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE IN_ARRAY4[ANY][ANY][ANY][ANY])
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[4] = { $1_dim0, $1_dim1, $1_dim2 , $1_dim3};
+ array = obj_to_array_contiguous_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 4) ||
+ !require_size(array, size, 4)) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+%typemap(freearg)
+ (DATA_TYPE IN_ARRAY4[ANY][ANY][ANY][ANY])
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE* IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3, DIM_TYPE DIM4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[4] = { -1, -1, -1, -1 };
+ array = obj_to_array_contiguous_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 4) ||
+ !require_size(array, size, 4)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+ $4 = (DIM_TYPE) array_size(array,2);
+ $5 = (DIM_TYPE) array_size(array,3);
+}
+%typemap(freearg)
+ (DATA_TYPE* IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE** IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3, DIM_TYPE DIM4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE** IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ /* for now, only concerned with lists */
+ $1 = PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE** IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ (DATA_TYPE** array=NULL, PyArrayObject** object_array=NULL, int* is_new_object_array=NULL)
+{
+ npy_intp size[3] = { -1, -1, -1 };
+ PyArrayObject* temp_array;
+ Py_ssize_t i;
+ int is_new_object;
+
+ /* length of the list */
+ $2 = PyList_Size($input);
+
+ /* the arrays */
+ array = (DATA_TYPE **)malloc($2*sizeof(DATA_TYPE *));
+ object_array = (PyArrayObject **)calloc($2,sizeof(PyArrayObject *));
+ is_new_object_array = (int *)calloc($2,sizeof(int));
+
+ if (array == NULL || object_array == NULL || is_new_object_array == NULL)
+ {
+ SWIG_fail;
+ }
+
+ for (i=0; i<$2; i++)
+ {
+ temp_array = obj_to_array_contiguous_allow_conversion(PySequence_GetItem($input,i), DATA_TYPECODE, &is_new_object);
+
+ /* the new array must be stored so that it can be destroyed in freearg */
+ object_array[i] = temp_array;
+ is_new_object_array[i] = is_new_object;
+
+ if (!temp_array || !require_dimensions(temp_array, 3)) SWIG_fail;
+
+ /* store the size of the first array in the list, then use that for comparison. */
+ if (i == 0)
+ {
+ size[0] = array_size(temp_array,0);
+ size[1] = array_size(temp_array,1);
+ size[2] = array_size(temp_array,2);
+ }
+
+ if (!require_size(temp_array, size, 3)) SWIG_fail;
+
+ array[i] = (DATA_TYPE*) array_data(temp_array);
+ }
+
+ $1 = (DATA_TYPE**) array;
+ $3 = (DIM_TYPE) size[0];
+ $4 = (DIM_TYPE) size[1];
+ $5 = (DIM_TYPE) size[2];
+}
+%typemap(freearg)
+ (DATA_TYPE** IN_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ Py_ssize_t i;
+
+ if (array$argnum!=NULL) free(array$argnum);
+
+ /*freeing the individual arrays if needed */
+ if (object_array$argnum!=NULL)
+ {
+ if (is_new_object_array$argnum!=NULL)
+ {
+ for (i=0; i<$2; i++)
+ {
+ if (object_array$argnum[i] != NULL && is_new_object_array$argnum[i])
+ { Py_DECREF(object_array$argnum[i]); }
+ }
+ free(is_new_object_array$argnum);
+ }
+ free(object_array$argnum);
+ }
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4,
+ * DATA_TYPE* IN_ARRAY4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* IN_ARRAY4)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* IN_ARRAY4)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[4] = { -1, -1, -1 , -1};
+ array = obj_to_array_contiguous_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 4) ||
+ !require_size(array, size, 4)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DIM_TYPE) array_size(array,2);
+ $4 = (DIM_TYPE) array_size(array,3);
+ $5 = (DATA_TYPE*) array_data(array);
+}
+%typemap(freearg)
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* IN_ARRAY4)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DATA_TYPE* IN_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3, DIM_TYPE DIM4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* IN_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* IN_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[4] = { -1, -1, -1, -1 };
+ array = obj_to_array_fortran_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 4) ||
+ !require_size(array, size, 4) | !require_fortran(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+ $4 = (DIM_TYPE) array_size(array,2);
+ $5 = (DIM_TYPE) array_size(array,3);
+}
+%typemap(freearg)
+ (DATA_TYPE* IN_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4,
+ * DATA_TYPE* IN_FARRAY4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* IN_FARRAY4)
+{
+ $1 = is_array($input) || PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* IN_FARRAY4)
+ (PyArrayObject* array=NULL, int is_new_object=0)
+{
+ npy_intp size[4] = { -1, -1, -1 , -1 };
+ array = obj_to_array_fortran_allow_conversion($input, DATA_TYPECODE,
+ &is_new_object);
+ if (!array || !require_dimensions(array, 4) ||
+ !require_size(array, size, 4) || !require_fortran(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DIM_TYPE) array_size(array,2);
+ $4 = (DIM_TYPE) array_size(array,3);
+ $5 = (DATA_TYPE*) array_data(array);
+}
+%typemap(freearg)
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* IN_FARRAY4)
+{
+ if (is_new_object$argnum && array$argnum)
+ { Py_DECREF(array$argnum); }
+}
+
+/***************************/
+/* In-Place Array Typemaps */
+/***************************/
+
+/* Typemap suite for (DATA_TYPE INPLACE_ARRAY1[ANY])
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE INPLACE_ARRAY1[ANY])
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE INPLACE_ARRAY1[ANY])
+ (PyArrayObject* array=NULL)
+{
+ npy_intp size[1] = { $1_dim0 };
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,1) || !require_size(array, size, 1) ||
+ !require_contiguous(array) || !require_native(array)) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE* INPLACE_ARRAY1, DIM_TYPE DIM1)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* INPLACE_ARRAY1, DIM_TYPE DIM1)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* INPLACE_ARRAY1, DIM_TYPE DIM1)
+ (PyArrayObject* array=NULL, int i=1)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,1) || !require_contiguous(array)
+ || !require_native(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = 1;
+ for (i=0; i < array_numdims(array); ++i) $2 *= array_size(array,i);
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DATA_TYPE* INPLACE_ARRAY1)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DATA_TYPE* INPLACE_ARRAY1)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DATA_TYPE* INPLACE_ARRAY1)
+ (PyArrayObject* array=NULL, int i=0)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,1) || !require_contiguous(array)
+ || !require_native(array)) SWIG_fail;
+ $1 = 1;
+ for (i=0; i < array_numdims(array); ++i) $1 *= array_size(array,i);
+ $2 = (DATA_TYPE*) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE INPLACE_ARRAY2[ANY][ANY])
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE INPLACE_ARRAY2[ANY][ANY])
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE INPLACE_ARRAY2[ANY][ANY])
+ (PyArrayObject* array=NULL)
+{
+ npy_intp size[2] = { $1_dim0, $1_dim1 };
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,2) || !require_size(array, size, 2) ||
+ !require_contiguous(array) || !require_native(array)) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE* INPLACE_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* INPLACE_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* INPLACE_ARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,2) || !require_contiguous(array)
+ || !require_native(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* INPLACE_ARRAY2)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* INPLACE_ARRAY2)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* INPLACE_ARRAY2)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,2) || !require_contiguous(array) ||
+ !require_native(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DATA_TYPE*) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE* INPLACE_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* INPLACE_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* INPLACE_FARRAY2, DIM_TYPE DIM1, DIM_TYPE DIM2)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,2) || !require_contiguous(array)
+ || !require_native(array) || !require_fortran(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* INPLACE_FARRAY2)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* INPLACE_FARRAY2)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DATA_TYPE* INPLACE_FARRAY2)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,2) || !require_contiguous(array) ||
+ !require_native(array) || !require_fortran(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DATA_TYPE*) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE INPLACE_ARRAY3[ANY][ANY][ANY])
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE INPLACE_ARRAY3[ANY][ANY][ANY])
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE INPLACE_ARRAY3[ANY][ANY][ANY])
+ (PyArrayObject* array=NULL)
+{
+ npy_intp size[3] = { $1_dim0, $1_dim1, $1_dim2 };
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,3) || !require_size(array, size, 3) ||
+ !require_contiguous(array) || !require_native(array)) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE* INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,3) || !require_contiguous(array) ||
+ !require_native(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+ $4 = (DIM_TYPE) array_size(array,2);
+}
+
+/* Typemap suite for (DATA_TYPE** INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE** INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ $1 = PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE** INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ (DATA_TYPE** array=NULL, PyArrayObject** object_array=NULL)
+{
+ npy_intp size[2] = { -1, -1 };
+ PyArrayObject* temp_array;
+ Py_ssize_t i;
+
+ /* length of the list */
+ $2 = PyList_Size($input);
+
+ /* the arrays */
+ array = (DATA_TYPE **)malloc($2*sizeof(DATA_TYPE *));
+ object_array = (PyArrayObject **)calloc($2,sizeof(PyArrayObject *));
+
+ if (array == NULL || object_array == NULL)
+ {
+ SWIG_fail;
+ }
+
+ for (i=0; i<$2; i++)
+ {
+ temp_array = obj_to_array_no_conversion(PySequence_GetItem($input,i), DATA_TYPECODE);
+
+ /* the new array must be stored so that it can be destroyed in freearg */
+ object_array[i] = temp_array;
+
+ if ( !temp_array || !require_dimensions(temp_array, 2) ||
+ !require_contiguous(temp_array) ||
+ !require_native(temp_array) ||
+ !PyArray_EquivTypenums(array_type(temp_array), DATA_TYPECODE)
+ ) SWIG_fail;
+
+ /* store the size of the first array in the list, then use that for comparison. */
+ if (i == 0)
+ {
+ size[0] = array_size(temp_array,0);
+ size[1] = array_size(temp_array,1);
+ }
+
+ if (!require_size(temp_array, size, 2)) SWIG_fail;
+
+ array[i] = (DATA_TYPE*) array_data(temp_array);
+ }
+
+ $1 = (DATA_TYPE**) array;
+ $3 = (DIM_TYPE) size[0];
+ $4 = (DIM_TYPE) size[1];
+}
+%typemap(freearg)
+ (DATA_TYPE** INPLACE_ARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ if (array$argnum!=NULL) free(array$argnum);
+ if (object_array$argnum!=NULL) free(object_array$argnum);
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3,
+ * DATA_TYPE* INPLACE_ARRAY3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* INPLACE_ARRAY3)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* INPLACE_ARRAY3)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,3) || !require_contiguous(array)
+ || !require_native(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DIM_TYPE) array_size(array,2);
+ $4 = (DATA_TYPE*) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE* INPLACE_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* INPLACE_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* INPLACE_FARRAY3, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,3) || !require_contiguous(array) ||
+ !require_native(array) || !require_fortran(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+ $4 = (DIM_TYPE) array_size(array,2);
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3,
+ * DATA_TYPE* INPLACE_FARRAY3)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* INPLACE_FARRAY3)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DATA_TYPE* INPLACE_FARRAY3)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,3) || !require_contiguous(array)
+ || !require_native(array) || !require_fortran(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DIM_TYPE) array_size(array,2);
+ $4 = (DATA_TYPE*) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE INPLACE_ARRAY4[ANY][ANY][ANY][ANY])
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE INPLACE_ARRAY4[ANY][ANY][ANY][ANY])
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE INPLACE_ARRAY4[ANY][ANY][ANY][ANY])
+ (PyArrayObject* array=NULL)
+{
+ npy_intp size[4] = { $1_dim0, $1_dim1, $1_dim2 , $1_dim3 };
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,4) || !require_size(array, size, 4) ||
+ !require_contiguous(array) || !require_native(array)) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE* INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3, DIM_TYPE DIM4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,4) || !require_contiguous(array) ||
+ !require_native(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+ $4 = (DIM_TYPE) array_size(array,2);
+ $5 = (DIM_TYPE) array_size(array,3);
+}
+
+/* Typemap suite for (DATA_TYPE** INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3, DIM_TYPE DIM4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE** INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ $1 = PySequence_Check($input);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE** INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ (DATA_TYPE** array=NULL, PyArrayObject** object_array=NULL)
+{
+ npy_intp size[3] = { -1, -1, -1 };
+ PyArrayObject* temp_array;
+ Py_ssize_t i;
+
+ /* length of the list */
+ $2 = PyList_Size($input);
+
+ /* the arrays */
+ array = (DATA_TYPE **)malloc($2*sizeof(DATA_TYPE *));
+ object_array = (PyArrayObject **)calloc($2,sizeof(PyArrayObject *));
+
+ if (array == NULL || object_array == NULL)
+ {
+ SWIG_fail;
+ }
+
+ for (i=0; i<$2; i++)
+ {
+ temp_array = obj_to_array_no_conversion(PySequence_GetItem($input,i), DATA_TYPECODE);
+
+ /* the new array must be stored so that it can be destroyed in freearg */
+ object_array[i] = temp_array;
+
+ if ( !temp_array || !require_dimensions(temp_array, 3) ||
+ !require_contiguous(temp_array) ||
+ !require_native(temp_array) ||
+ !PyArray_EquivTypenums(array_type(temp_array), DATA_TYPECODE)
+ ) SWIG_fail;
+
+ /* store the size of the first array in the list, then use that for comparison. */
+ if (i == 0)
+ {
+ size[0] = array_size(temp_array,0);
+ size[1] = array_size(temp_array,1);
+ size[2] = array_size(temp_array,2);
+ }
+
+ if (!require_size(temp_array, size, 3)) SWIG_fail;
+
+ array[i] = (DATA_TYPE*) array_data(temp_array);
+ }
+
+ $1 = (DATA_TYPE**) array;
+ $3 = (DIM_TYPE) size[0];
+ $4 = (DIM_TYPE) size[1];
+ $5 = (DIM_TYPE) size[2];
+}
+%typemap(freearg)
+ (DATA_TYPE** INPLACE_ARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ if (array$argnum!=NULL) free(array$argnum);
+ if (object_array$argnum!=NULL) free(object_array$argnum);
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4,
+ * DATA_TYPE* INPLACE_ARRAY4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* INPLACE_ARRAY4)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* INPLACE_ARRAY4)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,4) || !require_contiguous(array)
+ || !require_native(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DIM_TYPE) array_size(array,2);
+ $4 = (DIM_TYPE) array_size(array,3);
+ $5 = (DATA_TYPE*) array_data(array);
+}
+
+/* Typemap suite for (DATA_TYPE* INPLACE_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2,
+ * DIM_TYPE DIM3, DIM_TYPE DIM4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* INPLACE_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* INPLACE_FARRAY4, DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,4) || !require_contiguous(array) ||
+ !require_native(array) || !require_fortran(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = (DIM_TYPE) array_size(array,0);
+ $3 = (DIM_TYPE) array_size(array,1);
+ $4 = (DIM_TYPE) array_size(array,2);
+ $5 = (DIM_TYPE) array_size(array,3);
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3,
+ * DATA_TYPE* INPLACE_FARRAY4)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* INPLACE_FARRAY4)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DIM_TYPE DIM2, DIM_TYPE DIM3, DIM_TYPE DIM4, DATA_TYPE* INPLACE_FARRAY4)
+ (PyArrayObject* array=NULL)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_dimensions(array,4) || !require_contiguous(array)
+ || !require_native(array) || !require_fortran(array)) SWIG_fail;
+ $1 = (DIM_TYPE) array_size(array,0);
+ $2 = (DIM_TYPE) array_size(array,1);
+ $3 = (DIM_TYPE) array_size(array,2);
+ $4 = (DIM_TYPE) array_size(array,3);
+ $5 = (DATA_TYPE*) array_data(array);
+}
+
+/*************************/
+/* Argout Array Typemaps */
+/*************************/
+
+/* Typemap suite for (DATA_TYPE ARGOUT_ARRAY1[ANY])
+ */
+%typemap(in,numinputs=0,
+ fragment="NumPy_Backward_Compatibility,NumPy_Macros")
+ (DATA_TYPE ARGOUT_ARRAY1[ANY])
+ (PyObject* array = NULL)
+{
+ npy_intp dims[1] = { $1_dim0 };
+ array = PyArray_SimpleNew(1, dims, DATA_TYPECODE);
+ if (!array) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+%typemap(argout)
+ (DATA_TYPE ARGOUT_ARRAY1[ANY])
+{
+ $result = SWIG_Python_AppendOutput($result,(PyObject*)array$argnum);
+}
+
+/* Typemap suite for (DATA_TYPE* ARGOUT_ARRAY1, DIM_TYPE DIM1)
+ */
+%typemap(in,numinputs=1,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* ARGOUT_ARRAY1, DIM_TYPE DIM1)
+ (PyObject* array = NULL)
+{
+ npy_intp dims[1];
+ if (!PyInt_Check($input))
+ {
+ const char* typestring = pytype_string($input);
+ PyErr_Format(PyExc_TypeError,
+ "Int dimension expected. '%s' given.",
+ typestring);
+ SWIG_fail;
+ }
+ $2 = (DIM_TYPE) PyInt_AsLong($input);
+ dims[0] = (npy_intp) $2;
+ array = PyArray_SimpleNew(1, dims, DATA_TYPECODE);
+ if (!array) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+}
+%typemap(argout)
+ (DATA_TYPE* ARGOUT_ARRAY1, DIM_TYPE DIM1)
+{
+ $result = SWIG_Python_AppendOutput($result,(PyObject*)array$argnum);
+}
+
+/* Typemap suite for (DIM_TYPE DIM1, DATA_TYPE* ARGOUT_ARRAY1)
+ */
+%typemap(in,numinputs=1,
+ fragment="NumPy_Fragments")
+ (DIM_TYPE DIM1, DATA_TYPE* ARGOUT_ARRAY1)
+ (PyObject* array = NULL)
+{
+ npy_intp dims[1];
+ if (!PyInt_Check($input))
+ {
+ const char* typestring = pytype_string($input);
+ PyErr_Format(PyExc_TypeError,
+ "Int dimension expected. '%s' given.",
+ typestring);
+ SWIG_fail;
+ }
+ $1 = (DIM_TYPE) PyInt_AsLong($input);
+ dims[0] = (npy_intp) $1;
+ array = PyArray_SimpleNew(1, dims, DATA_TYPECODE);
+ if (!array) SWIG_fail;
+ $2 = (DATA_TYPE*) array_data(array);
+}
+%typemap(argout)
+ (DIM_TYPE DIM1, DATA_TYPE* ARGOUT_ARRAY1)
+{
+ $result = SWIG_Python_AppendOutput($result,(PyObject*)array$argnum);
+}
+
+/* Typemap suite for (DATA_TYPE ARGOUT_ARRAY2[ANY][ANY])
+ */
+%typemap(in,numinputs=0,
+ fragment="NumPy_Backward_Compatibility,NumPy_Macros")
+ (DATA_TYPE ARGOUT_ARRAY2[ANY][ANY])
+ (PyObject* array = NULL)
+{
+ npy_intp dims[2] = { $1_dim0, $1_dim1 };
+ array = PyArray_SimpleNew(2, dims, DATA_TYPECODE);
+ if (!array) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+%typemap(argout)
+ (DATA_TYPE ARGOUT_ARRAY2[ANY][ANY])
+{
+ $result = SWIG_Python_AppendOutput($result,(PyObject*)array$argnum);
+}
+
+/* Typemap suite for (DATA_TYPE ARGOUT_ARRAY3[ANY][ANY][ANY])
+ */
+%typemap(in,numinputs=0,
+ fragment="NumPy_Backward_Compatibility,NumPy_Macros")
+ (DATA_TYPE ARGOUT_ARRAY3[ANY][ANY][ANY])
+ (PyObject* array = NULL)
+{
+ npy_intp dims[3] = { $1_dim0, $1_dim1, $1_dim2 };
+ array = PyArray_SimpleNew(3, dims, DATA_TYPECODE);
+ if (!array) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+%typemap(argout)
+ (DATA_TYPE ARGOUT_ARRAY3[ANY][ANY][ANY])
+{
+ $result = SWIG_Python_AppendOutput($result,(PyObject*)array$argnum);
+}
+
+/* Typemap suite for (DATA_TYPE ARGOUT_ARRAY4[ANY][ANY][ANY][ANY])
+ */
+%typemap(in,numinputs=0,
+ fragment="NumPy_Backward_Compatibility,NumPy_Macros")
+ (DATA_TYPE ARGOUT_ARRAY4[ANY][ANY][ANY][ANY])
+ (PyObject* array = NULL)
+{
+ npy_intp dims[4] = { $1_dim0, $1_dim1, $1_dim2, $1_dim3 };
+ array = PyArray_SimpleNew(4, dims, DATA_TYPECODE);
+ if (!array) SWIG_fail;
+ $1 = ($1_ltype) array_data(array);
+}
+%typemap(argout)
+ (DATA_TYPE ARGOUT_ARRAY4[ANY][ANY][ANY][ANY])
+{
+ $result = SWIG_Python_AppendOutput($result,(PyObject*)array$argnum);
+}
+
+/*****************************/
+/* Argoutview Array Typemaps */
+/*****************************/
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEW_ARRAY1, DIM_TYPE* DIM1)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEW_ARRAY1, DIM_TYPE* DIM1 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility")
+ (DATA_TYPE** ARGOUTVIEW_ARRAY1, DIM_TYPE* DIM1)
+{
+ npy_intp dims[1] = { *$2 };
+ PyObject* obj = PyArray_SimpleNewFromData(1, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DATA_TYPE** ARGOUTVIEW_ARRAY1)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DATA_TYPE** ARGOUTVIEW_ARRAY1)
+ (DIM_TYPE dim_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim_temp;
+ $2 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility")
+ (DIM_TYPE* DIM1, DATA_TYPE** ARGOUTVIEW_ARRAY1)
+{
+ npy_intp dims[1] = { *$1 };
+ PyObject* obj = PyArray_SimpleNewFromData(1, dims, DATA_TYPECODE, (void*)(*$2));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEW_ARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEW_ARRAY2, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility")
+ (DATA_TYPE** ARGOUTVIEW_ARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+{
+ npy_intp dims[2] = { *$2, *$3 };
+ PyObject* obj = PyArray_SimpleNewFromData(2, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEW_ARRAY2)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DATA_TYPE** ARGOUTVIEW_ARRAY2)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEW_ARRAY2)
+{
+ npy_intp dims[2] = { *$1, *$2 };
+ PyObject* obj = PyArray_SimpleNewFromData(2, dims, DATA_TYPECODE, (void*)(*$3));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEW_FARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEW_FARRAY2, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements")
+ (DATA_TYPE** ARGOUTVIEW_FARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+{
+ npy_intp dims[2] = { *$2, *$3 };
+ PyObject* obj = PyArray_SimpleNewFromData(2, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEW_FARRAY2)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DATA_TYPE** ARGOUTVIEW_FARRAY2)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEW_FARRAY2)
+{
+ npy_intp dims[2] = { *$1, *$2 };
+ PyObject* obj = PyArray_SimpleNewFromData(2, dims, DATA_TYPECODE, (void*)(*$3));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEW_ARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEW_ARRAY3, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility")
+ (DATA_TYPE** ARGOUTVIEW_ARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+{
+ npy_intp dims[3] = { *$2, *$3, *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(3, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3,
+ DATA_TYPE** ARGOUTVIEW_ARRAY3)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEW_ARRAY3)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DATA_TYPE* data_temp = NULL)
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEW_ARRAY3)
+{
+ npy_intp dims[3] = { *$1, *$2, *$3 };
+ PyObject* obj = PyArray_SimpleNewFromData(3, dims, DATA_TYPECODE, (void*)(*$4));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEW_FARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEW_FARRAY3, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements")
+ (DATA_TYPE** ARGOUTVIEW_FARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+{
+ npy_intp dims[3] = { *$2, *$3, *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(3, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3,
+ DATA_TYPE** ARGOUTVIEW_FARRAY3)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DATA_TYPE** ARGOUTVIEW_FARRAY3)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEW_FARRAY3)
+{
+ npy_intp dims[3] = { *$1, *$2, *$3 };
+ PyObject* obj = PyArray_SimpleNewFromData(3, dims, DATA_TYPECODE, (void*)(*$4));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEW_ARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEW_ARRAY4, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+ $5 = &dim4_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility")
+ (DATA_TYPE** ARGOUTVIEW_ARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+{
+ npy_intp dims[4] = { *$2, *$3, *$4 , *$5 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4,
+ DATA_TYPE** ARGOUTVIEW_ARRAY4)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 , DATA_TYPE** ARGOUTVIEW_ARRAY4)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &dim4_temp;
+ $5 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEW_ARRAY4)
+{
+ npy_intp dims[4] = { *$1, *$2, *$3 , *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$5));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEW_FARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEW_FARRAY4, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+ $5 = &dim4_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements")
+ (DATA_TYPE** ARGOUTVIEW_FARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+{
+ npy_intp dims[4] = { *$2, *$3, *$4 , *$5 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4,
+ DATA_TYPE** ARGOUTVIEW_FARRAY4)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 , DATA_TYPE** ARGOUTVIEW_FARRAY4)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &dim4_temp;
+ $5 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEW_FARRAY4)
+{
+ npy_intp dims[4] = { *$1, *$2, *$3 , *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$5));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/*************************************/
+/* Managed Argoutview Array Typemaps */
+/*************************************/
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_ARRAY1, DIM_TYPE* DIM1)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY1, DIM_TYPE* DIM1 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY1, DIM_TYPE* DIM1)
+{
+ npy_intp dims[1] = { *$2 };
+ PyObject* obj = PyArray_SimpleNewFromData(1, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DATA_TYPE** ARGOUTVIEWM_ARRAY1)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DATA_TYPE** ARGOUTVIEWM_ARRAY1)
+ (DIM_TYPE dim_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim_temp;
+ $2 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DATA_TYPE** ARGOUTVIEWM_ARRAY1)
+{
+ npy_intp dims[1] = { *$1 };
+ PyObject* obj = PyArray_SimpleNewFromData(1, dims, DATA_TYPECODE, (void*)(*$2));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_ARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY2, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+{
+ npy_intp dims[2] = { *$2, *$3 };
+ PyObject* obj = PyArray_SimpleNewFromData(2, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEWM_ARRAY2)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DATA_TYPE** ARGOUTVIEWM_ARRAY2)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEWM_ARRAY2)
+{
+ npy_intp dims[2] = { *$1, *$2 };
+ PyObject* obj = PyArray_SimpleNewFromData(2, dims, DATA_TYPECODE, (void*)(*$3));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_FARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_FARRAY2, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_FARRAY2, DIM_TYPE* DIM1, DIM_TYPE* DIM2)
+{
+ npy_intp dims[2] = { *$2, *$3 };
+ PyObject* obj = PyArray_SimpleNewFromData(2, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEWM_FARRAY2)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DATA_TYPE** ARGOUTVIEWM_FARRAY2)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DATA_TYPE** ARGOUTVIEWM_FARRAY2)
+{
+ npy_intp dims[2] = { *$1, *$2 };
+ PyObject* obj = PyArray_SimpleNewFromData(2, dims, DATA_TYPECODE, (void*)(*$3));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_ARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY3, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+{
+ npy_intp dims[3] = { *$2, *$3, *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(3, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3,
+ DATA_TYPE** ARGOUTVIEWM_ARRAY3)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DATA_TYPE** ARGOUTVIEWM_ARRAY3)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEWM_ARRAY3)
+{
+ npy_intp dims[3] = { *$1, *$2, *$3 };
+ PyObject* obj= PyArray_SimpleNewFromData(3, dims, DATA_TYPECODE, (void*)(*$4));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_FARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_FARRAY3, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_FARRAY3, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+{
+ npy_intp dims[3] = { *$2, *$3, *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(3, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3,
+ DATA_TYPE** ARGOUTVIEWM_FARRAY3)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DATA_TYPE** ARGOUTVIEWM_FARRAY3)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DATA_TYPE** ARGOUTVIEWM_FARRAY3)
+{
+ npy_intp dims[3] = { *$1, *$2, *$3 };
+ PyObject* obj = PyArray_SimpleNewFromData(3, dims, DATA_TYPECODE, (void*)(*$4));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_ARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY4, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+ $5 = &dim4_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+{
+ npy_intp dims[4] = { *$2, *$3, *$4 , *$5 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4,
+ DATA_TYPE** ARGOUTVIEWM_ARRAY4)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 , DATA_TYPE** ARGOUTVIEWM_ARRAY4)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &dim4_temp;
+ $5 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEWM_ARRAY4)
+{
+ npy_intp dims[4] = { *$1, *$2, *$3 , *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$5));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_FARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_FARRAY4, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+ $5 = &dim4_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_FARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3)
+{
+ npy_intp dims[4] = { *$2, *$3, *$4 , *$5 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4,
+ DATA_TYPE** ARGOUTVIEWM_FARRAY4)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 , DATA_TYPE** ARGOUTVIEWM_FARRAY4)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &dim4_temp;
+ $5 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEWM_FARRAY4)
+{
+ npy_intp dims[4] = { *$1, *$2, *$3 , *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$5));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_ARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY4, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+ $5 = &dim4_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_ARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+{
+ npy_intp dims[4] = { *$2, *$3, *$4 , *$5 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4,
+ DATA_TYPE** ARGOUTVIEWM_ARRAY4)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 , DATA_TYPE** ARGOUTVIEWM_ARRAY4)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &dim4_temp;
+ $5 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEWM_ARRAY4)
+{
+ npy_intp dims[4] = { *$1, *$2, *$3 , *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$5));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DATA_TYPE** ARGOUTVIEWM_FARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2,
+ DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+ */
+%typemap(in,numinputs=0)
+ (DATA_TYPE** ARGOUTVIEWM_FARRAY4, DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 )
+ (DATA_TYPE* data_temp = NULL , DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp)
+{
+ $1 = &data_temp;
+ $2 = &dim1_temp;
+ $3 = &dim2_temp;
+ $4 = &dim3_temp;
+ $5 = &dim4_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements,NumPy_Utilities")
+ (DATA_TYPE** ARGOUTVIEWM_FARRAY4, DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4)
+{
+ npy_intp dims[4] = { *$2, *$3, *$4 , *$5 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$1));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/* Typemap suite for (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4,
+ DATA_TYPE** ARGOUTVIEWM_FARRAY4)
+ */
+%typemap(in,numinputs=0)
+ (DIM_TYPE* DIM1 , DIM_TYPE* DIM2 , DIM_TYPE* DIM3 , DIM_TYPE* DIM4 , DATA_TYPE** ARGOUTVIEWM_FARRAY4)
+ (DIM_TYPE dim1_temp, DIM_TYPE dim2_temp, DIM_TYPE dim3_temp, DIM_TYPE dim4_temp, DATA_TYPE* data_temp = NULL )
+{
+ $1 = &dim1_temp;
+ $2 = &dim2_temp;
+ $3 = &dim3_temp;
+ $4 = &dim4_temp;
+ $5 = &data_temp;
+}
+%typemap(argout,
+ fragment="NumPy_Backward_Compatibility,NumPy_Array_Requirements,NumPy_Utilities")
+ (DIM_TYPE* DIM1, DIM_TYPE* DIM2, DIM_TYPE* DIM3, DIM_TYPE* DIM4, DATA_TYPE** ARGOUTVIEWM_FARRAY4)
+{
+ npy_intp dims[4] = { *$1, *$2, *$3 , *$4 };
+ PyObject* obj = PyArray_SimpleNewFromData(4, dims, DATA_TYPECODE, (void*)(*$5));
+ PyArrayObject* array = (PyArrayObject*) obj;
+
+ if (!array || !require_fortran(array)) SWIG_fail;
+
+%#ifdef SWIGPY_USE_CAPSULE
+ PyObject* cap = PyCapsule_New((void*)(*$1), SWIGPY_CAPSULE_NAME, free_cap);
+%#else
+ PyObject* cap = PyCObject_FromVoidPtr((void*)(*$1), free);
+%#endif
+
+%#if NPY_API_VERSION < 0x00000007
+ PyArray_BASE(array) = cap;
+%#else
+ PyArray_SetBaseObject(array,cap);
+%#endif
+
+ $result = SWIG_Python_AppendOutput($result,obj);
+}
+
+/**************************************/
+/* In-Place Array Typemap - flattened */
+/**************************************/
+
+/* Typemap suite for (DATA_TYPE* INPLACE_ARRAY_FLAT, DIM_TYPE DIM_FLAT)
+ */
+%typecheck(SWIG_TYPECHECK_DOUBLE_ARRAY,
+ fragment="NumPy_Macros")
+ (DATA_TYPE* INPLACE_ARRAY_FLAT, DIM_TYPE DIM_FLAT)
+{
+ $1 = is_array($input) && PyArray_EquivTypenums(array_type($input),
+ DATA_TYPECODE);
+}
+%typemap(in,
+ fragment="NumPy_Fragments")
+ (DATA_TYPE* INPLACE_ARRAY_FLAT, DIM_TYPE DIM_FLAT)
+ (PyArrayObject* array=NULL, int i=1)
+{
+ array = obj_to_array_no_conversion($input, DATA_TYPECODE);
+ if (!array || !require_c_or_f_contiguous(array)
+ || !require_native(array)) SWIG_fail;
+ $1 = (DATA_TYPE*) array_data(array);
+ $2 = 1;
+ for (i=0; i < array_numdims(array); ++i) $2 *= array_size(array,i);
+}
+
+%enddef /* %numpy_typemaps() macro */
+/* *************************************************************** */
+
+/* Concrete instances of the %numpy_typemaps() macro: Each invocation
+ * below applies all of the typemaps above to the specified data type.
+ */
+%numpy_typemaps(signed char , NPY_BYTE , int)
+%numpy_typemaps(unsigned char , NPY_UBYTE , int)
+%numpy_typemaps(short , NPY_SHORT , int)
+%numpy_typemaps(unsigned short , NPY_USHORT , int)
+%numpy_typemaps(int , NPY_INT , int)
+%numpy_typemaps(unsigned int , NPY_UINT , int)
+%numpy_typemaps(long , NPY_LONG , int)
+%numpy_typemaps(unsigned long , NPY_ULONG , int)
+%numpy_typemaps(long long , NPY_LONGLONG , int)
+%numpy_typemaps(unsigned long long, NPY_ULONGLONG, int)
+%numpy_typemaps(float , NPY_FLOAT , int)
+%numpy_typemaps(double , NPY_DOUBLE , int)
+%numpy_typemaps(int8_t , NPY_INT8 , int)
+%numpy_typemaps(int16_t , NPY_INT16 , int)
+%numpy_typemaps(int32_t , NPY_INT32 , int)
+%numpy_typemaps(int64_t , NPY_INT64 , int)
+%numpy_typemaps(uint8_t , NPY_UINT8 , int)
+%numpy_typemaps(uint16_t , NPY_UINT16 , int)
+%numpy_typemaps(uint32_t , NPY_UINT32 , int)
+%numpy_typemaps(uint64_t , NPY_UINT64 , int)
+
+
+/* ***************************************************************
+ * The follow macro expansion does not work, because C++ bool is 4
+ * bytes and NPY_BOOL is 1 byte
+ *
+ * %numpy_typemaps(bool, NPY_BOOL, int)
+ */
+
+/* ***************************************************************
+ * On my Mac, I get the following warning for this macro expansion:
+ * 'swig/python detected a memory leak of type 'long double *', no destructor found.'
+ *
+ * %numpy_typemaps(long double, NPY_LONGDOUBLE, int)
+ */
+
+#ifdef __cplusplus
+
+%include <std_complex.i>
+
+%numpy_typemaps(std::complex<float>, NPY_CFLOAT , int)
+%numpy_typemaps(std::complex<double>, NPY_CDOUBLE, int)
+
+#endif
+
+#endif /* SWIGPYTHON */
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/setup.py b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..69146a53eaf4aefdc86a7ade8ea2d07fb5592053
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/cfsim/setup.py
@@ -0,0 +1,35 @@
+"""Compiles the cffirmware C extension."""
+
+from distutils.core import setup, Extension
+import os
+
+import numpy as np
+
+
+fw_dir = "../../../../../../crazyflie-firmware"
+include = [
+ os.path.join(fw_dir, "src/modules/interface"),
+ os.path.join(fw_dir, "src/hal/interface"),
+ os.path.join(fw_dir, "src/utils/interface/lighthouse"),
+ np.get_include(),
+]
+
+modules = [
+ "collision_avoidance.c",
+ "planner.c",
+ "pptraj.c",
+ "pptraj_compressed.c",
+]
+fw_sources = [os.path.join(fw_dir, "src/modules/src", mod) for mod in modules]
+
+cffirmware = Extension(
+ "_cffirmware",
+ include_dirs=include,
+ sources=fw_sources + ["cffirmware_wrap.c"],
+ extra_compile_args=[
+ "-O3",
+ "-D__fp16=uint16_t",
+ ],
+)
+
+setup(name="cffirmware", version="1.0", ext_modules=[cffirmware])
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflie.py b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflie.py
new file mode 100644
index 0000000000000000000000000000000000000000..d065f52de85d1b69370513038968ca87bdb5c36a
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflie.py
@@ -0,0 +1,695 @@
+#!/usr/bin/env python
+
+
+import sys
+import yaml
+import rospy
+import numpy as np
+import time
+import tf_conversions
+from std_srvs.srv import Empty
+import std_msgs
+from crazyswarm.srv import *
+from crazyswarm.msg import TrajectoryPolynomialPiece, FullState, Position, VelocityWorld
+from tf import TransformListener
+from .visualizer import visNull
+
+def arrayToGeometryPoint(a):
+ return geometry_msgs.msg.Point(a[0], a[1], a[2])
+
+class TimeHelper:
+ """Object containing all time-related functionality.
+
+ This class mainly exists to support both real hardware and (potentially
+ faster or slower than realtime) simulation with the same script.
+ When running on real hardware, this class uses ROS time functions.
+ The simulation equivalent does not depend on ROS.
+
+ Attributes:
+ visualizer: No-op object conforming to the Visualizer API used in
+ simulation scripts. Maintains the property that scripts should not
+ know/care if they are running in simulation or not.
+ """
+ def __init__(self):
+ self.rosRate = None
+ self.rateHz = None
+ self.visualizer = visNull.VisNull()
+
+ def time(self):
+ """Returns the current time in seconds."""
+ return rospy.Time.now().to_sec()
+
+ def sleep(self, duration):
+ """Sleeps for the provided duration in seconds."""
+ rospy.sleep(duration)
+
+ def sleepForRate(self, rateHz):
+ """Sleeps so that, if called in a loop, executes at specified rate."""
+ if self.rosRate is None or self.rateHz != rateHz:
+ self.rosRate = rospy.Rate(rateHz)
+ self.rateHz = rateHz
+ self.rosRate.sleep()
+
+ def isShutdown(self):
+ """Returns true if the script should abort, e.g. from Ctrl-C."""
+ return rospy.is_shutdown()
+
+
+class Crazyflie:
+ """Object representing a single robot.
+
+ The bulk of the module's functionality is contained in this class.
+ """
+
+ def __init__(self, id, initialPosition, tf):
+ """Constructor.
+
+ Args:
+ id (int): Integer ID in range [0, 255]. The last byte of the robot's
+ radio address.
+ initialPosition (iterable of float): Initial position of the robot:
+ [x, y, z]. Typically on the floor of the experiment space with
+ z == 0.0.
+ tf (tf.TransformListener): ROS TransformListener used to query the
+ robot's current state.
+ """
+ self.id = id
+ prefix = "/cf" + str(id)
+ self.prefix = prefix
+ self.initialPosition = np.array(initialPosition)
+
+ self.tf = tf
+
+ rospy.wait_for_service(prefix + "/set_group_mask")
+ self.setGroupMaskService = rospy.ServiceProxy(prefix + "/set_group_mask", SetGroupMask)
+ rospy.wait_for_service(prefix + "/takeoff")
+ self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff)
+ rospy.wait_for_service(prefix + "/land")
+ self.landService = rospy.ServiceProxy(prefix + "/land", Land)
+ # rospy.wait_for_service(prefix + "/stop")
+ # self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop)
+ rospy.wait_for_service(prefix + "/go_to")
+ self.goToService = rospy.ServiceProxy(prefix + "/go_to", GoTo)
+ rospy.wait_for_service(prefix + "/upload_trajectory")
+ self.uploadTrajectoryService = rospy.ServiceProxy(prefix + "/upload_trajectory", UploadTrajectory)
+ rospy.wait_for_service(prefix + "/start_trajectory")
+ self.startTrajectoryService = rospy.ServiceProxy(prefix + "/start_trajectory", StartTrajectory)
+ rospy.wait_for_service(prefix + "/notify_setpoints_stop")
+ self.notifySetpointsStopService = rospy.ServiceProxy(prefix + "/notify_setpoints_stop", NotifySetpointsStop)
+ rospy.wait_for_service(prefix + "/update_params")
+ self.updateParamsService = rospy.ServiceProxy(prefix + "/update_params", UpdateParams)
+
+ self.cmdFullStatePublisher = rospy.Publisher(prefix + "/cmd_full_state", FullState, queue_size=1)
+ self.cmdFullStateMsg = FullState()
+ self.cmdFullStateMsg.header.seq = 0
+ self.cmdFullStateMsg.header.frame_id = "/world"
+
+ self.cmdStopPublisher = rospy.Publisher(prefix + "/cmd_stop", std_msgs.msg.Empty, queue_size=1)
+
+ self.cmdVelPublisher = rospy.Publisher(prefix + "/cmd_vel", geometry_msgs.msg.Twist, queue_size=1)
+
+ self.cmdPositionPublisher = rospy.Publisher(prefix + "/cmd_position", Position, queue_size=1)
+ self.cmdPositionMsg = Position()
+ self.cmdPositionMsg.header.seq = 0
+ self.cmdPositionMsg.header.frame_id = "/world"
+
+ self.cmdVelocityWorldPublisher = rospy.Publisher(prefix + "/cmd_velocity_world", VelocityWorld, queue_size=1)
+ self.cmdVelocityWorldMsg = VelocityWorld()
+ self.cmdVelocityWorldMsg.header.seq = 0
+ self.cmdVelocityWorldMsg.header.frame_id = "/world"
+
+ def setGroupMask(self, groupMask):
+ """Sets the group mask bits for this robot.
+
+ The purpose of groups is to make it possible to trigger an action
+ (for example, executing a previously-uploaded trajectory) on a subset
+ of all robots without needing to send more than one radio packet.
+ This is important to achieve tight, synchronized "choreography".
+
+ Up to 8 groups may exist, corresponding to bits in the groupMask byte.
+ When a broadcast command is triggered on the :obj:`CrazyflieServer` object
+ with a groupMask argument, the command only affects those robots whose
+ groupMask has a nonzero bitwise-AND with the command's groupMask.
+ A command with a groupMask of zero applies to all robots regardless of
+ group membership.
+
+ Some individual robot (not broadcast) commands also support groupMask,
+ but it is not especially useful in that case.
+
+ Args:
+ groupMask (int): An 8-bit integer representing this robot's
+ membership status in each of the <= 8 possible groups.
+ """
+ self.setGroupMaskService(groupMask)
+
+ def enableCollisionAvoidance(self, others, ellipsoidRadii):
+ """Enables onboard collision avoidance.
+
+ When enabled, avoids colliding with other Crazyflies by using the
+ Buffered Voronoi Cells method [1]. Computation is performed onboard.
+
+ Args:
+ others (List[Crazyflie]): List of other :obj:`Crazyflie` objects.
+ In simulation, collision avoidance is checked only with members
+ of this list. With real hardware, this list is **ignored**, and
+ collision avoidance is checked with all other Crazyflies on the
+ same radio channel.
+ ellipsoidRadii (array-like of float[3]): Radii of collision volume ellipsoid in meters.
+ The Crazyflie's boundary for collision checking is a tall
+ ellipsoid. This accounts for the downwash effect: Due to the
+ fast-moving stream of air produced by the rotors, the safe
+ distance to pass underneath another rotorcraft is much further
+ than the safe distance to pass to the side.
+
+ [1] D. Zhou, Wang, Z., Bandyopadhyay, S., and Schwager, M.
+ Fast, On-line Collision Avoidance for Dynamic Vehicles using
+ Buffered Voronoi Cells. IEEE Robotics and Automation Letters
+ (RA-L), vol. 2, no. 2, pp. 1047 - 1054, 2017.
+ https://msl.stanford.edu/fast-line-collision-avoidance-dynamic-vehicles-using-buffered-voronoi-cells
+ """
+ # Set radii before enabling to ensure collision avoidance never
+ # observes a wrong radius value.
+ self.setParams({
+ "colAv/ellipsoidX": float(ellipsoidRadii[0]),
+ "colAv/ellipsoidY": float(ellipsoidRadii[1]),
+ "colAv/ellipsoidZ": float(ellipsoidRadii[2]),
+ })
+ self.setParam("colAv/enable", 1)
+
+
+ def disableCollisionAvoidance(self):
+ """Disables onboard collision avoidance."""
+ self.setParam("colAv/enable", 0)
+
+ def takeoff(self, targetHeight, duration, groupMask = 0):
+ """Execute a takeoff - fly straight up, then hover indefinitely.
+
+ Asynchronous command; returns immediately.
+
+ Args:
+ targetHeight (float): The z-coordinate at which to hover.
+ duration (float): How long until the height is reached. Seconds.
+ groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc.
+ """
+ self.takeoffService(groupMask, targetHeight, rospy.Duration.from_sec(duration))
+
+ def land(self, targetHeight, duration, groupMask = 0):
+ """Execute a landing - fly straight down. User must cut power after.
+
+ Asynchronous command; returns immediately.
+
+ Args:
+ targetHeight (float): The z-coordinate at which to land. Meters.
+ Usually should be a few centimeters above the initial position
+ to ensure that the controller does not try to penetrate the
+ floor if the mocap coordinate origin is not perfect.
+ duration (float): How long until the height is reached. Seconds.
+ groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc.
+ """
+ self.landService(groupMask, targetHeight, rospy.Duration.from_sec(duration))
+
+ def stop(self, groupMask = 0):
+ """Cuts power to the motors when operating in low-level command mode.
+
+ Intended for non-emergency scenarios, e.g. landing with the possibility
+ of taking off again later. Future low- or high-level commands will
+ restart the motors.
+
+ Args:
+ groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc.
+ """
+ self.stopService(groupMask)
+
+ def goTo(self, goal, yaw, duration, relative = False, groupMask = 0):
+ """Move smoothly to the goal, then hover indefinitely.
+
+ Asynchronous command; returns immediately.
+
+ Plans a smooth trajectory from the current state to the goal position.
+ Will stop smoothly at the goal with minimal overshoot. If the current
+ state is at hover, the planned trajectory will be a straight line;
+ however, if the current velocity is nonzero, the planned trajectory
+ will be a smooth curve.
+
+ Plans the trajectory by solving for the unique degree-7 polynomial that
+ satisfies the initial conditions of the current position, velocity,
+ and acceleration, and ends at the goal position with zero velocity and
+ acceleration. The jerk (derivative of acceleration) is fixed at zero at
+ both boundary conditions.
+
+ .. warning::
+ Calling ``goTo`` rapidly and/or with short durations (<< 1 sec) can
+ cause instability. Consider using :meth:`cmdPosition()` instead.
+
+ Args:
+ goal (iterable of 3 floats): The goal position. Meters.
+ yaw (float): The goal yaw angle (heading). Radians.
+ duration (float): How long until the goal is reached. Seconds.
+ relative (bool): If true, the goal position is interpreted as a
+ relative offset from the current position. Otherwise, the goal
+ position is interpreted as absolute coordintates in the global
+ reference frame.
+ groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc.
+ """
+ gp = arrayToGeometryPoint(goal)
+ self.goToService(groupMask, relative, gp, yaw, rospy.Duration.from_sec(duration))
+
+ def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
+ """Uploads a piecewise polynomial trajectory for later execution.
+
+ See uav_trajectory.py for more information about piecewise polynomial
+ trajectories.
+
+ Args:
+ trajectoryId (int): ID number of this trajectory. Multiple
+ trajectories can be uploaded. TODO: what is the maximum ID?
+ pieceOffset (int): TODO(whoenig): explain this.
+ trajectory (:obj:`pycrazyswarm.uav_trajectory.Trajectory`): Trajectory object.
+ """
+ pieces = []
+ for poly in trajectory.polynomials:
+ piece = TrajectoryPolynomialPiece()
+ piece.duration = rospy.Duration.from_sec(poly.duration)
+ piece.poly_x = poly.px.p
+ piece.poly_y = poly.py.p
+ piece.poly_z = poly.pz.p
+ piece.poly_yaw = poly.pyaw.p
+ pieces.append(piece)
+ self.uploadTrajectoryService(trajectoryId, pieceOffset, pieces)
+
+ def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
+ """Begins executing a previously uploaded trajectory.
+
+ Asynchronous command; returns immediately.
+
+ Args:
+ trajectoryId (int): ID number as given to :meth:`uploadTrajectory()`.
+ timescale (float): Scales the trajectory duration by this factor.
+ For example if timescale == 2.0, the trajectory will take twice
+ as long to execute as the nominal duration.
+ reverse (bool): If true, executes the trajectory backwards in time.
+ relative (bool): If true (default), the position of the trajectory
+ is shifted such that it begins at the current position setpoint.
+ This is usually the desired behavior.
+ groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc.
+ """
+ self.startTrajectoryService(groupMask, trajectoryId, timescale, reverse, relative)
+
+ def notifySetpointsStop(self, remainValidMillisecs=100, groupMask=0):
+ """Informs that streaming low-level setpoint packets are about to stop.
+
+ Streaming setpoints are :meth:`cmdVelocityWorld`, :meth:`cmdFullState`,
+ and so on. For safety purposes, they normally preempt onboard high-level
+ commands such as :meth:`goTo`.
+
+ Once preempted, the Crazyflie will not switch back to high-level
+ commands (or other behaviors determined by onboard planning/logic) until
+ a significant amount of time has elapsed where no low-level setpoint
+ was received.
+
+ This command short-circuits that waiting period to a user-chosen time.
+ It should be called after sending the last low-level setpoint, and
+ before sending any high-level command.
+
+ A common use case is to execute the :meth:`land` command after using
+ streaming setpoint modes.
+
+ Args:
+ remainValidMillisecs (int): Number of milliseconds that the last
+ streaming setpoint should be followed before reverting to the
+ onboard-determined behavior. May be longer e.g. if one radio
+ is controlling many robots.
+ """
+ self.notifySetpointsStopService(groupMask, remainValidMillisecs)
+
+ def position(self):
+ """Returns the last true position measurement from motion capture.
+
+ If at least one position measurement for this robot has been received
+ from the motion capture system since startup, this function returns
+ immediately with the most recent measurement. However, if **no**
+ position measurements have been received, it blocks until the first
+ one arrives.
+
+ Returns:
+ position (np.array[3]): Current position. Meters.
+ """
+ self.tf.waitForTransform("/world", "/cf" + str(self.id), rospy.Time(0), rospy.Duration(10))
+ position, quaternion = self.tf.lookupTransform("/world", "/cf" + str(self.id), rospy.Time(0))
+ return np.array(position)
+
+ def getParam(self, name):
+ """Returns the current value of the onboard named parameter.
+
+ Parameters are named values of various primitive C types that control
+ the firmware's behavior. For more information, see
+ https://www.bitcraze.io/documentation/repository/crazyflie-firmware/master/userguides/logparam/.
+
+ Parameters are read at system startup over the radio and cached.
+ The ROS launch file can also be used to set parameter values at startup.
+ Subsequent calls to :meth:`setParam()` will update the cached value.
+ However, if the parameter changes for any other reason, the cached value
+ might become stale. This situation is not common.
+
+ Args:
+ name (str): The parameter's name.
+
+ Returns:
+ value (Any): The parameter's value.
+ """
+ return rospy.get_param(self.prefix + "/" + name)
+
+ def setParam(self, name, value):
+ """Changes the value of the given parameter.
+
+ See :meth:`getParam()` docs for overview of the parameter system.
+
+ Args:
+ name (str): The parameter's name.
+ value (Any): The parameter's value.
+ """
+ rospy.set_param(self.prefix + "/" + name, value)
+ self.updateParamsService([name])
+
+ def setParams(self, params):
+ """Changes the value of several parameters at once.
+
+ See :meth:`getParam()` docs for overview of the parameter system.
+
+ Args:
+ params (Dict[str, Any]): Dict of parameter names/values.
+ """
+ for name, value in params.items():
+ rospy.set_param(self.prefix + "/" + name, value)
+ self.updateParamsService(params.keys())
+
+ def cmdFullState(self, pos, vel, acc, yaw, omega):
+ """Sends a streaming full-state controller setpoint command.
+
+ The full-state setpoint is most useful for aggressive maneuvers where
+ feedforward inputs for acceleration and angular velocity are critical
+ to obtaining good tracking performance. Full-state setpoints can be
+ obtained from any trajectory parameterization that is at least three
+ times differentiable, e.g. piecewise polynomials.
+
+ Sending a streaming setpoint of any type will force a change from
+ high-level to low-level command mode. Currently, there is no mechanism
+ to change back, but it is a high-priority feature to implement.
+ This means it is not possible to use e.g. :meth:`land()` or
+ :meth:`goTo()` after a streaming setpoint has been sent.
+
+ Args:
+ pos (array-like of float[3]): Position. Meters.
+ vel (array-like of float[3]): Velocity. Meters / second.
+ acc (array-like of float[3]): Acceleration. Meters / second^2.
+ yaw (float): Yaw angle. Radians.
+ omega (array-like of float[3]): Angular velocity in body frame.
+ Radians / sec.
+ """
+ self.cmdFullStateMsg.header.stamp = rospy.Time.now()
+ self.cmdFullStateMsg.header.seq += 1
+ self.cmdFullStateMsg.pose.position.x = pos[0]
+ self.cmdFullStateMsg.pose.position.y = pos[1]
+ self.cmdFullStateMsg.pose.position.z = pos[2]
+ self.cmdFullStateMsg.twist.linear.x = vel[0]
+ self.cmdFullStateMsg.twist.linear.y = vel[1]
+ self.cmdFullStateMsg.twist.linear.z = vel[2]
+ self.cmdFullStateMsg.acc.x = acc[0]
+ self.cmdFullStateMsg.acc.y = acc[1]
+ self.cmdFullStateMsg.acc.z = acc[2]
+ self.cmdFullStateMsg.pose.orientation = geometry_msgs.msg.Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 0, yaw))
+ self.cmdFullStateMsg.twist.angular.x = omega[0]
+ self.cmdFullStateMsg.twist.angular.y = omega[1]
+ self.cmdFullStateMsg.twist.angular.z = omega[2]
+ self.cmdFullStatePublisher.publish(self.cmdFullStateMsg)
+
+ def cmdVelocityWorld(self, vel, yawRate):
+ """Sends a streaming velocity-world controller setpoint command.
+
+ In this mode, the PC specifies desired velocity vector and yaw rate.
+ The onboard controller will try to achive this velocity.
+
+ NOTE: the Mellinger controller is Crazyswarm's default controller, but
+ it has not been tuned (or even tested) for velocity control mode.
+ Switch to the PID controller by changing
+ `firmwareParams.stabilizer.controller` to `1` in your launch file.
+
+ Sending a streaming setpoint of any type will force a change from
+ high-level to low-level command mode. Currently, there is no mechanism
+ to change back, but it is a high-priority feature to implement.
+ This means it is not possible to use e.g. :meth:`land()` or
+ :meth:`goTo()` after a streaming setpoint has been sent.
+
+ Args:
+ vel (array-like of float[3]): Velocity. Meters / second.
+ yawRate (float): Yaw angular velocity. Degrees / second.
+ """
+ self.cmdVelocityWorldMsg.header.stamp = rospy.Time.now()
+ self.cmdVelocityWorldMsg.header.seq += 1
+ self.cmdVelocityWorldMsg.vel.x = vel[0]
+ self.cmdVelocityWorldMsg.vel.y = vel[1]
+ self.cmdVelocityWorldMsg.vel.z = vel[2]
+ self.cmdVelocityWorldMsg.yawRate = yawRate
+ self.cmdVelocityWorldPublisher.publish(self.cmdVelocityWorldMsg)
+
+ def cmdStop(self):
+ """Interrupts any high-level command to stop and cut motor power.
+
+ Intended for non-emergency scenarios, e.g. landing with the possibility
+ of taking off again later. Future low- or high-level commands will
+ restart the motors. Equivalent of :meth:`stop()` when in high-level mode.
+ """
+ self.cmdStopPublisher.publish(std_msgs.msg.Empty())
+
+ def cmdVel(self, roll, pitch, yawrate, thrust):
+ """Sends a streaming command of the "easy mode" manual control inputs.
+
+ The (absolute roll & pitch, yaw rate, thrust) inputs are typically
+ used for manual flying by beginners or causal pilots, as opposed to the
+ "acrobatic mode" inputs where roll and pitch rates are controlled
+ instead of absolute angles. This mode limits the possible maneuvers,
+ e.g. it is not possible to do a flip because the controller joystick
+ would need to rotate 360 degrees.
+
+ For more information on streaming setpoint commands, see the
+ :meth:`cmdFullState()` documentation.
+
+ !NOTE!: The angles and angular velocities in this command are in
+ degrees, whereas they are in radians for cmdFullState.
+
+ TODO: should we change the name from cmdVel to something else?
+ IMO (japreiss), cmdVel implies controlling linear velocity.
+
+ Args:
+ roll (float): Roll angle. Degrees. Positive values == roll right.
+ pitch (float): Pitch angle. Degrees. Positive values == pitch
+ forward/down.
+ yawrate (float): Yaw angular velocity. Degrees / second. Positive
+ values == turn counterclockwise.
+ thrust (float): Thrust magnitude. Non-meaningful units in [0, 2^16),
+ where the maximum value corresponds to maximum thrust.
+ """
+ msg = geometry_msgs.msg.Twist()
+ msg.linear.x = pitch
+ msg.linear.y = roll
+ msg.angular.z = yawrate
+ msg.linear.z = thrust
+ self.cmdVelPublisher.publish(msg)
+
+ def cmdPosition(self, pos, yaw = 0):
+ """Sends a streaming command of absolute position and yaw setpoint.
+
+ Useful for slow maneuvers where a high-level planner determines the
+ desired position, and the rest is left to the onboard controller.
+
+ For more information on streaming setpoint commands, see the
+ :meth:`cmdFullState()` documentation.
+
+ .. warning::
+ As a streaming setpoint, ``cmdPosition`` must be sent many times
+ per second (10Hz is a conservative minimum). For applications that
+ generate goal positions slowly, :meth:`goTo()` may be more
+ appropriate, especially if the goal positions are far apart.
+
+ Args:
+ pos (array-like of float[3]): Position. Meters.
+ yaw (float): Yaw angle. Radians.
+ """
+ self.cmdPositionMsg.header.stamp = rospy.Time.now()
+ self.cmdPositionMsg.header.seq += 1
+ self.cmdPositionMsg.x = pos[0]
+ self.cmdPositionMsg.y = pos[1]
+ self.cmdPositionMsg.z = pos[2]
+ self.cmdPositionMsg.yaw = yaw
+ self.cmdPositionPublisher.publish(self.cmdPositionMsg)
+
+ def setLEDColor(self, r, g, b):
+ """Sets the color of the LED ring deck.
+
+ While most params (such as PID gains) only need to be set once, it is
+ common to change the LED ring color many times during a flight, e.g.
+ as some kind of status indicator. This method makes it convenient.
+
+ PRECONDITION: The param "ring/effect" must be set to 7 (solid color)
+ for this command to have any effect. The default mode uses the ring
+ color to indicate radio connection quality.
+
+ This is a blocking command, so it may cause stability problems for
+ large swarms and/or high-frequency changes.
+
+ Args:
+ r (float): Red component of color, in range [0, 1].
+ g (float): Green component of color, in range [0, 1].
+ b (float): Blue component of color, in range [0, 1].
+ """
+ self.setParam("ring/solidRed", int(r * 255))
+ self.setParam("ring/solidGreen", int(g * 255))
+ self.setParam("ring/solidBlue", int(b * 255))
+
+
+class CrazyflieServer:
+ """Object for broadcasting commands to all robots at once.
+
+ Also is the container for the individual :obj:`Crazyflie` objects.
+
+ Attributes:
+ crazyfiles (List[Crazyflie]): List of one Crazyflie object per robot,
+ as determined by the crazyflies.yaml config file.
+ crazyfliesById (Dict[int, Crazyflie]): Index to the same Crazyflie
+ objects by their ID number (last byte of radio address).
+ """
+ def __init__(self, crazyflies_yaml="../launch/crazyflies.yaml"):
+ """Initialize the server. Waits for all ROS services before returning.
+
+ Args:
+ timeHelper (TimeHelper): TimeHelper instance.
+ crazyflies_yaml (str): If ends in ".yaml", interpret as a path and load
+ from file. Otherwise, interpret as YAML string and parse
+ directly from string.
+ """
+ rospy.init_node("CrazyflieAPI", anonymous=False)
+ rospy.wait_for_service("/emergency")
+ self.emergencyService = rospy.ServiceProxy("/emergency", Empty)
+ rospy.wait_for_service("/takeoff")
+ self.takeoffService = rospy.ServiceProxy("/takeoff", Takeoff)
+ rospy.wait_for_service("/land")
+ self.landService = rospy.ServiceProxy("/land", Land)
+ # rospy.wait_for_service("/stop")
+ # self.stopService = rospy.ServiceProxy("/stop", Stop)
+ rospy.wait_for_service("/go_to")
+ self.goToService = rospy.ServiceProxy("/go_to", GoTo)
+ rospy.wait_for_service("/start_trajectory");
+ self.startTrajectoryService = rospy.ServiceProxy("/start_trajectory", StartTrajectory)
+ rospy.wait_for_service("/update_params")
+ self.updateParamsService = rospy.ServiceProxy("/update_params", UpdateParams)
+
+ if crazyflies_yaml.endswith(".yaml"):
+ with open(crazyflies_yaml, 'r') as ymlfile:
+ cfg = yaml.load(ymlfile)
+ else:
+ cfg = yaml.load(crazyflies_yaml)
+
+ self.tf = TransformListener()
+
+ self.crazyflies = []
+ self.crazyfliesById = dict()
+ for crazyflie in cfg["crazyflies"]:
+ id = int(crazyflie["id"])
+ initialPosition = crazyflie["initialPosition"]
+ cf = Crazyflie(id, initialPosition, self.tf)
+ self.crazyflies.append(cf)
+ self.crazyfliesById[id] = cf
+
+ def emergency(self):
+ """Emergency stop. Cuts power; causes future commands to be ignored.
+
+ This command is useful if the operator determines that the control
+ script is flawed, and that continuing to follow it will cause wrong/
+ self-destructive behavior from the robots. In addition to cutting
+ power to the motors, it ensures that any future commands, both high-
+ level and streaming, will have no effect.
+
+ The only ways to reset the firmware after an emergency stop has occurred
+ are a physical hard reset or an nRF51 Reboot command.
+ """
+ self.emergencyService()
+
+ def takeoff(self, targetHeight, duration, groupMask = 0):
+ """Broadcasted takeoff - fly straight up, then hover indefinitely.
+
+ Broadcast version of :meth:`Crazyflie.takeoff()`. All robots that match the
+ groupMask take off at exactly the same time. Use for synchronized
+ movement. Asynchronous command; returns immediately.
+
+ Args:
+ targetHeight (float): The z-coordinate at which to hover.
+ duration (float): How long until the height is reached. Seconds.
+ groupMask (int): Group mask bits. See :meth:`setGroupMask()` doc.
+ """
+ self.takeoffService(groupMask, targetHeight, rospy.Duration.from_sec(duration))
+
+ def land(self, targetHeight, duration, groupMask = 0):
+ """Broadcasted landing - fly straight down. User must cut power after.
+
+ Broadcast version of :meth:`Crazyflie.land()`. All robots that match the
+ groupMask land at exactly the same time. Use for synchronized
+ movement. Asynchronous command; returns immediately.
+
+ Args:
+ targetHeight (float): The z-coordinate at which to land. Meters.
+ Usually should be a few centimeters above the initial position
+ to ensure that the controller does not try to penetrate the
+ floor if the mocap coordinate origin is not perfect.
+ duration (float): How long until the height is reached. Seconds.
+ groupMask (int): Group mask bits. See :meth:`Crazyflie.setGroupMask()` doc.
+ """
+ self.landService(groupMask, targetHeight, rospy.Duration.from_sec(duration))
+
+ # def stop(self, groupMask = 0):
+ # self.stopService(groupMask)
+
+ def goTo(self, goal, yaw, duration, groupMask = 0):
+ """Broadcasted goTo - Move smoothly to goal, then hover indefinitely.
+
+ Broadcast version of :meth:`Crazyflie.goTo()`. All robots that match the
+ groupMask start moving at exactly the same time. Use for synchronized
+ movement. Asynchronous command; returns immediately.
+
+ While the individual goTo() supports both relative and absolute
+ coordinates, the broadcasted goTo only makes sense with relative
+ coordinates (since absolute broadcasted goTo() would cause a collision).
+ Therefore, there is no `relative` kwarg.
+
+ See docstring of :meth:`Crazyflie.goTo()` for additional details.
+
+ Args:
+ goal (iterable of 3 floats): The goal offset. Meters.
+ yaw (float): The goal yaw angle (heading). Radians.
+ duration (float): How long until the goal is reached. Seconds.
+ groupMask (int): Group mask bits. See :meth:`Crazyflie.setGroupMask()` doc.
+ """
+ gp = arrayToGeometryPoint(goal)
+ self.goToService(groupMask, True, gp, yaw, rospy.Duration.from_sec(duration))
+
+ def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
+ """Broadcasted - begins executing a previously uploaded trajectory.
+
+ Broadcast version of :meth:`Crazyflie.startTrajectory()`.
+ Asynchronous command; returns immediately.
+
+ Args:
+ trajectoryId (int): ID number as given to :meth:`Crazyflie.uploadTrajectory()`.
+ timescale (float): Scales the trajectory duration by this factor.
+ For example if timescale == 2.0, the trajectory will take twice
+ as long to execute as the nominal duration.
+ reverse (bool): If true, executes the trajectory backwards in time.
+ relative (bool): If true (default), the position of the trajectory
+ is shifted such that it begins at the current position setpoint.
+ groupMask (int): Group mask bits. See :meth:`Crazyflie.setGroupMask()` doc.
+ """
+ self.startTrajectoryService(groupMask, trajectoryId, timescale, reverse, relative)
+
+ def setParam(self, name, value):
+ """Broadcasted setParam. See Crazyflie.setParam() for details."""
+ rospy.set_param("/allcfs/" + name, value)
+ self.updateParamsService([name])
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflie.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflie.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..faa6fa7f46b3a8e2ad1399cdb455a88008504498
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflie.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflieSim.py b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflieSim.py
new file mode 100644
index 0000000000000000000000000000000000000000..7a03623ae04c7c99d286625328127361645478c7
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyflieSim.py
@@ -0,0 +1,496 @@
+#!/usr/bin/env python
+
+import math
+
+import yaml
+import numpy as np
+
+from .cfsim import cffirmware as firm
+
+# main class of simulation.
+# crazyflies keep reference to this object to ask what time it is.
+# also does the plotting.
+#
+class TimeHelper:
+ def __init__(self, vis, dt, writecsv, disturbanceSize, maxVel=np.inf, videopath=None):
+ if vis == "mpl":
+ from .visualizer import visMatplotlib
+ self.visualizer = visMatplotlib.VisMatplotlib()
+ elif vis == "vispy":
+ from .visualizer import visVispy
+ resizable = videopath is None
+ self.visualizer = visVispy.VisVispy(resizable=resizable)
+ elif vis == "null":
+ from .visualizer import visNull
+ self.visualizer = visNull.VisNull()
+ else:
+ raise Exception("Unknown visualization backend: {}".format(vis))
+ self.t = 0.0
+ self.dt = dt
+ # Since our integration/animation ticks are always the fixed duration
+ # dt, any call to sleep() with a non-multiple of dt will have some
+ # "leftover" time. Keep track of it here and add extra ticks in future.
+ self.sleepResidual = 0.0
+ self.crazyflies = []
+ self.disturbanceSize = disturbanceSize
+ self.maxVel = maxVel
+ if writecsv:
+ from . import output
+ self.output = output.Output()
+ else:
+ self.output = None
+
+ if videopath is not None:
+ from .videowriter import VideoWriter
+ frame = self.visualizer.render()
+ self.videoWriter = VideoWriter(videopath, dt, frame.shape[:2])
+ else:
+ self.videoWriter = None
+
+ def time(self):
+ return self.t
+
+ def step(self, duration):
+ self.t += duration
+ for cf in self.crazyflies:
+ cf.integrate(duration, self.disturbanceSize, self.maxVel)
+ for cf in self.crazyflies:
+ cf.flip()
+
+ # should be called "animate" or something
+ # but called "sleep" for source-compatibility with real-robot scripts
+ def sleep(self, duration):
+ # operator // has unexpected (wrong ?) behavior for this calculation.
+ ticks = math.floor((duration + self.sleepResidual) / self.dt)
+ self.sleepResidual += duration - self.dt * ticks
+ assert -1e-9 <= self.sleepResidual < self.dt
+
+ for _ in range(int(ticks)):
+ self.visualizer.update(self.t, self.crazyflies)
+ if self.output:
+ self.output.update(self.t, self.crazyflies)
+ if self.videoWriter is not None:
+ frame = self.visualizer.render()
+ self.videoWriter.writeFrame(frame)
+ self.step(self.dt)
+
+ # Mock for abstraction of rospy.Rate.sleep().
+ def sleepForRate(self, rate):
+ # TODO: account for rendering time, or is it worth the complexity?
+ self.sleep(1.0 / rate)
+
+ # Mock for abstraction of rospy.is_shutdown().
+ def isShutdown(self):
+ return False
+
+ def addObserver(self, observer):
+ self.observers.append(observer)
+
+ def _atexit(self):
+ if self.videoWriter is not None:
+ self.videoWriter.close()
+
+
+def collisionAvoidanceUpdateSetpoint(
+ collisionParams, collisionState, mode, state, setState, otherCFs):
+ """Modifies a setpoint based on firmware collision-avoidance algorithm.
+
+ Main purpose is to hide the firmware's stabilizer_types.h types, because we
+ prefer to work with cmath3d-based types.
+
+ Args:
+ collisionParams (firmware collisionAvoidanceParams_t): Collision
+ avoidance algorithm parameters. Generally will remain constant.
+ collisionState: (firmware collisionAvoidanceState_t): Opaque collision
+ avoidance internal state. **Is modified in-place.** The same object
+ should be passed to this function in repeated calls.
+ mode (Crazyflie.MODE_* enum): The current flight mode.
+ state (firmware traj_eval): The Crazyflie's currents state.
+ setState (firmware traj_eval): The desired state generated by polynomial
+ trajectory, user low-level commands, etc.
+ otherCFs (array of Crazyflie): The other Crazyflie objects in the swarm.
+
+ Returns:
+ newSetState (firmware traj_eval): A new desired state that attempts to
+ remain close to setState input while ensuring collision avoidance.
+ """
+
+ # This is significantly faster than calling position() on all the other CFs:
+ # 1.2 vs 1.8 seconds in test_collisionAvoidance.py::test_goToWithCA_random.
+ nOthers = len(otherCFs)
+ otherPositions = np.zeros((nOthers, 3), dtype=np.float32)
+ for i, cf in enumerate(otherCFs):
+ otherPositions[i][0] = cf.state.pos.x
+ otherPositions[i][1] = cf.state.pos.y
+ otherPositions[i][2] = cf.state.pos.z
+
+ cmdState = firm.state_t()
+ # Position and velocity are the only states collision avoidance observes.
+ cmdState.position = firm.svec2vec(state.pos)
+ cmdState.velocity = firm.svec2vec(state.vel)
+
+ # Dummy - it accepts the input to match the API of SitAw, but it's unused.
+ sensorData = firm.sensorData_t()
+
+ setpoint = firm.setpoint_t()
+ if mode == Crazyflie.MODE_IDLE:
+ pass
+ elif mode in (Crazyflie.MODE_HIGH_POLY, Crazyflie.MODE_LOW_FULLSTATE):
+ setpoint.mode.x = firm.modeAbs
+ setpoint.position = firm.svec2vec(setState.pos)
+ setpoint.velocity = firm.svec2vec(setState.vel)
+ elif mode == Crazyflie.MODE_LOW_POSITION:
+ setpoint.mode.x = firm.modeAbs
+ setpoint.position = firm.svec2vec(setState.pos)
+ elif mode == Crazyflie.MODE_LOW_VELOCITY:
+ setpoint.mode.x = firm.modeVelocity
+ setpoint.velocity = firm.svec2vec(setState.vel)
+ else:
+ raise ValueError("Unknown flight mode.")
+
+ firm.collisionAvoidanceUpdateSetpointWrap(
+ collisionParams,
+ collisionState,
+ otherPositions.flatten(),
+ setpoint,
+ sensorData,
+ cmdState)
+
+ newSetState = firm.traj_eval_zero()
+ newSetState.pos = firm.vec2svec(setpoint.position)
+ newSetState.vel = firm.vec2svec(setpoint.velocity)
+ newSetState.yaw = setState.yaw
+ newSetState.omega.z = setState.omega[2]
+ return newSetState
+
+
+class Crazyflie:
+
+ # Flight modes.
+ MODE_IDLE = 0
+ MODE_HIGH_POLY = 1
+ MODE_LOW_FULLSTATE = 2
+ MODE_LOW_POSITION = 3
+ MODE_LOW_VELOCITY = 4
+
+
+ def __init__(self, id, initialPosition, timeHelper):
+
+ # Core.
+ self.id = id
+ self.groupMask = 0
+ self.initialPosition = np.array(initialPosition)
+ self.time = lambda: timeHelper.time()
+
+ # Commander.
+ self.mode = Crazyflie.MODE_IDLE
+ self.planner = firm.planner()
+ firm.plan_init(self.planner)
+ self.trajectories = dict()
+ self.setState = firm.traj_eval_zero()
+
+ # State. Public np.array-returning getters below for physics state.
+ self.state = firm.traj_eval_zero()
+ self.state.pos = firm.mkvec(*initialPosition)
+ self.state.vel = firm.vzero()
+ self.state.acc = firm.vzero()
+ self.state.yaw = 0.0
+ self.state.omega = firm.vzero()
+ self.ledRGB = (0.5, 0.5, 1)
+
+ # Double-buffering: Ensure that all CFs observe the same world state
+ # during an integration step, regardless of the order in which their
+ # integrate() methods are called. flip() swaps front and back state.
+ # See http://gameprogrammingpatterns.com/double-buffer.html for more
+ # motivation.
+ self.backState = firm.traj_eval(self.state)
+
+ # For collision avoidance.
+ self.otherCFs = []
+ self.collisionAvoidanceParams = None
+ self.collisionAvoidanceState = None
+
+ def setGroupMask(self, groupMask):
+ self.groupMask = groupMask
+
+ def enableCollisionAvoidance(self, others, ellipsoidRadii, bboxMin=np.repeat(-np.inf, 3), bboxMax=np.repeat(np.inf, 3), horizonSecs=1.0, maxSpeed=2.0):
+ self.otherCFs = [cf for cf in others if cf is not self]
+
+ # TODO: Accept more of these from arguments.
+ params = firm.collision_avoidance_params_t()
+ params.ellipsoidRadii = firm.mkvec(*ellipsoidRadii)
+ params.bboxMin = firm.mkvec(*bboxMin)
+ params.bboxMax = firm.mkvec(*bboxMax)
+ params.horizonSecs = horizonSecs
+ params.maxSpeed = maxSpeed
+ params.sidestepThreshold = 0.25
+ params.voronoiProjectionTolerance = 1e-5
+ params.voronoiProjectionMaxIters = 100
+ self.collisionAvoidanceParams = params
+
+ state = firm.collision_avoidance_state_t()
+ state.lastFeasibleSetPosition = firm.mkvec(np.nan, np.nan, np.nan)
+ self.collisionAvoidanceState = state
+
+ def disableCollisionAvoidance(self):
+ self.otherCFs = None
+ self.collisionAvoidanceParams = None
+ self.collisionAvoidanceState = None
+
+ def takeoff(self, targetHeight, duration, groupMask = 0):
+ if self._isGroup(groupMask):
+ self.mode = Crazyflie.MODE_HIGH_POLY
+ targetYaw = 0.0
+ firm.plan_takeoff(self.planner,
+ self.state.pos, self.state.yaw, targetHeight, targetYaw, duration, self.time())
+
+ def land(self, targetHeight, duration, groupMask = 0):
+ if self._isGroup(groupMask):
+ self.mode = Crazyflie.MODE_HIGH_POLY
+ targetYaw = 0.0
+ firm.plan_land(self.planner,
+ self.state.pos, self.state.yaw, targetHeight, targetYaw, duration, self.time())
+
+ def stop(self, groupMask = 0):
+ if self._isGroup(groupMask):
+ self.mode = Crazyflie.MODE_IDLE
+ firm.plan_stop(self.planner)
+
+ def goTo(self, goal, yaw, duration, relative = False, groupMask = 0):
+ if self._isGroup(groupMask):
+ if self.mode != Crazyflie.MODE_HIGH_POLY:
+ # We need to update to the latest firmware that has go_to_from.
+ raise ValueError("goTo from low-level modes not yet supported.")
+ self.mode = Crazyflie.MODE_HIGH_POLY
+ firm.plan_go_to(self.planner, relative, firm.mkvec(*goal), yaw, duration, self.time())
+
+ def uploadTrajectory(self, trajectoryId, pieceOffset, trajectory):
+ traj = firm.piecewise_traj()
+ traj.t_begin = 0
+ traj.timescale = 1.0
+ traj.shift = firm.mkvec(0, 0, 0)
+ traj.n_pieces = len(trajectory.polynomials)
+ traj.pieces = firm.malloc_poly4d(len(trajectory.polynomials))
+ for i, poly in enumerate(trajectory.polynomials):
+ piece = firm.pp_get_piece(traj, i)
+ piece.duration = poly.duration
+ for coef in range(0, 8):
+ firm.poly4d_set(piece, 0, coef, poly.px.p[coef])
+ firm.poly4d_set(piece, 1, coef, poly.py.p[coef])
+ firm.poly4d_set(piece, 2, coef, poly.pz.p[coef])
+ firm.poly4d_set(piece, 3, coef, poly.pyaw.p[coef])
+ self.trajectories[trajectoryId] = traj
+
+ def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
+ if self._isGroup(groupMask):
+ self.mode = Crazyflie.MODE_HIGH_POLY
+ traj = self.trajectories[trajectoryId]
+ traj.t_begin = self.time()
+ traj.timescale = timescale
+ if relative:
+ traj.shift = firm.vzero()
+ if reverse:
+ traj_init = firm.piecewise_eval_reversed(traj, traj.t_begin)
+ else:
+ traj_init = firm.piecewise_eval(traj, traj.t_begin)
+ traj.shift = self.state.pos - traj_init.pos
+ else:
+ traj.shift = firm.vzero()
+ firm.plan_start_trajectory(self.planner, traj, reverse)
+
+ def notifySetpointsStop(self, remainValidMillisecs=100):
+ # No-op - the real Crazyflie prioritizes streaming setpoints over
+ # high-level commands. This tells it to stop doing that. We don't
+ # simulate this behavior.
+ pass
+
+ def position(self):
+ return np.array(self.state.pos)
+
+ def getParam(self, name):
+ print("WARNING: getParam not implemented in simulation!")
+
+ def setParam(self, name, value):
+ print("WARNING: setParam not implemented in simulation!")
+
+ def setParams(self, params):
+ print("WARNING: setParams not implemented in simulation!")
+
+ # - this is a part of the param system on the real crazyflie,
+ # but we implement it in simulation too for debugging
+ # - is a blocking command on real CFs, so may cause stability problems
+ def setLEDColor(self, r, g, b):
+ self.ledRGB = (r, g, b)
+
+ # simulation only functions
+ def yaw(self):
+ return float(self.state.yaw)
+
+ def velocity(self):
+ return np.array(self.state.vel)
+
+ def acceleration(self):
+ return np.array(self.state.acc)
+
+ def rpy(self):
+ yaw = self.yaw()
+ # Unpack the matrix columns.
+ x_body, y_body, z_body = self.rotBodyToWorld().T
+ pitch = math.asin(-x_body[2])
+ roll = math.atan2(y_body[2], z_body[2])
+ return (roll, pitch, yaw)
+
+ def rotBodyToWorld(self):
+ acc = self.acceleration()
+ yaw = self.yaw()
+ norm = np.linalg.norm(acc)
+ # TODO: This causes a vertical flip for faster-than-gravity vertical
+ # deceleration, but fixing it would essentially require introducing the
+ # idea of a controller, which we have avoided so far.
+ thrust = acc + np.array([0, 0, 9.81])
+ z_body = thrust / np.linalg.norm(thrust)
+ x_world = np.array([math.cos(yaw), math.sin(yaw), 0])
+ y_body = np.cross(z_body, x_world)
+ # TODO: This can have a singularity if z_body = x_world.
+ y_body /= np.linalg.norm(y_body)
+ x_body = np.cross(y_body, z_body)
+ return np.column_stack([x_body, y_body, z_body])
+
+ def cmdFullState(self, pos, vel, acc, yaw, omega):
+ self.mode = Crazyflie.MODE_LOW_FULLSTATE
+ self.setState.pos = firm.mkvec(*pos)
+ self.setState.vel = firm.mkvec(*vel)
+ self.setState.acc = firm.mkvec(*acc)
+ self.setState.yaw = yaw
+ self.setState.omega = firm.mkvec(*omega)
+
+ def cmdPosition(self, pos, yaw = 0):
+ self.mode = Crazyflie.MODE_LOW_POSITION
+ self.setState.pos = firm.mkvec(*pos)
+ self.setState.yaw = yaw
+ # TODO: should we set vel, acc, omega to zero, or rely on modes to not read them?
+
+ def cmdVelocityWorld(self, vel, yawRate):
+ self.mode = Crazyflie.MODE_LOW_VELOCITY
+ self.setState.vel = firm.mkvec(*vel)
+ self.setState.omega = firm.mkvec(0.0, 0.0, yawRate)
+ # TODO: should we set pos, acc, yaw to zero, or rely on modes to not read them?
+
+ def cmdStop(self):
+ # TODO: set mode to MODE_IDLE?
+ pass
+
+ def integrate(self, time, disturbanceSize, maxVel):
+ if self.mode == Crazyflie.MODE_HIGH_POLY:
+ self.setState = firm.plan_current_goal(self.planner, self.time())
+
+ if self.collisionAvoidanceState is not None:
+ setState = collisionAvoidanceUpdateSetpoint(
+ self.collisionAvoidanceParams,
+ self.collisionAvoidanceState,
+ self.mode,
+ self.state,
+ self.setState,
+ self.otherCFs,
+ )
+ else:
+ setState = firm.traj_eval(self.setState)
+
+ if self.mode == Crazyflie.MODE_IDLE:
+ return
+
+ if self.mode in (Crazyflie.MODE_HIGH_POLY, Crazyflie.MODE_LOW_FULLSTATE, Crazyflie.MODE_LOW_POSITION):
+ velocity = (setState.pos - self.state.pos) / time
+ elif self.mode == Crazyflie.MODE_LOW_VELOCITY:
+ velocity = setState.vel
+ else:
+ raise ValueError("Unknown flight mode.")
+
+ # Limit velocity for realism.
+ # Note: This will result in the state having a different velocity than
+ # the setState in HIGH_POLY and LOW_FULLSTATE modes even when no
+ # clamping occurs, because we are essentially getting rid of the
+ # feedforward commands. We assume this is not a problem.
+
+ velocity = firm.vclampnorm(velocity, maxVel)
+
+ disturbance = disturbanceSize * np.random.normal(size=3)
+ velocity = velocity + firm.mkvec(*disturbance)
+ self.backState = firm.traj_eval(setState)
+ self.backState.pos = self.state.pos + time * velocity
+ self.backState.vel = velocity
+
+ if self.mode == Crazyflie.MODE_LOW_POSITION:
+ yawRate = (setState.yaw - self.state.yaw) / time
+ self.backState.yaw = setState.yaw
+ self.backState.omega = firm.mkvec(0.0, 0.0, yawRate)
+ elif self.mode == Crazyflie.MODE_LOW_VELOCITY:
+ # Omega is already set.
+ self.backState.yaw += time * self.setState.omega.z
+
+ # In HIGH_POLY and LOW_FULLSTATE, yaw and omega are already specified
+ # in setState and have been copied.
+
+ def flip(self):
+ # Swap double-buffered state. Called at the end of the tick update,
+ # after *all* CFs' integrate() methods have been called.
+ self.state, self.backState = self.backState, self.state
+
+ # "private" methods
+ def _isGroup(self, groupMask):
+ return groupMask == 0 or (self.groupMask & groupMask) > 0
+
+
+class CrazyflieServer:
+ def __init__(self, timeHelper, crazyflies_yaml="../launch/crazyflies.yaml"):
+ """Initialize the server.
+
+ Args:
+ timeHelper (TimeHelper): TimeHelper instance.
+ crazyflies_yaml (str): If ends in ".yaml", interpret as a path and load
+ from file. Otherwise, interpret as YAML string and parse
+ directly from string.
+ """
+ if crazyflies_yaml.endswith(".yaml"):
+ with open(crazyflies_yaml, 'r') as ymlfile:
+ cfg = yaml.safe_load(ymlfile)
+ else:
+ cfg = yaml.safe_load(crazyflies_yaml)
+
+ self.crazyflies = []
+ self.crazyfliesById = dict()
+ for crazyflie in cfg["crazyflies"]:
+ id = int(crazyflie["id"])
+ initialPosition = crazyflie["initialPosition"]
+ cf = Crazyflie(id, initialPosition, timeHelper)
+ self.crazyflies.append(cf)
+ self.crazyfliesById[id] = cf
+
+ self.timeHelper = timeHelper
+ self.timeHelper.crazyflies = self.crazyflies
+
+ def emergency(self):
+ print("WARNING: emergency not implemented in simulation!")
+
+ def takeoff(self, targetHeight, duration, groupMask = 0):
+ for crazyflie in self.crazyflies:
+ crazyflie.takeoff(targetHeight, duration, groupMask)
+
+ def land(self, targetHeight, duration, groupMask = 0):
+ for crazyflie in self.crazyflies:
+ crazyflie.land(targetHeight, duration, groupMask)
+
+ def stop(self, groupMask = 0):
+ for crazyflie in self.crazyflies:
+ crazyflie.stop(groupMask)
+
+ def goTo(self, goal, yaw, duration, groupMask = 0):
+ for crazyflie in self.crazyflies:
+ crazyflie.goTo(goal, yaw, duration, relative=True, groupMask=groupMask)
+
+ def startTrajectory(self, trajectoryId, timescale = 1.0, reverse = False, relative = True, groupMask = 0):
+ for crazyflie in self.crazyflies:
+ crazyflie.startTrajectory(trajectoryId, timescale, reverse, relative, groupMask)
+
+ def setParam(self, name, value):
+ print("WARNING: setParam not implemented in simulation!")
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/crazyswarm_py.py b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyswarm_py.py
new file mode 100644
index 0000000000000000000000000000000000000000..df6a14bd71423442c4ae523594ee2b7e521b91be
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyswarm_py.py
@@ -0,0 +1,58 @@
+import argparse
+import atexit
+
+import numpy as np
+
+from . import genericJoystick
+
+# Building the parser in a separate function allows sphinx-argparse to
+# auto-generate the documentation for the command-line flags.
+def build_argparser(parent_parsers=[]):
+ parser = argparse.ArgumentParser(
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter,
+ parents=parent_parsers
+ )
+ parser.add_argument("--sim", help="Run using simulation.", action="store_true")
+
+ group = parser.add_argument_group("Simulation-only", "")
+ group.add_argument("--vis", help="Visualization backend.", choices=['mpl', 'vispy', 'null'], default="mpl")
+ group.add_argument("--dt", help="Duration of seconds between rendered visualization frames.", type=float, default=0.1)
+ group.add_argument("--writecsv", help="Enable CSV output.", action="store_true")
+ group.add_argument("--disturbance", help="Simulate Gaussian-distributed disturbance when using cmdVelocityWorld.", type=float, default=0.0)
+ group.add_argument("--maxvel", help="Limit simulated velocity (meters/sec).", type=float, default=np.inf)
+ group.add_argument("--video", help="Video output path.", type=str)
+
+ return parser
+
+
+class Crazyswarm:
+ def __init__(self, crazyflies_yaml=None, parent_parser=None, args=None):
+ if parent_parser is not None:
+ parents = [parent_parser]
+ else:
+ parents = []
+ parser = build_argparser(parents)
+ if isinstance(args, str):
+ args = args.split()
+ args, unknown = parser.parse_known_args(args)
+
+ if crazyflies_yaml is None:
+ crazyflies_yaml = "../launch/crazyflies.yaml"
+ if crazyflies_yaml.endswith(".yaml"):
+ crazyflies_yaml = open(crazyflies_yaml, 'r').read()
+
+ if args.sim:
+ from .crazyflieSim import TimeHelper, CrazyflieServer
+ self.timeHelper = TimeHelper(args.vis, args.dt, args.writecsv, disturbanceSize=args.disturbance, maxVel=args.maxvel, videopath=args.video)
+ self.allcfs = CrazyflieServer(self.timeHelper, crazyflies_yaml)
+ atexit.register(self.timeHelper._atexit)
+ else:
+ from .crazyflie import TimeHelper, CrazyflieServer
+ self.allcfs = CrazyflieServer(crazyflies_yaml)
+ self.timeHelper = TimeHelper()
+ if args.writecsv:
+ print("WARNING: writecsv argument ignored! This is only available in simulation.")
+ if args.video:
+ print("WARNING: video argument ignored! This is only available in simulation.")
+
+ self.input = genericJoystick.Joystick(self.timeHelper)
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/crazyswarm_py.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyswarm_py.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cb7763f54dd55966da46ba080370a882c4a7028b
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/crazyswarm_py.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/genericJoystick.py b/real_robot/crazyswarm/scripts/pycrazyswarm/genericJoystick.py
new file mode 100644
index 0000000000000000000000000000000000000000..2dbb08be3054b3e9055fdc823968f3989a7e596e
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/genericJoystick.py
@@ -0,0 +1,99 @@
+import time
+import copy
+# import pyglet
+from . import keyboard
+
+# class JoyStickHandler:
+# def __init__(self):
+# self.buttonWasPressed = False
+
+# def on_joybutton_press(joystick, button):
+# if button == 5:
+# self.buttonWasPressed = True
+
+# def on_joybutton_release(joystick, button):
+# pass
+
+# def on_joyaxis_motion(joystick, axis, value):
+# pass
+
+# def on_joyhat_motion(joystick, hat_x, hat_y):
+# pass
+
+class Joystick:
+ def __init__(self, timeHelper):
+ # joysticks = pyglet.input.get_joysticks()
+ # joystick = joysticks[0]
+ # joystick.open()
+ # self.buttonWasPressed = False
+ # joystick.push_handlers(self)
+ self.timeHelper = timeHelper
+ self.joyID = None
+
+ try:
+ from . import linuxjsdev
+ self.js = linuxjsdev.Joystick()
+ devices = self.js.devices()
+ if len(devices) == 0:
+ print("Warning: No joystick found!")
+ else:
+ ids = [dev["id"] for dev in devices]
+ # For backwards compatibility, always choose device 0 if available.
+ self.joyID = 0 if 0 in ids else devices[0]["id"]
+ self.js.open(self.joyID)
+ except ImportError:
+ print("Warning: Joystick only supported on Linux.")
+
+ # def on_joybutton_press(joystick, button):
+ # print(button)
+ # if button == 5:
+ # self.buttonWasPressed = True
+
+ # def wasButtonPressed(self):
+ # return self.buttonWasPressed
+
+ # def clearButtonPressed(self):
+ # self.buttonWasPressed = False
+
+ def checkIfButtonIsPressed(self):
+ if self.joyID is not None:
+ state = self.js.read(self.joyID)
+ return state[1][5] == 1
+ else:
+ return False
+
+ def waitUntilButtonPressed(self):
+ if self.joyID is not None:
+ while not self.checkIfButtonIsPressed():
+ self.timeHelper.sleep(0.01)
+ while self.checkIfButtonIsPressed():
+ self.timeHelper.sleep(0.01)
+ else:
+ with keyboard.KeyPoller() as keyPoller:
+ # Wait until a key is pressed.
+ while keyPoller.poll() is None:
+ self.timeHelper.sleep(0.01)
+ # Wait until the key is released.
+ while keyPoller.poll() is not None:
+ self.timeHelper.sleep(0.01)
+
+
+ def checkIfAnyButtonIsPressed(self):
+ if self.joyID is not None:
+ state = self.js.read(self.joyID)
+ if state[1][5] == 1 or state[1][4] == 1 or state[1][3] == 1:
+ return state[1]
+ else:
+ return None
+ else:
+ return None
+
+ def waitUntilAnyButtonPressed(self):
+ if self.joyID is not None:
+ buttons = self.checkIfAnyButtonIsPressed()
+ while buttons is None:
+ self.timeHelper.sleep(0.01)
+ buttons = copy.copy(self.checkIfAnyButtonIsPressed())
+ while self.checkIfAnyButtonIsPressed() is not None:
+ self.timeHelper.sleep(0.01)
+ return buttons
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/genericJoystick.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/genericJoystick.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..995cf849e9a98efb76b3be04b55a5ee481a23aa9
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/genericJoystick.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/joystick.py b/real_robot/crazyswarm/scripts/pycrazyswarm/joystick.py
new file mode 100644
index 0000000000000000000000000000000000000000..ba8207965e3069cb8224671affab4df021543703
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/joystick.py
@@ -0,0 +1,21 @@
+import time
+import rospy
+from sensor_msgs.msg import Joy
+
+class Joystick:
+ def __init__(self):
+ self.lastButtonState = 0
+ self.buttonWasPressed = False
+ rospy.Subscriber("/joy", Joy, self.joyChanged)
+
+ def joyChanged(self, data):
+ if (not self.buttonWasPressed and
+ data.buttons[5] == 1 and
+ self.lastButtonState == 0):
+ self.buttonWasPressed = True
+ self.lastButtonState = data.buttons[5]
+
+ def waitUntilButtonPressed(self):
+ while not rospy.is_shutdown() and not self.buttonWasPressed:
+ time.sleep(0.01)
+ self.buttonWasPressed = False
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/keyboard.py b/real_robot/crazyswarm/scripts/pycrazyswarm/keyboard.py
new file mode 100644
index 0000000000000000000000000000000000000000..e5f788775516c76c3d16db1b17b7950a4543f9e0
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/keyboard.py
@@ -0,0 +1,26 @@
+import sys
+import select
+import termios
+
+
+class KeyPoller():
+ def __enter__(self):
+ # Save the terminal settings
+ self.fd = sys.stdin.fileno()
+ self.new_term = termios.tcgetattr(self.fd)
+ self.old_term = termios.tcgetattr(self.fd)
+
+ # New terminal setting unbuffered
+ self.new_term[3] = (self.new_term[3] & ~termios.ICANON & ~termios.ECHO)
+ termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.new_term)
+
+ return self
+
+ def __exit__(self, type, value, traceback):
+ termios.tcsetattr(self.fd, termios.TCSAFLUSH, self.old_term)
+
+ def poll(self):
+ dr,dw,de = select.select([sys.stdin], [], [], 0)
+ if not dr == []:
+ return sys.stdin.read(1)
+ return None
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/keyboard.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/keyboard.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..83c1115cf711f2512a11555c04150a584761c7bd
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/keyboard.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/linuxjsdev.py b/real_robot/crazyswarm/scripts/pycrazyswarm/linuxjsdev.py
new file mode 100644
index 0000000000000000000000000000000000000000..ba5df731e600815c72d418b09424b7b6149eea4f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/linuxjsdev.py
@@ -0,0 +1,233 @@
+# -*- coding: utf-8 -*-
+# ||
+# +------+ / __ )(_) /_______________ _____ ___
+# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
+# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
+# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
+#
+# Copyright (C) 2011-2013 Bitcraze AB
+#
+# Crazyflie Nano Quadcopter Client
+#
+# This program is free software; you can redistribute it and/or
+# modify it under the terms of the GNU General Public License
+# as published by the Free Software Foundation; either version 2
+# of the License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+# GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License
+# along with this program; if not, write to the Free Software Foundation,
+# Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
+"""
+Linux joystick driver using the Linux input_joystick subsystem. Requires sysfs
+to be mounted on /sys and /dev/input/js* to be readable.
+
+This module is very linux specific but should work on any CPU platform
+"""
+import ctypes
+import glob
+import logging
+import os
+import struct
+import sys
+
+if not sys.platform.startswith('linux'):
+ raise ImportError("Only supported on Linux")
+
+try:
+ import fcntl
+except ImportError as e:
+ raise Exception("fcntl library probably not installed ({})".format(e))
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Joystick']
+
+logger = logging.getLogger(__name__)
+
+JS_EVENT_FMT = "@IhBB"
+JE_TIME = 0
+JE_VALUE = 1
+JE_TYPE = 2
+JE_NUMBER = 3
+
+JS_EVENT_BUTTON = 0x001
+JS_EVENT_AXIS = 0x002
+JS_EVENT_INIT = 0x080
+
+# ioctls
+JSIOCGAXES = 0x80016a11
+JSIOCGBUTTONS = 0x80016a12
+
+MODULE_MAIN = "Joystick"
+MODULE_NAME = "linuxjsdev"
+
+
+class JEvent(object):
+ """
+ Joystick event class. Encapsulate single joystick event.
+ """
+
+ def __init__(self, evt_type, number, value):
+ self.type = evt_type
+ self.number = number
+ self.value = value
+
+ def __repr__(self):
+ return "JEvent(type={}, number={}, value={})".format(
+ self.type, self.number, self.value)
+
+# Constants
+TYPE_BUTTON = 1
+TYPE_AXIS = 2
+
+
+class _JS():
+
+ def __init__(self, num, name):
+ self.num = num
+ self.name = name
+ self._f_name = "/dev/input/js{}".format(num)
+ self._f = None
+
+ self.opened = False
+ self.buttons = []
+ self.axes = []
+ self._prev_pressed = {}
+
+ def open(self):
+ if self._f:
+ raise Exception("{} at {} is already "
+ "opened".format(self.name, self._f_name))
+
+ self._f = open("/dev/input/js{}".format(self.num), "rb")
+ fcntl.fcntl(self._f.fileno(), fcntl.F_SETFL, os.O_NONBLOCK)
+
+ # Get number of axis and button
+ val = ctypes.c_int()
+ if fcntl.ioctl(self._f.fileno(), JSIOCGAXES, val) != 0:
+ self._f.close()
+ self._f = None
+ raise Exception("Failed to read number of axes")
+
+ self.axes = list(0 for i in range(val.value))
+ if fcntl.ioctl(self._f.fileno(), JSIOCGBUTTONS, val) != 0:
+ self._f.close()
+ self._f = None
+ raise Exception("Failed to read number of axes")
+
+ self.buttons = list(0 for i in range(val.value))
+ self.__initvalues()
+
+ def close(self):
+ """Open the joystick device"""
+ if not self._f:
+ return
+
+ logger.info("Closed {} ({})".format(self.name, self.num))
+
+ self._f.close()
+ self._f = None
+
+ def __initvalues(self):
+ """Read the buttons and axes initial values from the js device"""
+ for _ in range(len(self.axes) + len(self.buttons)):
+ data = self._f.read(struct.calcsize(JS_EVENT_FMT))
+ jsdata = struct.unpack(JS_EVENT_FMT, data)
+ self.__updatestate(jsdata)
+
+ def __updatestate(self, jsdata):
+ """Update the internal absolute state of buttons and axes"""
+ if jsdata[JE_TYPE] & JS_EVENT_AXIS != 0:
+ self.axes[jsdata[JE_NUMBER]] = jsdata[JE_VALUE] / 32768.0
+ elif jsdata[JE_TYPE] & JS_EVENT_BUTTON != 0:
+ self.buttons[jsdata[JE_NUMBER]] = jsdata[JE_VALUE]
+
+ def __decode_event(self, jsdata):
+ """ Decode a jsdev event into a dict """
+ # TODO: Add timestamp?
+ if jsdata[JE_TYPE] & JS_EVENT_AXIS != 0:
+ return JEvent(evt_type=TYPE_AXIS,
+ number=jsdata[JE_NUMBER],
+ value=jsdata[JE_VALUE] / 32768.0)
+ if jsdata[JE_TYPE] & JS_EVENT_BUTTON != 0:
+ return JEvent(evt_type=TYPE_BUTTON,
+ number=jsdata[JE_NUMBER],
+ value=jsdata[JE_VALUE] / 32768.0)
+
+ def _read_all_events(self):
+ """Consume all the events queued up in the JS device"""
+ try:
+ while True:
+ data = self._f.read(struct.calcsize(JS_EVENT_FMT))
+ jsdata = struct.unpack(JS_EVENT_FMT, data)
+ self.__updatestate(jsdata)
+ except IOError as e:
+ if e.errno != 11:
+ logger.info(str(e))
+ self._f.close()
+ self._f = None
+ raise IOError("Device has been disconnected")
+ except TypeError:
+ pass
+ except ValueError:
+ # This will happen if I/O operations are done on a closed device,
+ # which is the case when you first close and then open the device
+ # while switching device. But, in order for SDL2 to work on Linux
+ # (for debugging) the device needs to be closed before it's opened.
+ # This is the workaround to make both cases work.
+ pass
+
+ def read(self):
+ """ Returns a list of all joystick event since the last call """
+ if not self._f:
+ raise Exception("Joystick device not opened")
+
+ self._read_all_events()
+
+ return [self.axes, self.buttons]
+
+
+class Joystick():
+ """
+ Linux jsdev implementation of the Joystick class
+ """
+
+ def __init__(self):
+ self.name = MODULE_NAME
+ self._js = {}
+ self._devices = []
+
+ def devices(self):
+ """
+ Returns a list containing an {"id": id, "name": name} dict for each
+ detected device. Result is cached once one or more devices are found.
+ """
+ if len(self._devices) == 0:
+ syspaths = glob.glob("/sys/class/input/js*")
+
+ for path in syspaths:
+ device_id = int(os.path.basename(path)[2:])
+ with open(path + "/device/name") as namefile:
+ name = namefile.read().strip()
+ self._js[device_id] = _JS(device_id, name)
+ self._devices.append({"id": device_id, "name": name})
+
+ return self._devices
+
+ def open(self, device_id):
+ """
+ Open the joystick device. The device_id is given by available_devices
+ """
+ self._js[device_id].open()
+
+ def close(self, device_id):
+ """Open the joystick device"""
+ self._js[device_id].close()
+
+ def read(self, device_id):
+ """ Returns a list of all joystick event since the last call """
+ return self._js[device_id].read()
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/linuxjsdev.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/linuxjsdev.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1941aaf09424339c628c535c3eb9611f55987dc8
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/linuxjsdev.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/output.py b/real_robot/crazyswarm/scripts/pycrazyswarm/output.py
new file mode 100644
index 0000000000000000000000000000000000000000..47d72546387f778962b1d2faba451d1d94c7f0f1
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/output.py
@@ -0,0 +1,18 @@
+import os
+import math
+import numpy as np
+
+
+class Output:
+ def __init__(self):
+ self.data = dict()
+ self.starttime = None
+
+ def update(self, t, crazyflies):
+ for cf in crazyflies:
+ x, y, z = cf.position()
+ roll, pitch, yaw = cf.rpy()
+ if cf.id not in self.data:
+ self.data[cf.id] = np.empty((0, 7), float)
+ self.starttime = t
+ self.data[cf.id] = np.vstack([self.data[cf.id], np.array([t - self.starttime, x, y, z, roll, pitch, yaw])])
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/testCrazyflieSim.py b/real_robot/crazyswarm/scripts/pycrazyswarm/testCrazyflieSim.py
new file mode 100644
index 0000000000000000000000000000000000000000..255a3292689055e0752e21bab1eeb9be49be2f76
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/testCrazyflieSim.py
@@ -0,0 +1,31 @@
+import numpy as np
+from numpy.testing import assert_almost_equal
+
+from cfsim import CrazyflieSim, TimeHelper
+
+def main():
+
+ time = TimeHelper()
+ cf_id = 42
+ initial = np.array([1, 1, 0])
+ cf = CrazyflieSim(id, initial, time)
+
+ duration = 2.5
+ takeoff_height = 1.0
+ cf.takeoff(takeoff_height, duration)
+
+ p0 = cf.position()
+ assert_almost_equal(p0, initial, decimal=3)
+
+ time.step(duration / 2)
+ pHalf = cf.position()
+ assert(pHalf[2] > initial[2])
+ assert(pHalf[2] < takeoff_height)
+
+ time.step(duration / 2)
+ p1 = cf.position()
+ assert_almost_equal(p1[2], takeoff_height, decimal=3)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/testVispy.py b/real_robot/crazyswarm/scripts/pycrazyswarm/testVispy.py
new file mode 100644
index 0000000000000000000000000000000000000000..fdb0318b6ec6cd31bba94a79d85047ab24061386
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/testVispy.py
@@ -0,0 +1,67 @@
+# -*-coding: utf-8 -*-
+# vispy: gallery 30
+# -----------------------------------------------------------------------------
+# Copyright (c) 2015, Vispy Development Team. All Rights Reserved.
+# Distributed under the (new) BSD License. See LICENSE.txt for more info.
+# -----------------------------------------------------------------------------
+"""
+Simple use of SceneCanvas to display a cube with an arcball camera.
+"""
+import sys
+
+from vispy import scene, app, io
+from vispy.color import Color
+from vispy.visuals import transforms
+import time
+
+canvas = scene.SceneCanvas(keys='interactive', size=(800, 600), show=True)
+
+# Set up a viewbox to display the cube with interactive arcball
+view = canvas.central_widget.add_view()
+view.bgcolor = '#efefef'
+view.camera = 'turntable'
+view.padding = 100
+
+color = Color("#3f51b5")
+
+# cube = scene.visuals.Cube(size=1, color=color, edge_color="black",
+# parent=view.scene)
+
+
+verts, faces, normals, nothin = io.read_mesh("crazyflie2.obj.gz")
+
+cfs = []
+# cftransforms = []
+for i in range(0, 10):
+ mesh = scene.visuals.Mesh(vertices=verts, shading='smooth', faces=faces, parent=view.scene)
+ mesh.transform = transforms.MatrixTransform()
+ cfs.append(mesh)
+
+# cube_transform = transforms.MatrixTransform()
+# mesh.transform = cube_transform
+theta = 0
+lastTime = time.time()
+
+def update(ev):
+ global cfs, theta, lastTime
+ x = 0
+ for cf in cfs:
+ cf.transform.reset()
+ cf.transform.rotate(theta, (0, 0, 1))
+ cf.transform.scale((0.01, 0.01, 0.01))
+ cf.transform.translate((x, 0, 0))
+ x += 1.0
+ # cube_transform.reset()
+ # cube_transform.rotate(theta, (0, 0, 1))
+ theta += 1.0
+ now = time.time()
+ dt = now - lastTime
+ print(1 / dt)
+ lastTime = now
+
+timer = app.Timer()
+timer.connect(update)
+timer.start(0)
+
+if __name__ == '__main__' and sys.flags.interactive == 0:
+ canvas.app.run()
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/util.py b/real_robot/crazyswarm/scripts/pycrazyswarm/util.py
new file mode 100644
index 0000000000000000000000000000000000000000..a501d6b93113d75211787f79f59ba0274e181b57
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/util.py
@@ -0,0 +1,85 @@
+"""Useful functions for both pycrazyswarm internals and user scripts."""
+
+import numpy as np
+import scipy as sp
+import scipy.spatial
+
+
+def check_ellipsoid_collisions(positions, radii):
+ """Checks for collisions between a set of ellipsoids at given positions.
+
+ Args:
+ positions (array float[n, 3]): The ellipsoid centers.
+ radii (array float[3]): The radii of the axis-aligned ellipsoids.
+
+ Returns:
+ colliding (array bool[n]): True at index i if the i'th ellipsoid
+ intersects any of the other ellipsoids.
+ """
+ scaled = positions / radii[None, :]
+ dists = sp.spatial.distance.pdist(scaled)
+ dists = sp.spatial.distance.squareform(dists)
+ # Do not consider 0 distance to self as a collision!
+ n, _ = positions.shape
+ dists[range(n), range(n)] = np.inf
+ colliding = np.any(dists < 1.97, axis=1)
+ return colliding
+
+
+def poisson_disk_sample(n, dim, mindist):
+ """Generates random points with guaranteed minimum pairwise distance.
+
+ Uses extremely naive and slow "dart throwing" algorithm.
+ TODO(jpreiss): find/implement a library with a fast algorithm.
+
+ Args:
+ n (int): Number of points.
+ dim (int): Dimensionality of points.
+ mindist (float): Minimum Euclidean distance between any two points.
+
+ Returns:
+ pts (array float[n, dim]): The sampled points.
+ """
+
+ # Select hypercube volume such that n points will not pack it too tightly.
+ # Note: Will be too sparse for dim >> 3, but reasonable for dim == 2 or 3.
+ measure_ratio = 1.25
+ std = (measure_ratio * n) ** (1.0 / dim) * mindist
+ def sample():
+ return std * np.random.uniform(-0.5, 0.5, size=dim)
+
+ # Sample the points using dart-throwing.
+ pts = sample()[None,:]
+ while len(pts) < n:
+ pt = sample()
+ dists = np.linalg.norm(pts - pt, axis=1)
+ if np.all(dists >= mindist):
+ pts = np.concatenate([pts, pt[None,:]], axis=0)
+ return pts
+
+
+def grid_yaml(rows, cols, spacing=0.5):
+ """Generate crazyflies.yaml string for a grid in the XZ plane.
+
+ Args:
+ rows (int): Number of rows (distinct X-values) in the grid.
+ cols (int): Number of columns (distinct Y-values) in the grid.
+ spacing (float): Grid spacing for both axes.
+
+ Returns:
+ yaml (str): String containing crazyflies.yaml. Will contain
+ (rows * cols) crazyflies, all assigned on radio channel 100.
+ Positions will be centered about the origin. No particular
+ id-position mapping should be assumed.
+ """
+ x, y = spacing * np.mgrid[:rows, :cols]
+ x -= np.mean(x)
+ y -= np.mean(y)
+ return "crazyflies:\n" + "\n".join([
+ """- channel: 100
+ id: {}
+ initialPosition: [{}, {}, 0.0]""".format(i, x, y)
+ for i, (x, y) in enumerate(zip(x.flat, y.flat))
+ ])
+
+
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/videowriter.py b/real_robot/crazyswarm/scripts/pycrazyswarm/videowriter.py
new file mode 100644
index 0000000000000000000000000000000000000000..58a2efc80f225201a50ab3b79bd06fdf69c43439
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/videowriter.py
@@ -0,0 +1,57 @@
+from __future__ import print_function
+
+import ffmpeg
+import numpy as np
+
+
+class VideoWriter:
+
+ def __init__(self, path, dt, shape):
+ """Initializes video output to a file.
+
+ Args:
+ path (str): Filesystem path to write output.
+ dt (float): Time duration in seconds of frame, 1/fps.
+ shape (tuple(int, int)): Height, width dimensions of video frames.
+ """
+ height, width = shape
+ size_str = "{}x{}".format(width, height)
+ # crf=0 option specifies libx264's lossless mode, which still gives
+ # surprisingly small files.
+ self.ffmpegProcess = (
+ ffmpeg
+ .input("pipe:", format="rawvideo", pix_fmt="rgb24", s=size_str, r=1.0/dt)
+ .output(path, vcodec="libx264", crf=0)
+ .overwrite_output()
+ .run_async(pipe_stdin=True)
+ )
+ self.path = path
+ self.shape = shape
+ self.frames = 0
+
+ def writeFrame(self, frame):
+ """Appends a video frame to the file.
+
+ Args:
+ frame (np.ndarray): RGB image of shape `shape + (3,)`, where
+ `shape` is the constructor arg.
+ """
+ assert len(frame.shape) == 3
+ assert frame.shape[2] == 3
+ if frame.shape[:2] != self.shape:
+ msg = (
+ "Rendered frame shape changed after video output started: "
+ "from {} to {}.".format(self.shape, frame.shape[:2])
+ )
+ raise ValueError(msg)
+ framebytes = frame.astype(np.uint8).tobytes()
+ self.ffmpegProcess.stdin.write(framebytes)
+ self.frames += 1
+
+ def close(self):
+ """Closes the video output and prints a status message to console."""
+ self.ffmpegProcess.stdin.close()
+ self.ffmpegProcess.wait()
+ self.ffmpegProcess = None
+ print("wrote {} frames to {}".format(self.frames, self.path))
+
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/__init__.py b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/__init__.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/__init__.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..db3c2253d715636459692eab7fae733b9b2a866f
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/__init__.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/crazyflie2.obj.gz b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/crazyflie2.obj.gz
new file mode 100644
index 0000000000000000000000000000000000000000..df8427c34759b6bec62bee56943a0f8d88a042b5
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/crazyflie2.obj.gz differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visMatplotlib.py b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visMatplotlib.py
new file mode 100644
index 0000000000000000000000000000000000000000..83eeb0326392b0ba1b5cecad5850763cd3f91c99
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visMatplotlib.py
@@ -0,0 +1,81 @@
+import warnings
+
+from mpl_toolkits.mplot3d import Axes3D
+from mpl_toolkits.mplot3d.art3d import Line3DCollection
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+class VisMatplotlib:
+ def __init__(self):
+ self.fig = plt.figure()
+ self.ax = self.fig.add_subplot(111, projection='3d')
+ self.ax.set_xlim([-5, 5])
+ self.ax.set_ylim([-5, 5])
+ self.ax.set_zlim([0, 3])
+ self.ax.set_xlabel("X")
+ self.ax.set_ylabel("Y")
+ self.ax.set_zlabel("Z")
+ self.plot = None
+ self.timeAnnotation = self.ax.annotate("Time", xy=(0, 0), xycoords='axes fraction', fontsize=12, ha='right', va='bottom')
+
+ self.line_color = 0.3 * np.ones(3)
+
+ # Lazy-constructed data for connectivity graph gfx.
+ self.graph_edges = None
+ self.graph_lines = None
+ self.graph = None
+
+ def setGraph(self, edges):
+ """Set edges of graph visualization - sequence of (i,j) tuples."""
+
+ # Only allocate new memory if we need to.
+ n_edges = len(edges)
+ if self.graph_edges is None or n_edges != len(self.graph_edges):
+ self.graph_lines = np.zeros((n_edges, 2, 3))
+ self.graph_edges = edges
+
+ # Lazily construct Matplotlib object for graph.
+ if self.graph is None:
+ self.graph = Line3DCollection(self.graph_lines, edgecolor=self.line_color)
+ self.ax.add_collection(self.graph)
+
+ def showEllipsoids(self, radii):
+ warnings.warn("showEllipsoids not implemented in Matplotlib visualizer.")
+
+ def update(self, t, crazyflies):
+ xs = []
+ ys = []
+ zs = []
+ cs = []
+ for cf in crazyflies:
+ x, y, z = cf.position()
+ color = cf.ledRGB
+ xs.append(x)
+ ys.append(y)
+ zs.append(z)
+ cs.append(color)
+
+ if self.plot is None:
+ self.plot = self.ax.scatter(xs, ys, zs, c=cs)
+ else:
+ # TODO: Don't use protected members.
+ self.plot._offsets3d = (xs, ys, zs)
+ self.plot.set_facecolors(cs)
+ self.plot.set_edgecolors(cs)
+ self.plot._facecolor3d = self.plot.get_facecolor()
+ self.plot._edgecolor3d = self.plot.get_edgecolor()
+
+ if self.graph is not None:
+ # Update graph line segments to match new Crazyflie positions.
+ for k, (i, j) in enumerate(self.graph_edges):
+ self.graph_lines[k, 0, :] = xs[i], ys[i], zs[i]
+ self.graph_lines[k, 1, :] = xs[j], ys[j], zs[j]
+ self.graph.set_segments(self.graph_lines)
+
+ self.timeAnnotation.set_text("{} s".format(t))
+ plt.pause(0.0001)
+
+ def render(self):
+ warnings.warn("Rendering video not supported in VisMatplotlib yet.")
+ return None
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visNull.py b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visNull.py
new file mode 100644
index 0000000000000000000000000000000000000000..6073056fe09f3138b5b8d3710e70e639463a4136
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visNull.py
@@ -0,0 +1,17 @@
+"""No-op visualizer for real hardware runs, so script can be oblivious."""
+
+class VisNull:
+ def __init__(self):
+ pass
+
+ def setGraph(self, edges):
+ pass
+
+ def showEllipsoids(self, radii):
+ pass
+
+ def update(self, t, crazyflies):
+ pass
+
+ def render(self):
+ return None
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visNull.pyc b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visNull.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..8b00544e3c9cf6bffdbd3e2823417a45a34fa33b
Binary files /dev/null and b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visNull.pyc differ
diff --git a/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visVispy.py b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visVispy.py
new file mode 100644
index 0000000000000000000000000000000000000000..c329b669f490911ef386a646ebcb7b7bf24c6337
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pycrazyswarm/visualizer/visVispy.py
@@ -0,0 +1,167 @@
+import os
+import math
+
+import ffmpeg
+import numpy as np
+from vispy import scene, app, io, geometry
+from vispy.color import Color
+from vispy.visuals import transforms
+from vispy.scene.cameras import TurntableCamera
+
+from .. import util as util
+
+
+CF_MESH_PATH = os.path.join(os.path.dirname(__file__), "crazyflie2.obj.gz")
+# Convert millimeters to meters, but make twice as big so easier to see.
+MESHFILE_SCALE = 2.0 * 0.001
+# The matrix that rotates the coordinates of the .obj file to agree with the
+# Crazyflie's standard coordinate system. VisPy uses [row vector] * [matrix]
+# (like DirectX), so this is the transpose of what we would expect.
+UNROT_MESHFILE_TRANSPOSE = MESHFILE_SCALE * np.array([
+ [-1, 0, 0],
+ [ 0, 0, 1],
+ [ 0, -1, 0],
+])
+ELLIPSOID_COLOR_OK = Color("#11FF22", alpha=0.1)
+ELLIPSOID_COLOR_COLLISION = Color("#FF0000", alpha=0.1)
+
+
+class VisVispy:
+ def __init__(self, show=True, resizable=True):
+ self.canvas = scene.SceneCanvas(
+ keys='interactive', size=(1024, 768), show=show, config=dict(samples=4), resizable=resizable
+ )
+
+ self.plane_color = 0.25 * np.ones((1, 3))
+ self.bg_color = 0.9 * np.ones((1, 3))
+ self.line_color = 0.7 * np.ones((1, 3))
+
+ # Set up a viewbox to display the cube with interactive arcball
+ self.view = self.canvas.central_widget.add_view()
+ self.view.bgcolor = self.bg_color
+ self.view.camera = TurntableCamera(
+ fov=30.0, elevation=30.0, azimuth=90.0, center=(0.0, 0.0, 1.25)
+ )
+ self.cam_state = self.view.camera.get_state()
+
+ # add a colored 3D axis for orientation
+ axis = scene.visuals.XYZAxis(parent=self.view.scene)
+ self.cfs = []
+ self.led_color_cache = []
+
+ ground = scene.visuals.Plane(
+ 32.0, 32.0, direction="+z", color=self.plane_color, parent=self.view.scene
+ )
+
+ # Lazy-constructed vispy objects and data for connectivity graph gfx.
+ self.graph_edges = None
+ self.graph_lines = None
+ self.graph = None
+
+ # Lazy-constructed vispy objects for collision ellipsoids.
+ self.ellipsoids = None
+ self.ellipsoid_radii = None
+
+ def setGraph(self, edges):
+ """Set edges of graph visualization - sequence of (i,j) tuples."""
+
+ # Only allocate new memory if we need to.
+ n_edges = len(edges)
+ if self.graph_edges is None or n_edges != len(self.graph_edges):
+ self.graph_lines = np.zeros((2 * n_edges, 3))
+ self.graph_edges = edges
+
+ # Lazily construct VisPy object for graph.
+ if self.graph is None:
+ self.graph = scene.visuals.Line(
+ parent=self.view.scene,
+ color=self.line_color,
+ pos=self.graph_lines,
+ connect="segments",
+ method="gl",
+ antialias=True,
+ )
+
+ def showEllipsoids(self, radii):
+ self.ellipsoid_radii = np.array(radii)
+
+ def update(self, t, crazyflies):
+ if len(self.cfs) == 0:
+ verts, faces, normals, nothin = io.read_mesh(CF_MESH_PATH)
+ for i, cf in enumerate(crazyflies):
+ color = cf.ledRGB
+ mesh = scene.visuals.Mesh(
+ parent=self.view.scene,
+ vertices=verts,
+ faces=faces,
+ color=color,
+ shading="smooth",
+ )
+ mesh.unfreeze()
+ mesh.light_dir = (0.1, 0.1, 1.0)
+ mesh.shininess = 0.01
+ mesh.ambient_light_color = [0.5] * 3
+ mesh.transform = transforms.MatrixTransform()
+ self.cfs.append(mesh)
+ self.led_color_cache.append(color)
+
+ if self.ellipsoid_radii is not None and self.ellipsoids is None:
+ sphere_mesh = geometry.create_sphere(radius=1.0)
+ self.ellipsoids = [
+ scene.visuals.Mesh(
+ parent=self.view.scene,
+ meshdata=sphere_mesh,
+ color=ELLIPSOID_COLOR_OK,
+ shading="smooth",
+ )
+ for _ in self.cfs
+ ]
+ for ell in self.ellipsoids:
+ ell.light_dir = (0.1, 0.1, 1.0)
+ ell.shininess = 0.0
+ ell.ambient_light_color = [0.5] * 3
+ ell.transform = transforms.MatrixTransform()
+
+ positions = np.stack([cf.position() for cf in crazyflies])
+
+ for i in range(0, len(self.cfs)):
+ R_state = crazyflies[i].rotBodyToWorld()
+ # Recall VisPy uses [row vector] * [matrix]!!
+ T = np.eye(4)
+ T[:3, :3] = np.dot(UNROT_MESHFILE_TRANSPOSE, R_state.T)
+ T[3, :3] = positions[i]
+ self.cfs[i].transform = transforms.MatrixTransform(T)
+ # vispy does not do this check
+ color = crazyflies[i].ledRGB
+ if color != self.led_color_cache[i]:
+ self.led_color_cache[i] = color
+ self.cfs[i].color = color # sets dirty flag
+
+ # Update graph line segments to match new Crazyflie positions.
+ if self.graph is not None:
+ for k, (i, j) in enumerate(self.graph_edges):
+ self.graph_lines[2 * k, :] = positions[i]
+ self.graph_lines[2 * k + 1, :] = positions[j]
+ self.graph.set_data(self.graph_lines)
+
+ # Update collsiion ellipsoids.
+ if self.ellipsoids is not None:
+ colliding = util.check_ellipsoid_collisions(positions, self.ellipsoid_radii)
+ for i, pos in enumerate(positions):
+ ell = self.ellipsoids[i]
+ tf = ell.transform
+ tf.reset()
+ tf.scale(self.ellipsoid_radii)
+ tf.translate(pos)
+ new_color = ELLIPSOID_COLOR_COLLISION if colliding[i] else ELLIPSOID_COLOR_OK
+ if not (new_color == ell.color): # vispy Color lacks != override.
+ ell.color = new_color
+
+ self.canvas.app.process_events()
+
+ def render(self):
+ frame = self.canvas.render()
+ # Do not render alpha channel - we always use rgb24 format.
+ if frame.shape[2] == 4:
+ frame = frame[:, :, :3]
+ return frame
diff --git a/real_robot/crazyswarm/scripts/pytest.ini b/real_robot/crazyswarm/scripts/pytest.ini
new file mode 100644
index 0000000000000000000000000000000000000000..7aee21b00a64bdcd8f9463c2058f04ef2ced1214
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/pytest.ini
@@ -0,0 +1,3 @@
+[pytest]
+markers =
+ ros: tests that can only run when ROS is installed.
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/1.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..aa9168796daca792dd7b263a35c71cc8ff1b068a
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/1.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-1.500000,0.000000,0.000000,0.000000,0.017623,-0.018891,0.008684,-0.001582,0.000000,0.000000,0.000000,0.000000,0.009122,-0.009778,0.004495,-0.000819,1.500000,0.000000,0.000000,0.000000,-0.024018,0.025745,-0.011835,0.002156,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.494166,0.017070,0.013874,-0.000096,-0.001931,0.001595,-0.000669,0.000119,0.003020,0.008837,0.007183,-0.000049,-0.001000,0.000826,-0.000346,0.000061,1.492048,-0.023267,-0.018913,0.000129,0.002632,-0.002174,0.000912,-0.000162,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.464205,0.041593,0.010399,-0.001106,0.000155,-0.000107,0.000041,-0.000007,0.018532,0.021536,0.005386,-0.000572,0.000080,-0.000055,0.000021,-0.000004,1.451206,-0.056703,-0.014180,0.001507,-0.000211,0.000146,-0.000056,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.413237,0.059354,0.007409,-0.000979,-0.000010,0.000006,-0.000002,0.000000,0.044924,0.030737,0.003839,-0.000507,-0.000005,0.000003,-0.000001,0.000000,1.381719,-0.080927,-0.010108,0.001335,0.000013,-0.000008,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.347459,0.071215,0.004447,-0.000990,0.000001,-0.000000,0.000000,-0.000000,0.078990,0.036883,0.002305,-0.000513,0.000000,-0.000000,0.000000,-0.000000,1.292027,-0.097110,-0.006069,0.001349,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.272786,0.077142,0.001480,-0.000989,0.000000,-0.000000,0.000000,-0.000000,0.117665,0.039956,0.000768,-0.000512,-0.000000,0.000000,-0.000000,0.000000,1.190197,-0.105201,-0.002023,0.001348,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.195152,0.077137,-0.001485,-0.000988,0.000000,-0.000000,0.000000,-0.000000,0.157877,0.039956,-0.000768,-0.000512,0.000000,-0.000000,0.000000,-0.000000,1.084322,-0.105201,0.002023,0.001348,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.120489,0.071201,-0.004451,-0.000989,-0.000000,0.000001,-0.000000,0.000000,0.196552,0.036882,-0.002305,-0.000513,-0.000000,0.000000,-0.000000,0.000000,0.982493,-0.097110,0.006069,0.001349,0.000001,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.054729,0.059333,-0.007412,-0.000978,0.000010,-0.000009,0.000009,-0.000007,0.230617,0.030736,-0.003839,-0.000507,0.000005,-0.000005,0.000005,-0.000004,0.892801,-0.080927,0.010108,0.001335,-0.000013,0.000012,-0.000012,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.003783,0.041572,-0.010397,-0.001105,-0.000154,0.000069,-0.000160,0.000118,0.257008,0.021536,-0.005386,-0.000572,-0.000080,0.000036,-0.000083,0.000061,0.823314,-0.056702,0.014180,0.001507,0.000211,-0.000094,0.000219,-0.000162,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.973840,0.017058,-0.013866,-0.000095,0.001930,-0.000003,0.002386,-0.001580,0.272520,0.008837,-0.007183,-0.000049,0.001000,-0.000002,0.001236,-0.000819,0.782472,-0.023267,0.018912,0.000129,-0.002632,0.000004,-0.003254,0.002156,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/10.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/10.csv
new file mode 100644
index 0000000000000000000000000000000000000000..8a924c7d281bb746f482910c0bcf9f5e1eeb59af
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/10.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/11.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/11.csv
new file mode 100644
index 0000000000000000000000000000000000000000..1347e189718616cf111d5aa0407905544a30bbd4
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/11.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,-0.928930,0.000000,0.000000,0.000000,0.018359,-0.019622,0.009008,-0.001639,-0.253540,0.000000,0.000000,0.000000,0.004469,-0.004774,0.002191,-0.000399,1.247200,0.000000,0.000000,0.000000,0.015019,-0.016055,0.007371,-0.001342,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.922824,0.017899,0.014630,0.000002,-0.002006,0.001652,-0.000692,0.000122,-0.252053,0.004361,0.003568,0.000004,-0.000488,0.000402,-0.000168,0.000030,1.252194,0.014637,0.011960,-0.000003,-0.001641,0.001352,-0.000566,0.000100,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.891215,0.044111,0.011317,-0.001053,0.000160,-0.000110,0.000043,-0.000007,-0.244345,0.010765,0.002772,-0.000253,0.000039,-0.000027,0.000010,-0.000002,1.278033,0.036048,0.009234,-0.000866,0.000131,-0.000090,0.000035,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.836756,0.063878,0.008498,-0.000923,-0.000011,0.000006,-0.000002,0.000000,-0.231040,0.015620,0.002094,-0.000223,-0.000003,0.000001,-0.000001,0.000000,1.322518,0.052156,0.006914,-0.000759,-0.000008,0.000005,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.765309,0.078082,0.005700,-0.000936,-0.000000,-0.000000,-0.000000,0.000000,-0.213551,0.019132,0.001416,-0.000228,-0.000001,0.000000,-0.000000,0.000000,1.380824,0.063690,0.004615,-0.000769,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.682465,0.086670,0.002888,-0.000939,-0.000001,0.000000,-0.000000,0.000000,-0.193230,0.021280,0.000730,-0.000230,-0.000001,0.000000,-0.000000,0.000000,1.448360,0.070613,0.002309,-0.000769,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.593847,0.089625,0.000065,-0.000943,-0.000001,0.000000,-0.000000,0.000000,-0.171452,0.022047,0.000036,-0.000233,-0.000001,0.000000,-0.000000,0.000000,1.520513,0.072924,0.000002,-0.000769,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.505101,0.086921,-0.002772,-0.000948,-0.000001,0.000000,-0.000000,0.000000,-0.149603,0.021418,-0.000666,-0.000235,-0.000001,0.000000,-0.000000,0.000000,1.592671,0.070623,-0.002304,-0.000769,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.421901,0.078526,-0.005626,-0.000954,-0.000002,0.000001,-0.000001,0.000000,-0.129087,0.019378,-0.001375,-0.000238,-0.000001,0.000000,-0.000000,0.000000,1.660220,0.063706,-0.004612,-0.000769,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.349956,0.064410,-0.008488,-0.000948,0.000009,-0.000009,0.000009,-0.000007,-0.111322,0.015914,-0.002088,-0.000236,0.000002,-0.000002,0.000002,-0.000002,1.718546,0.052177,-0.006914,-0.000760,0.000008,-0.000008,0.000007,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.294981,0.044583,-0.011389,-0.001084,-0.000163,0.000072,-0.000168,0.000124,-0.097732,0.011027,-0.002812,-0.000270,-0.000040,0.000018,-0.000042,0.000031,1.763050,0.036065,-0.009237,-0.000867,-0.000131,0.000058,-0.000135,0.000100,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.263005,0.018126,-0.014801,-0.000016,0.002034,-0.000000,0.002505,-0.001663,-0.089820,0.004487,-0.003662,-0.000006,0.000504,-0.000000,0.000621,-0.000412,1.788903,0.014646,-0.011966,-0.000004,0.001642,0.000000,0.002021,-0.001343,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/12.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/12.csv
new file mode 100644
index 0000000000000000000000000000000000000000..16abb3d4f2e46137539e5f7a1c2359a9fce9d4a8
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/12.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.256820,0.000000,0.000000,0.000000,0.002673,-0.002869,0.001320,-0.000241,-0.088289,0.000000,0.000000,0.000000,0.025135,-0.027029,0.012443,-0.002268,1.793900,0.000000,0.000000,0.000000,-0.040169,0.043197,-0.019886,0.003625,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.255937,0.002580,0.002090,-0.000023,-0.000294,0.000243,-0.000102,0.000018,-0.080009,0.024173,0.019525,-0.000290,-0.002765,0.002288,-0.000962,0.000171,1.780668,-0.038628,-0.031198,0.000467,0.004418,-0.003657,0.001537,-0.000273,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.251425,0.006246,0.001537,-0.000177,0.000023,-0.000016,0.000006,-0.000001,-0.037869,0.058157,0.014105,-0.001729,0.000222,-0.000154,0.000060,-0.000010,1.713334,-0.092923,-0.022530,0.002766,-0.000355,0.000246,-0.000096,0.000017,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.243806,0.008830,0.001053,-0.000160,-0.000002,0.000001,-0.000000,0.000000,0.032781,0.081583,0.009387,-0.001549,-0.000014,0.000008,-0.000003,0.000001,1.600460,-0.130330,-0.014984,0.002477,0.000023,-0.000013,0.000005,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.234086,0.010447,0.000561,-0.000167,-0.000001,-0.000000,-0.000000,-0.000000,0.122194,0.095680,0.004703,-0.001565,0.000000,-0.000000,0.000000,-0.000000,1.457637,-0.152817,-0.007493,0.002502,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.223246,0.011063,0.000052,-0.000173,-0.000002,-0.000000,0.000000,-0.000000,0.221013,0.100392,0.000009,-0.001565,-0.000000,-0.000000,-0.000000,-0.000000,1.299827,-0.160302,0.000008,0.002500,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.212305,0.010643,-0.000475,-0.000179,-0.000002,0.000000,0.000000,0.000000,0.319849,0.095713,-0.004689,-0.001567,-0.000001,0.000001,-0.000001,0.000001,1.142033,-0.152787,0.007507,0.002500,0.000001,-0.000002,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.202316,0.009152,-0.001018,-0.000181,0.000001,-0.000001,0.000001,-0.000001,0.409306,0.081636,-0.009382,-0.001552,0.000014,-0.000013,0.000013,-0.000010,0.999251,-0.130281,0.014989,0.002474,-0.000023,0.000021,-0.000021,0.000017,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.194363,0.006570,-0.001573,-0.000204,-0.000025,0.000012,-0.000027,0.000020,0.480010,0.058209,-0.014111,-0.001734,-0.000222,0.000101,-0.000233,0.000171,0.886428,-0.092874,0.022524,0.002762,0.000354,-0.000160,0.000371,-0.000272,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.189591,0.002745,-0.002211,-0.000040,0.000315,-0.000001,0.000393,-0.000259,0.522190,0.024199,-0.019544,-0.000293,0.002768,-0.000010,0.003441,-0.002271,0.819134,-0.038603,0.031180,0.000464,-0.004415,0.000016,-0.005488,0.003623,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/13.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/13.csv
new file mode 100644
index 0000000000000000000000000000000000000000..b00f782c3871af3b01783a5a12ba26f3a403f38e
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/13.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.188650,0.000000,0.000000,0.000000,-0.007257,0.007812,-0.003598,0.000656,0.530480,0.000000,0.000000,0.000000,-0.036547,0.039308,-0.018097,0.003299,0.805910,0.000000,0.000000,-0.000000,0.034481,-0.037077,0.017068,-0.003112,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.191037,-0.006964,-0.005614,0.000097,0.000799,-0.000662,0.000278,-0.000049,0.518443,-0.035134,-0.028368,0.000434,0.004021,-0.003328,0.001399,-0.000248,0.817270,0.033165,0.026791,-0.000395,-0.003792,0.003139,-0.001319,0.000234,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.203151,-0.016689,-0.004009,0.000512,-0.000065,0.000044,-0.000017,0.000003,0.457218,-0.084470,-0.020453,0.002526,-0.000323,0.000224,-0.000087,0.000015,0.875094,0.079812,0.019368,-0.002369,0.000304,-0.000211,0.000082,-0.000014,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.223372,-0.023290,-0.002614,0.000455,0.000002,-0.000003,0.000001,-0.000000,0.354650,-0.118387,-0.013562,0.002261,0.000019,-0.000012,0.000004,-0.000001,0.972065,0.111994,0.012905,-0.002123,-0.000020,0.000011,-0.000004,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.248821,-0.027156,-0.001256,0.000447,-0.000003,0.000001,-0.000000,0.000000,0.224972,-0.138692,-0.006736,0.002277,-0.000003,0.000000,0.000000,-0.000000,1.094828,0.131389,0.006480,-0.002148,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.276789,-0.028337,0.000070,0.000438,-0.000001,0.000000,-0.000000,0.000000,0.081818,-0.145344,0.000080,0.002267,-0.000003,-0.000000,0.000000,-0.000000,1.230548,0.137902,0.000032,-0.002151,-0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.304619,-0.026887,0.001377,0.000433,-0.000001,-0.000000,0.000000,-0.000000,-0.061182,-0.138394,0.006866,0.002258,-0.000001,-0.000002,0.000001,-0.000001,1.366330,0.131508,-0.006428,-0.002156,-0.000002,0.000002,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.329697,-0.022838,0.002669,0.000425,-0.000005,0.000004,-0.000004,0.000003,-0.190454,-0.117900,0.013615,0.002229,-0.000022,0.000019,-0.000019,0.000015,1.489254,0.112188,-0.012884,-0.002136,0.000019,-0.000018,0.000018,-0.000014,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.349443,-0.016227,0.003960,0.000473,0.000061,-0.000028,0.000064,-0.000047,-0.292516,-0.083982,0.020398,0.002486,0.000320,-0.000145,0.000335,-0.000246,1.586427,0.080006,-0.019390,-0.002385,-0.000305,0.000138,-0.000320,0.000235,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.361187,-0.006727,0.005441,0.000072,-0.000768,0.000003,-0.000953,0.000630,-0.353351,-0.034886,0.028186,0.000409,-0.003988,0.000014,-0.004956,0.003272,1.644405,0.033264,-0.026863,-0.000405,0.003805,-0.000014,0.004731,-0.003123,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/14.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/14.csv
new file mode 100644
index 0000000000000000000000000000000000000000..e193cb9ce5723315e3d12e4fcebbe764ffe514ed
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/14.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.363490,0.000000,0.000000,0.000000,-0.006351,0.006841,-0.003152,0.000575,-0.365300,0.000000,0.000000,-0.000000,0.046869,-0.050397,0.023200,-0.004229,1.655800,0.000000,0.000000,0.000000,-0.021876,0.023532,-0.010835,0.001975,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.365577,-0.006086,-0.004899,0.000093,0.000700,-0.000580,0.000244,-0.000043,-0.349858,0.045081,0.036417,-0.000536,-0.005155,0.004266,-0.001793,0.000318,1.648597,-0.021023,-0.016970,0.000266,0.002407,-0.001993,0.000838,-0.000149,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.376149,-0.014545,-0.003471,0.000456,-0.000056,0.000039,-0.000015,0.000003,-0.271259,0.108489,0.026329,-0.003219,0.000414,-0.000287,0.000112,-0.000019,1.611973,-0.050515,-0.012213,0.001518,-0.000193,0.000134,-0.000052,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.393737,-0.020219,-0.002220,0.000411,0.000004,-0.000002,0.000001,-0.000000,-0.139442,0.152243,0.017550,-0.002882,-0.000026,0.000015,-0.000005,0.000001,1.550661,-0.070737,-0.008068,0.001361,0.000012,-0.000007,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.415764,-0.023420,-0.000979,0.000415,0.000000,0.000000,-0.000000,-0.000000,0.027453,0.178641,0.008836,-0.002911,0.000001,-0.000000,0.000000,-0.000000,1.473225,-0.082764,-0.003953,0.001375,-0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.439748,-0.024134,0.000264,0.000411,-0.000005,-0.000002,0.000000,-0.000000,0.212019,0.187582,0.000106,-0.002911,-0.000002,-0.000001,0.000000,-0.000000,1.387883,-0.086547,0.000168,0.001371,-0.000003,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.463214,-0.022404,0.001444,0.000367,-0.000019,-0.000004,0.000003,-0.000000,0.396794,0.179049,-0.008650,-0.002931,-0.000009,0.000001,-0.000000,0.000001,1.302871,-0.082115,0.004250,0.001344,-0.000012,-0.000004,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.483828,-0.018500,0.002417,0.000286,-0.000014,0.000009,-0.000004,0.000002,0.564254,0.152927,-0.017474,-0.002932,0.000023,-0.000022,0.000024,-0.000019,1.226335,-0.069641,0.008193,0.001282,-0.000019,0.000015,-0.000012,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.499632,-0.012827,0.003267,0.000316,0.000047,-0.000021,0.000048,-0.000036,0.696780,0.109168,-0.026411,-0.003274,-0.000417,0.000189,-0.000437,0.000321,1.166162,-0.049419,0.012083,0.001429,0.000188,-0.000085,0.000196,-0.000144,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.508838,-0.005222,0.004261,0.000008,-0.000587,0.000000,-0.000723,0.000480,0.775918,0.045422,-0.036669,-0.000569,0.005199,-0.000020,0.006466,-0.004267,1.130409,-0.020473,0.016563,0.000212,-0.002335,0.000007,-0.002898,0.001915,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/15.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/15.csv
new file mode 100644
index 0000000000000000000000000000000000000000..cd3315562ae1633292d0d62615fdf2c69f84f8e6
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/15.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.510620,0.000000,0.000000,0.000000,0.008697,-0.009352,0.004305,-0.000785,0.791480,0.000000,0.000000,0.000000,-0.045367,0.048787,-0.022459,0.004095,1.123400,0.000000,0.000000,0.000000,-0.004625,0.004974,-0.002290,0.000417,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.507755,0.008365,0.006757,-0.000100,-0.000957,0.000792,-0.000333,0.000059,0.776535,-0.043628,-0.035237,0.000526,0.004990,-0.004130,0.001736,-0.000308,1.121877,-0.004446,-0.003590,0.000055,0.000509,-0.000421,0.000177,-0.000031,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.493171,0.020128,0.004884,-0.000598,0.000077,-0.000053,0.000021,-0.000004,0.700484,-0.104955,-0.025450,0.003123,-0.000400,0.000278,-0.000108,0.000019,1.114128,-0.010692,-0.002590,0.000319,-0.000041,0.000028,-0.000011,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.468716,0.028242,0.003253,-0.000536,-0.000005,0.000003,-0.000001,0.000000,0.572991,-0.147213,-0.016929,0.002797,0.000026,-0.000015,0.000005,-0.000001,1.101145,-0.014987,-0.001719,0.000286,0.000002,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.437760,0.033130,0.001632,-0.000542,-0.000000,-0.000000,0.000000,-0.000000,0.411662,-0.172625,-0.008471,0.002825,-0.000001,0.000001,-0.000000,0.000000,1.084726,-0.017562,-0.000856,0.000288,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.403539,0.034769,0.000006,-0.000542,-0.000000,-0.000000,0.000000,-0.000000,0.233391,-0.181095,0.000000,0.002823,-0.000000,0.000000,-0.000000,0.000000,1.066595,-0.018412,0.000006,0.000287,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.369307,0.033153,-0.001622,-0.000543,-0.000000,0.000000,-0.000000,0.000000,0.055119,-0.172625,0.008471,0.002825,0.000001,-0.000002,0.000001,-0.000001,1.048475,-0.017541,0.000865,0.000286,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.338319,0.028280,-0.003249,-0.000538,0.000005,-0.000005,0.000004,-0.000004,-0.106210,-0.147213,0.016929,0.002797,-0.000026,0.000024,-0.000023,0.000019,1.032086,-0.014953,0.001722,0.000283,-0.000003,0.000002,-0.000002,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.313825,0.020166,-0.004888,-0.000601,-0.000077,0.000035,-0.000081,0.000059,-0.233703,-0.104955,0.025449,0.003123,0.000400,-0.000181,0.000420,-0.000308,1.019138,-0.010657,0.002586,0.000316,0.000041,-0.000018,0.000043,-0.000031,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.299212,0.008384,-0.006771,-0.000102,0.000959,-0.000004,0.001192,-0.000787,-0.309755,-0.043628,0.035237,0.000526,-0.004990,0.000018,-0.006203,0.004095,1.011417,-0.004429,0.003577,0.000053,-0.000506,0.000002,-0.000629,0.000416,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/16.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/16.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0229b56361e7bd11a4ee139c395752683384b7d2
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/16.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.296340,0.000000,0.000000,0.000000,0.024896,-0.026775,0.012327,-0.002247,-0.324700,0.000000,0.000000,0.000000,0.027538,-0.029612,0.013632,-0.002485,1.009900,0.000000,0.000000,0.000000,0.029305,-0.031514,0.014507,-0.002645,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.288140,0.023937,0.019330,-0.000293,-0.002739,0.002267,-0.000953,0.000169,-0.315628,0.026485,0.021394,-0.000317,-0.003029,0.002507,-0.001054,0.000187,1.019554,0.028183,0.022763,-0.000339,-0.003223,0.002668,-0.001121,0.000199,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.246421,0.057564,0.013946,-0.001718,0.000220,-0.000153,0.000059,-0.000010,-0.269454,0.063729,0.015461,-0.001893,0.000243,-0.000169,0.000066,-0.000011,1.068683,0.067803,0.016443,-0.002017,0.000259,-0.000180,0.000070,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.176513,0.080702,0.009258,-0.001539,-0.000014,0.000008,-0.000003,0.000000,-0.192028,0.089414,0.010297,-0.001695,-0.000016,0.000009,-0.000003,0.000001,1.151049,0.095110,0.010942,-0.001806,-0.000017,0.000009,-0.000003,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.088099,0.094573,0.004607,-0.001553,0.000001,-0.000000,0.000000,0.000000,-0.094022,0.104890,0.005172,-0.001712,0.000001,-0.000000,-0.000000,-0.000000,1.255286,0.111541,0.005480,-0.001826,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.009529,0.099134,-0.000045,-0.001548,0.000002,0.000000,-0.000000,-0.000000,0.014329,0.110099,0.000037,-0.001713,-0.000002,-0.000001,0.000000,-0.000000,1.370480,0.117022,0.000001,-0.001826,0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.107073,0.094410,-0.004675,-0.001539,0.000001,0.000001,-0.000000,0.000000,0.122750,0.105026,-0.005117,-0.001724,-0.000003,0.000002,-0.000001,0.000001,1.485678,0.111547,-0.005476,-0.001826,-0.000000,0.000001,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.195270,0.080450,-0.009278,-0.001522,0.000014,-0.000013,0.000013,-0.000010,0.220935,0.089620,-0.010282,-0.001709,0.000016,-0.000015,0.000014,-0.000011,1.589924,0.095123,-0.010940,-0.001807,0.000017,-0.000015,0.000015,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.264923,0.057323,-0.013914,-0.001700,-0.000219,0.000099,-0.000229,0.000168,0.298567,0.063924,-0.015488,-0.001908,-0.000244,0.000110,-0.000256,0.000188,1.672304,0.067816,-0.016445,-0.002018,-0.000259,0.000117,-0.000271,0.000199,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.306451,0.023818,-0.019241,-0.000282,0.002723,-0.000010,0.003385,-0.002234,0.344894,0.026581,-0.021465,-0.000325,0.003041,-0.000011,0.003781,-0.002496,1.721443,0.028190,-0.022768,-0.000340,0.003224,-0.000012,0.004008,-0.002646,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/17.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/17.csv
new file mode 100644
index 0000000000000000000000000000000000000000..f1cd81b9bae85d3ffcdf5866907bf9ea6cee6a71
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/17.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/18.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/18.csv
new file mode 100644
index 0000000000000000000000000000000000000000..07977da3f1d410993f5781500b85d1e4b5efcc8f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/18.csv
@@ -0,0 +1,8 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/19.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/19.csv
new file mode 100644
index 0000000000000000000000000000000000000000..23548ffef1b62e361f95f544490a185a81269e3c
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/19.csv
@@ -0,0 +1,20 @@
+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,
+1.000000,0.084563,0.000000,0.000000,0.000000,-0.017594,0.018648,-0.008529,0.001548,-0.483880,0.000000,0.000000,0.000000,0.008489,-0.009112,0.004191,-0.000764,2.261100,0.000000,0.000000,0.000000,-0.022098,0.023309,-0.010637,0.001928,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.078636,-0.017472,-0.014504,-0.000283,0.001904,-0.001559,0.000649,-0.000114,-0.481075,0.008198,0.006645,-0.000069,-0.000933,0.000770,-0.000324,0.000057,2.253602,-0.022173,-0.018562,-0.000555,0.002381,-0.001940,0.000807,-0.000142,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.047257,-0.044415,-0.012186,0.000721,-0.000158,0.000102,-0.000040,0.000007,-0.466731,0.019860,0.004893,-0.000563,0.000072,-0.000051,0.000020,-0.000003,2.213417,-0.057292,-0.016219,0.000741,-0.000181,0.000128,-0.000048,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.008711,-0.066931,-0.010395,0.000562,0.000002,-0.000003,0.000002,-0.000000,-0.442504,0.028083,0.003347,-0.000514,-0.000008,0.000003,0.000000,0.000001,2.140554,-0.087827,-0.014363,0.000609,0.000017,-0.000008,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.085475,-0.086033,-0.008705,0.000568,0.000007,0.000005,-0.000002,-0.000000,-0.411593,0.033224,0.001805,-0.000485,0.000039,0.000024,-0.000004,-0.000001,2.038983,-0.114687,-0.012482,0.000640,0.000008,0.000005,-0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.179634,-0.101696,-0.006936,0.000608,0.000002,-0.000008,0.000003,-0.000000,-0.376990,0.035627,0.000748,-0.000198,0.000069,-0.000016,-0.000001,0.000001,1.912465,-0.137683,-0.010485,0.000696,0.000014,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.287661,-0.113759,-0.005144,0.000578,-0.000012,-0.000001,0.000001,-0.000000,-0.340761,0.036724,0.000410,-0.000073,0.000003,-0.000006,0.000003,-0.000000,1.765005,-0.156519,-0.008331,0.000736,0.000009,-0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.405998,-0.122358,-0.003476,0.000537,-0.000007,0.000002,-0.000000,-0.000000,-0.303701,0.037318,0.000178,-0.000084,-0.000003,0.000001,-0.000000,0.000000,1.600900,-0.170941,-0.006079,0.000761,0.000003,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.531300,-0.127719,-0.001893,0.000521,-0.000002,0.000000,-0.000000,-0.000000,-0.266291,0.037414,-0.000085,-0.000090,-0.000001,0.000000,-0.000000,-0.000000,1.424642,-0.180808,-0.003785,0.000766,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.660394,-0.129953,-0.000344,0.000513,-0.000002,0.000000,0.000000,-0.000000,-0.229052,0.036972,-0.000359,-0.000093,-0.000001,-0.000000,0.000000,-0.000000,1.240815,-0.186079,-0.001485,0.000767,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.790180,-0.129109,0.001184,0.000506,-0.000001,0.000000,0.000000,-0.000000,-0.192533,0.035970,-0.000645,-0.000098,-0.000001,-0.000000,0.000000,-0.000000,1.054018,-0.186746,0.000819,0.000769,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.917600,-0.125228,0.002695,0.000501,-0.000001,-0.000000,0.000000,-0.000000,-0.157307,0.034382,-0.000945,-0.000103,-0.000001,-0.000000,0.000000,-0.000000,0.868860,-0.182801,0.003127,0.000770,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.039633,-0.118341,0.004189,0.000494,-0.000002,-0.000000,0.000000,-0.000000,-0.123975,0.032178,-0.001262,-0.000109,-0.000002,-0.000000,0.000000,-0.000000,0.689956,-0.174235,0.005441,0.000772,0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.153293,-0.108489,0.005658,0.000485,-0.000003,-0.000000,0.000000,-0.000000,-0.093169,0.029321,-0.001598,-0.000115,-0.000001,0.000000,0.000000,-0.000000,0.521935,-0.161034,0.007762,0.000775,0.000001,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.255640,-0.095727,0.007099,0.000476,-0.000002,0.000000,0.000000,-0.000000,-0.065563,0.025774,-0.001950,-0.000119,-0.000001,0.000000,-0.000000,-0.000000,0.369439,-0.143183,0.010090,0.000777,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.343794,-0.080108,0.008517,0.000470,-0.000001,-0.000001,0.000000,-0.000000,-0.041859,0.021514,-0.002310,-0.000121,-0.000000,0.000000,-0.000000,0.000000,0.237123,-0.120672,0.012422,0.000778,0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.414916,-0.061669,0.009916,0.000458,-0.000009,0.000007,-0.000007,0.000006,-0.022777,0.016531,-0.002672,-0.000119,0.000002,-0.000002,0.000002,-0.000002,0.129650,-0.093496,0.014748,0.000764,-0.000013,0.000011,-0.000011,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.466214,-0.040463,0.011332,0.000568,0.000134,-0.000057,0.000134,-0.000102,-0.009036,0.010830,-0.003041,-0.000149,-0.000036,0.000015,-0.000036,0.000027,0.051663,-0.061701,0.017111,0.000936,0.000207,-0.000088,0.000207,-0.000157,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.494668,-0.015751,0.013146,-0.000344,-0.001700,-0.000012,-0.002050,0.001379,-0.001425,0.004210,-0.003516,0.000095,0.000454,0.000003,0.000547,-0.000368,0.008179,-0.024137,0.020095,-0.000464,-0.002617,-0.000017,-0.003164,0.002125,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/2.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..04ed80c86d5024fd6ef0e7e29ff3a1f1e0f16dff
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/2.csv
@@ -0,0 +1,17 @@
+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,
+1.000000,-0.968010,0.000000,0.000000,0.000000,0.012990,-0.013793,0.006314,-0.001147,0.275540,0.000000,0.000000,0.000000,-0.014705,0.015606,-0.007142,0.001297,0.774520,0.000000,0.000000,0.000000,0.020335,-0.021583,0.009877,-0.001794,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.963646,0.012851,0.010634,0.000167,-0.001407,0.001156,-0.000483,0.000085,0.270596,-0.014561,-0.012059,-0.000200,0.001594,-0.001306,0.000545,-0.000096,0.781356,0.020133,0.016671,0.000273,-0.002205,0.001806,-0.000754,0.000133,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.940642,0.032473,0.008802,-0.000574,0.000113,-0.000077,0.000029,-0.000005,0.244513,-0.036836,-0.010000,0.000649,-0.000127,0.000087,-0.000033,0.000006,0.817414,0.050917,0.013815,-0.000901,0.000175,-0.000120,0.000046,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.899881,0.048563,0.007324,-0.000479,-0.000007,0.000004,-0.000001,0.000000,0.198260,-0.055119,-0.008323,0.000545,0.000008,-0.000004,0.000002,-0.000000,0.881337,0.076165,0.011488,-0.000756,-0.000011,0.000006,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.844477,0.061758,0.005869,-0.000486,0.000000,-0.000000,0.000000,-0.000000,0.135368,-0.070114,-0.006668,0.000554,-0.000000,0.000000,-0.000000,0.000000,0.968227,0.096848,0.009191,-0.000768,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.777336,0.072038,0.004411,-0.000486,0.000000,-0.000000,0.000000,-0.000000,0.059140,-0.081789,-0.005008,0.000553,0.000000,0.000000,-0.000000,0.000000,1.073497,0.112926,0.006887,-0.000768,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.701374,0.079402,0.002953,-0.000486,-0.000000,-0.000000,-0.000000,0.000000,-0.027105,-0.090147,-0.003349,0.000553,0.000000,-0.000000,0.000000,-0.000000,1.192543,0.124398,0.004585,-0.000768,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.619504,0.083851,0.001496,-0.000486,-0.000000,-0.000000,-0.000000,0.000000,-0.120047,-0.095185,-0.001690,0.000553,0.000000,0.000000,0.000000,-0.000000,1.320757,0.131264,0.002282,-0.000768,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.534643,0.085385,0.000038,-0.000486,-0.000000,-0.000000,-0.000000,0.000000,-0.216369,-0.096905,-0.000030,0.000553,0.000000,0.000000,0.000000,-0.000000,1.453536,0.133526,-0.000021,-0.000768,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.449707,0.084001,-0.001422,-0.000487,-0.000000,0.000000,-0.000000,0.000000,-0.312751,-0.095305,0.001631,0.000554,0.000000,-0.000000,0.000000,-0.000000,1.586274,0.131182,-0.002323,-0.000767,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.367616,0.079695,-0.002885,-0.000489,-0.000001,0.000000,-0.000000,0.000000,-0.405871,-0.090380,0.003294,0.000555,0.000000,-0.000000,0.000000,-0.000000,1.714365,0.124233,-0.004625,-0.000767,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.291295,0.072456,-0.004356,-0.000492,-0.000001,0.000000,-0.000000,0.000000,-0.492401,-0.082125,0.004963,0.000558,0.000001,-0.000000,0.000000,-0.000000,1.833206,0.112683,-0.006924,-0.000766,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.223689,0.062261,-0.005842,-0.000500,-0.000003,0.000001,-0.000000,0.000000,-0.569004,-0.070521,0.006643,0.000563,0.000002,-0.000001,0.000000,-0.000000,1.938200,0.096540,-0.009217,-0.000762,0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.167771,0.049071,-0.007348,-0.000499,0.000006,-0.000006,0.000006,-0.000005,-0.632318,-0.055541,0.008337,0.000560,-0.000007,0.000007,-0.000007,0.000006,2.024764,0.075831,-0.011484,-0.000743,0.000012,-0.000010,0.000009,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.126547,0.032872,-0.008886,-0.000595,-0.000114,0.000049,-0.000115,0.000086,-0.678964,-0.037183,0.010062,0.000668,0.000129,-0.000055,0.000129,-0.000097,2.088372,0.050633,-0.013767,-0.000884,-0.000173,0.000075,-0.000175,0.000132,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.103249,0.013021,-0.010772,0.000164,0.001428,0.000006,0.001737,-0.001162,-0.705311,-0.014721,0.012182,-0.000190,-0.001614,-0.000007,-0.001963,0.001314,2.124211,0.019999,-0.016569,0.000283,0.002188,0.000010,0.002658,-0.001780,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/3.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..55c11ce214a52a5367ba4c9e0342f7c2c96bbb17
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/3.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/4.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..5dd698ee16cbd11fb3eafb2a6d45c31d8e037640
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/4.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.032904,0.000000,0.000000,0.000000,-0.018746,0.020159,-0.009280,0.001692,-0.421430,0.000000,0.000000,0.000000,-0.002915,0.003135,-0.001443,0.000263,1.623700,0.000000,0.000000,0.000000,0.017904,-0.019254,0.008863,-0.001616,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.026729,-0.018026,-0.014559,0.000218,0.002062,-0.001707,0.000717,-0.000127,-0.422390,-0.002802,-0.002263,0.000034,0.000321,-0.000265,0.000112,-0.000020,1.629598,0.017218,0.013906,-0.000208,-0.001969,0.001630,-0.000685,0.000122,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.004692,-0.043362,-0.010513,0.001291,-0.000166,0.000115,-0.000045,0.000008,-0.427274,-0.006739,-0.001633,0.000201,-0.000026,0.000018,-0.000007,0.000001,1.659611,0.041420,0.010044,-0.001233,0.000158,-0.000110,0.000043,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.057364,-0.060816,-0.006991,0.001156,0.000011,-0.000006,0.000002,-0.000000,-0.435459,-0.009448,-0.001084,0.000180,0.000002,-0.000001,0.000000,-0.000000,1.709926,0.058097,0.006681,-0.001104,-0.000010,0.000006,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.124009,-0.071308,-0.003496,0.001167,-0.000001,0.000000,-0.000000,0.000000,-0.445811,-0.011074,-0.000540,0.000182,-0.000000,0.000000,0.000000,-0.000000,1.773594,0.068126,0.003343,-0.001115,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.197647,-0.074802,0.000003,0.001166,-0.000000,0.000000,-0.000000,0.000000,-0.457243,-0.011611,0.000003,0.000181,-0.000000,0.000000,0.000000,-0.000000,1.843949,0.071469,0.000000,-0.001114,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.271280,-0.071298,0.003501,0.001167,0.000000,-0.000001,0.000000,-0.000000,-0.468670,-0.011061,0.000546,0.000181,0.000000,-0.000000,0.000000,-0.000000,1.914304,0.068126,-0.003343,-0.001115,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.337910,-0.060798,0.006993,0.001155,-0.000011,0.000010,-0.000010,0.000008,-0.479005,-0.009428,0.001087,0.000179,-0.000002,0.000002,-0.000001,0.000001,1.977973,0.058098,-0.006681,-0.001104,0.000010,-0.000009,0.000009,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.390562,-0.043343,0.010511,0.001289,0.000165,-0.000075,0.000173,-0.000127,-0.487167,-0.006718,0.001631,0.000199,0.000026,-0.000012,0.000027,-0.000020,2.028288,0.041421,-0.010044,-0.001233,-0.000158,0.000072,-0.000166,0.000122,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.421968,-0.018016,0.014551,0.000217,-0.002061,0.000008,-0.002561,0.001691,-0.492034,-0.002791,0.002255,0.000033,-0.000319,0.000001,-0.000397,0.000262,2.058302,0.017218,-0.013906,-0.000208,0.001969,-0.000007,0.002448,-0.001616,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/5.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/5.csv
new file mode 100644
index 0000000000000000000000000000000000000000..5f84f3ba0d1ecc24f6f12424947d98760607ad5c
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/5.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/6.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/6.csv
new file mode 100644
index 0000000000000000000000000000000000000000..69b7463e84e9c3b0276b0437f037aead73d4b0cc
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/6.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.763560,0.000000,0.000000,0.000000,-0.000168,0.000199,-0.000095,0.000018,-0.291250,0.000000,0.000000,0.000000,0.024511,-0.026359,0.012135,-0.002212,1.815600,0.000000,0.000000,0.000000,-0.021370,0.022977,-0.010577,0.001928,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.763607,-0.000126,-0.000076,0.000033,0.000020,-0.000018,0.000008,-0.000001,-0.283176,0.023570,0.019036,-0.000285,-0.002696,0.002232,-0.000938,0.000166,1.808558,-0.020559,-0.016610,0.000241,0.002350,-0.001945,0.000817,-0.000145,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.763767,-0.000151,0.000053,0.000040,-0.000003,0.000001,-0.000001,0.000000,-0.242091,0.056697,0.013745,-0.001688,0.000217,-0.000150,0.000058,-0.000010,1.772708,-0.049492,-0.012020,0.001465,-0.000188,0.000131,-0.000051,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.763828,0.000062,0.000153,0.000025,-0.000005,-0.000001,0.000000,0.000000,-0.173222,0.079519,0.009143,-0.001510,-0.000013,0.000008,-0.000003,0.000000,1.712562,-0.069477,-0.008021,0.001315,0.000013,-0.000007,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.763594,0.000422,0.000195,0.000002,-0.000006,-0.000000,0.000000,0.000000,-0.086077,0.093249,0.004582,-0.001522,0.000001,-0.000000,0.000000,-0.000000,1.636387,-0.081543,-0.004037,0.001333,0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.762982,0.000792,0.000163,-0.000022,-0.000004,0.000001,-0.000001,0.000000,0.010233,0.097851,0.000021,-0.001519,-0.000000,-0.000000,-0.000000,0.000000,1.552141,-0.085614,-0.000033,0.001336,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.762051,0.001040,0.000079,-0.000034,-0.000003,0.000001,-0.000000,0.000000,0.106585,0.093332,-0.004544,-0.001526,-0.000003,0.000002,-0.000001,0.000001,1.467831,-0.081668,0.003980,0.001339,0.000001,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.760968,0.001088,-0.000035,-0.000041,-0.000001,0.000000,0.000000,-0.000000,0.193845,0.079661,-0.009124,-0.001519,0.000012,-0.000012,0.000012,-0.000010,1.391483,-0.069690,0.007993,0.001328,-0.000012,0.000011,-0.000011,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.759957,0.000891,-0.000164,-0.000047,-0.000005,0.000002,-0.000005,0.000003,0.262864,0.056845,-0.013760,-0.001701,-0.000218,0.000099,-0.000228,0.000167,1.331112,-0.049713,0.012042,0.001484,0.000190,-0.000086,0.000199,-0.000146,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.759282,0.000407,-0.000314,-0.000024,0.000050,-0.000001,0.000065,-0.000042,0.304069,0.023647,-0.019092,-0.000294,0.002706,-0.000010,0.003365,-0.002221,1.295082,-0.020674,0.016694,0.000254,-0.002365,0.000009,-0.002941,0.001941,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/7.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/7.csv
new file mode 100644
index 0000000000000000000000000000000000000000..1c9ce9f75f42b3c9c5dd9640e447f00aed876c8d
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/7.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-0.759140,0.000000,0.000000,0.000000,0.020863,-0.022366,0.010282,-0.001873,0.312170,0.000000,0.000000,-0.000000,-0.011527,0.012365,-0.005686,0.001036,1.288000,0.000000,0.000000,-0.000000,0.017452,-0.018710,0.008602,-0.001567,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.752234,0.020203,0.016416,-0.000119,-0.002287,0.001889,-0.000792,0.000140,0.308358,-0.011147,-0.009047,0.000079,0.001264,-0.001045,0.000438,-0.000078,1.293777,0.016899,0.013731,-0.000100,-0.001913,0.001580,-0.000663,0.000117,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.716784,0.049202,0.012286,-0.001315,0.000183,-0.000127,0.000049,-0.000008,0.288823,-0.027084,-0.006725,0.000740,-0.000101,0.000070,-0.000027,0.000005,1.323428,0.041152,0.010274,-0.001101,0.000153,-0.000106,0.000041,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.656515,0.070162,0.008729,-0.001166,-0.000012,0.000007,-0.000002,0.000000,0.255699,-0.038500,-0.004721,0.000657,0.000006,-0.000004,0.000001,-0.000000,1.373833,0.058675,0.007296,-0.000976,-0.000010,0.000006,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.578797,0.084098,0.005201,-0.001179,0.000000,-0.000000,0.000000,-0.000000,0.213138,-0.045960,-0.002735,0.000663,-0.000001,0.000000,0.000000,-0.000000,1.438823,0.070318,0.004343,-0.000987,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.490677,0.090963,0.001664,-0.001179,-0.000000,0.000000,0.000000,0.000000,0.165106,-0.049443,-0.000750,0.000660,-0.000002,-0.000000,-0.000000,-0.000000,1.512497,0.076046,0.001385,-0.000986,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.399230,0.090752,-0.001875,-0.001178,0.000002,-0.000001,0.000001,-0.000000,0.115571,-0.048973,0.001211,0.000641,-0.000011,0.000001,0.000000,-0.000000,1.588943,0.075861,-0.001568,-0.000982,0.000003,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.311529,0.083479,-0.005386,-0.001154,0.000014,-0.000001,-0.000002,0.000001,0.068439,-0.044671,0.003069,0.000595,-0.000017,0.000002,0.000002,-0.000001,1.662257,0.069793,-0.004491,-0.000962,0.000010,-0.000000,-0.000002,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.234578,0.069293,-0.008781,-0.001113,0.000013,-0.000011,0.000011,-0.000008,0.027419,-0.036799,0.004786,0.000559,-0.000007,0.000006,-0.000006,0.000004,1.726605,0.057957,-0.007332,-0.000933,0.000011,-0.000009,0.000009,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.175174,0.048394,-0.012172,-0.001257,-0.000179,0.000080,-0.000185,0.000137,-0.004037,-0.025549,0.006493,0.000636,0.000094,-0.000042,0.000097,-0.000072,1.776300,0.040492,-0.010178,-0.001055,-0.000150,0.000067,-0.000155,0.000115,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.140357,0.019809,-0.016121,-0.000085,0.002236,-0.000003,0.002762,-0.001831,-0.022380,-0.010411,0.008492,0.000021,-0.001171,0.000001,-0.001443,0.000958,1.805436,0.016579,-0.013491,-0.000074,0.001872,-0.000002,0.002312,-0.001533,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/8.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/8.csv
new file mode 100644
index 0000000000000000000000000000000000000000..d2bbe2132f0b8e4d5b74cf8233395ee3b84bd6a2
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/8.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/1/9.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/1/9.csv
new file mode 100644
index 0000000000000000000000000000000000000000..520f92613859598ae8d4a98ebad209ea33846356
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/1/9.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/1.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..f86b9cb63b411ae430c491a4003a898a98abcd2b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/1.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-1.000000,0.000000,0.000000,0.000000,0.029267,-0.031371,0.014421,-0.002627,0.000000,0.000000,0.000000,0.000000,-0.011809,0.012658,-0.005819,0.001060,1.500000,0.000000,0.000000,0.000000,-0.005453,0.005845,-0.002687,0.000489,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.990310,0.028352,0.023046,-0.000157,-0.003208,0.002649,-0.001111,0.000197,-0.003910,-0.011439,-0.009298,0.000064,0.001294,-0.001069,0.000448,-0.000079,1.498195,-0.005282,-0.004294,0.000029,0.000598,-0.000493,0.000207,-0.000037,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.940543,0.069094,0.017280,-0.001836,0.000257,-0.000178,0.000069,-0.000012,-0.023990,-0.027878,-0.006972,0.000741,-0.000104,0.000072,-0.000028,0.000005,1.488923,-0.012873,-0.003219,0.000342,-0.000048,0.000033,-0.000013,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.855869,0.098613,0.012317,-0.001626,-0.000016,0.000009,-0.000003,0.000001,-0.058153,-0.039788,-0.004969,0.000656,0.000007,-0.000004,0.000001,-0.000000,1.473148,-0.018372,-0.002295,0.000303,0.000003,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.746576,0.118333,0.007396,-0.001644,0.000001,-0.000000,0.000000,-0.000000,-0.102250,-0.047744,-0.002984,0.000663,-0.000000,0.000000,-0.000000,0.000000,1.452786,-0.022046,-0.001378,0.000306,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.622491,0.128193,0.002465,-0.001643,-0.000000,0.000000,-0.000000,0.000000,-0.152315,-0.051722,-0.000994,0.000663,0.000000,-0.000000,-0.000000,0.000000,1.429668,-0.023883,-0.000459,0.000306,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.493476,0.128194,-0.002465,-0.001643,0.000000,-0.000000,0.000000,-0.000000,-0.204368,-0.051721,0.000995,0.000663,-0.000000,0.000000,-0.000000,0.000000,1.405632,-0.023883,0.000459,0.000306,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.369390,0.118335,-0.007395,-0.001644,-0.000001,0.000001,-0.000001,0.000001,-0.254432,-0.047743,0.002984,0.000663,0.000000,-0.000001,0.000000,-0.000000,1.382515,-0.022046,0.001378,0.000306,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.260094,0.098615,-0.012317,-0.001627,0.000016,-0.000015,0.000015,-0.000012,-0.298528,-0.039787,0.004969,0.000656,-0.000007,0.000006,-0.000006,0.000005,1.362152,-0.018372,0.002295,0.000303,-0.000003,0.000003,-0.000003,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.175419,0.069096,-0.017280,-0.001836,-0.000257,0.000115,-0.000267,0.000197,-0.332691,-0.027877,0.006972,0.000741,0.000104,-0.000046,0.000108,-0.000079,1.346377,-0.012873,0.003219,0.000342,0.000048,-0.000021,0.000050,-0.000037,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.125651,0.028353,-0.023046,-0.000157,0.003208,-0.000005,0.003966,-0.002627,-0.352770,-0.011439,0.009298,0.000064,-0.001294,0.000002,-0.001600,0.001060,1.337105,-0.005282,0.004294,0.000029,-0.000598,0.000001,-0.000739,0.000489,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/10.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/10.csv
new file mode 100644
index 0000000000000000000000000000000000000000..149015531c08589e7a34c09f7560324877f418ba
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/10.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/11.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/11.csv
new file mode 100644
index 0000000000000000000000000000000000000000..23478dfefbd9d6b76c8c5439e713eb1f93eb8bfe
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/11.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,-0.709270,0.000000,0.000000,0.000000,0.022790,-0.024337,0.011168,-0.002032,0.319560,0.000000,0.000000,0.000000,-0.002105,0.002246,-0.001030,0.000187,0.894170,0.000000,0.000000,-0.000000,0.038025,-0.040652,0.018665,-0.003397,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.701680,0.022264,0.018228,0.000042,-0.002487,0.002048,-0.000857,0.000152,0.318858,-0.002061,-0.001691,-0.000008,0.000230,-0.000189,0.000079,-0.000014,0.906811,0.037053,0.030270,-0.000014,-0.004156,0.003424,-0.001434,0.000254,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.662291,0.055053,0.014232,-0.001269,0.000198,-0.000137,0.000053,-0.000009,0.315203,-0.005116,-0.001332,0.000115,-0.000018,0.000013,-0.000005,0.000001,0.972208,0.091221,0.023351,-0.002200,0.000332,-0.000229,0.000088,-0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.594170,0.080071,0.010846,-0.001108,-0.000013,0.000007,-0.000003,0.000000,0.308861,-0.007468,-0.001025,0.000101,0.000002,-0.000001,0.000000,-0.000000,1.084756,0.131929,0.017458,-0.001929,-0.000021,0.000012,-0.000004,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.504370,0.098408,0.007483,-0.001127,-0.000002,0.000000,-0.000000,0.000000,0.300471,-0.009207,-0.000712,0.000107,0.000002,0.000001,0.000001,-0.000000,1.232202,0.161013,0.011615,-0.001954,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.399608,0.109983,0.004088,-0.001138,-0.000004,0.000001,-0.000001,0.000000,0.290662,-0.010298,-0.000370,0.000124,0.000006,-0.000001,0.000001,-0.000000,1.402875,0.178378,0.005748,-0.001958,-0.000002,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.286679,0.114728,0.000647,-0.001158,-0.000006,0.000002,-0.000002,0.000000,0.280125,-0.010639,0.000041,0.000150,0.000007,-0.000002,0.000001,-0.000000,1.585042,0.183996,-0.000133,-0.001962,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.172468,0.112523,-0.002871,-0.001195,-0.000017,0.000001,-0.000006,0.000002,0.269682,-0.010088,0.000516,0.000161,-0.000002,-0.000001,-0.000003,0.000001,1.766943,0.177843,-0.006019,-0.001959,0.000003,0.000000,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.064031,0.103110,-0.006602,-0.001314,-0.000042,0.000008,-0.000000,0.000000,0.260266,-0.008600,0.000945,0.000109,-0.000024,0.000001,0.000001,-0.000000,1.936813,0.159949,-0.011858,-0.001925,0.000014,0.000001,-0.000002,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.031129,0.085837,-0.010713,-0.001397,0.000002,-0.000009,0.000012,-0.000010,0.252697,-0.006468,0.001152,0.000038,-0.000011,0.000003,-0.000001,0.000001,2.082994,0.130520,-0.017538,-0.001854,0.000028,-0.000021,0.000018,-0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.104850,0.060180,-0.015025,-0.001606,-0.000227,0.000101,-0.000233,0.000172,0.247411,-0.004080,0.001229,0.000025,0.000009,-0.000004,0.000012,-0.000009,2.194134,0.089898,-0.023175,-0.002103,-0.000322,0.000143,-0.000334,0.000248,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.148212,0.024714,-0.020080,-0.000148,0.002798,-0.000005,0.003461,-0.002292,0.244592,-0.001526,0.001300,-0.000067,-0.000158,-0.000002,-0.000186,0.000127,2.258489,0.036399,-0.029784,0.000046,0.004071,0.000002,0.005003,-0.003326,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/12.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/12.csv
new file mode 100644
index 0000000000000000000000000000000000000000..93c1eacab0aa6d15e87267520a9abd8ceb9e1efa
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/12.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.156660,0.000000,0.000000,0.000000,-0.013056,0.014080,-0.006490,0.001184,0.244080,0.000000,0.000000,0.000000,0.032890,-0.035368,0.016282,-0.002968,2.270900,0.000000,0.000000,0.000000,-0.039619,0.042586,-0.019601,0.003573,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.152378,-0.012474,-0.010017,0.000223,0.001441,-0.001195,0.000503,-0.000090,0.254915,0.031631,0.025549,-0.000380,-0.003618,0.002994,-0.001258,0.000223,2.257839,-0.038139,-0.030831,0.000425,0.004356,-0.003604,0.001514,-0.000268,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.130769,-0.029660,-0.006985,0.000969,-0.000117,0.000081,-0.000035,0.000006,0.310058,0.076104,0.018459,-0.002263,0.000290,-0.000201,0.000078,-0.000013,2.191292,-0.091916,-0.022387,0.002694,-0.000348,0.000242,-0.000092,0.000016,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.095029,-0.040950,-0.004362,0.000831,-0.000017,-0.000005,0.000003,-0.000000,0.402510,0.106757,0.012278,-0.002033,-0.000020,0.000012,-0.000004,0.000001,2.079501,-0.129230,-0.015020,0.002435,0.000035,-0.000014,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.050528,-0.047262,-0.001988,0.000756,-0.000013,0.000005,-0.000002,0.000000,0.519502,0.125176,0.006133,-0.002050,0.000002,-0.000001,0.000000,-0.000000,1.937711,-0.151873,-0.007594,0.002495,0.000003,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.002024,-0.049004,0.000236,0.000733,-0.000002,0.000001,-0.000000,0.000000,0.648763,0.131297,-0.000010,-0.002047,0.000000,-0.000000,-0.000000,-0.000000,1.780742,-0.159567,-0.000097,0.002501,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.046012,-0.046338,0.002429,0.000730,0.000000,-0.000001,0.000000,-0.000000,0.778003,0.125138,-0.006149,-0.002047,-0.000001,0.000002,-0.000001,0.000001,1.623579,-0.152257,0.007409,0.002503,0.000001,-0.000002,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.089192,-0.039292,0.004614,0.000723,-0.000007,0.000006,-0.000006,0.000005,0.894946,0.106704,-0.012276,-0.002026,0.000019,-0.000017,0.000017,-0.000014,1.481234,-0.129934,0.014903,0.002479,-0.000023,0.000021,-0.000021,0.000017,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.123149,-0.027895,0.006815,0.000809,0.000106,-0.000048,0.000111,-0.000081,0.987352,0.076067,-0.018448,-0.002262,-0.000290,0.000131,-0.000304,0.000223,1.368676,-0.092685,0.022453,0.002767,0.000354,-0.000160,0.000371,-0.000272,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.143332,-0.011559,0.009350,0.000121,-0.001319,0.000004,-0.001637,0.001082,1.042469,0.031618,-0.025537,-0.000380,0.003616,-0.000013,0.004495,-0.002967,1.301504,-0.038542,0.031123,0.000472,-0.004410,0.000016,-0.005483,0.003619,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/13.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/13.csv
new file mode 100644
index 0000000000000000000000000000000000000000..fb355f340e92e7c5fb52fa1068ce0eecd58b59ec
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/13.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.147290,0.000000,0.000000,0.000000,-0.003762,0.004045,-0.001862,0.000339,1.053300,0.000000,0.000000,0.000000,-0.032931,0.035413,-0.016302,0.002972,1.288300,0.000000,0.000000,0.000000,0.001073,-0.001154,0.000531,-0.000097,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.148530,-0.003619,-0.002924,0.000043,0.000414,-0.000342,0.000144,-0.000026,1.042452,-0.031669,-0.025578,0.000382,0.003622,-0.002998,0.001260,-0.000224,1.288654,0.001033,0.000835,-0.000012,-0.000118,0.000098,-0.000041,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.154840,-0.008711,-0.002116,0.000258,-0.000033,0.000023,-0.000009,0.000002,0.987247,-0.076187,-0.018475,0.002267,-0.000291,0.000202,-0.000078,0.000014,1.290455,0.002487,0.000605,-0.000073,0.000009,-0.000007,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.165426,-0.012229,-0.001412,0.000231,0.000002,-0.000001,0.000000,-0.000000,0.894698,-0.106865,-0.012291,0.002030,0.000019,-0.000011,0.000004,-0.000001,1.293478,0.003493,0.000405,-0.000066,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.178836,-0.014357,-0.000714,0.000233,-0.000000,0.000000,-0.000000,0.000000,0.777583,-0.125319,-0.006153,0.002050,-0.000001,0.000000,-0.000000,0.000000,1.297310,0.004104,0.000206,-0.000066,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.193674,-0.015086,-0.000015,0.000233,0.000000,0.000000,0.000000,0.000000,0.648160,-0.131477,-0.000005,0.002049,0.000000,0.000000,-0.000000,0.000000,1.301554,0.004317,0.000007,-0.000066,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.208542,-0.014417,0.000685,0.000234,0.000001,-0.000000,0.000000,-0.000000,0.518728,-0.125339,0.006144,0.002051,0.000001,-0.000002,0.000001,-0.000001,1.305812,0.004133,-0.000192,-0.000066,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.222039,-0.012342,0.001391,0.000236,-0.000000,0.000002,-0.000002,0.000002,0.401582,-0.106903,0.012285,0.002032,-0.000018,0.000017,-0.000017,0.000014,1.309686,0.003549,-0.000393,-0.000068,-0.000000,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.232753,-0.008843,0.002122,0.000271,0.000035,-0.000016,0.000036,-0.000026,0.308993,-0.076227,0.018478,0.002271,0.000291,-0.000132,0.000305,-0.000224,1.312773,0.002555,-0.000606,-0.000081,-0.000011,0.000005,-0.000011,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.239174,-0.003692,0.002976,0.000053,-0.000424,0.000002,-0.000528,0.000348,0.253756,-0.031690,0.025593,0.000384,-0.003625,0.000013,-0.004506,0.002975,1.314632,0.001072,-0.000862,-0.000018,0.000124,-0.000001,0.000154,-0.000102,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/14.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/14.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ef48fab79851cf5c8e50499cb47110889eda7e1b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/14.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.240440,0.000000,0.000000,0.000000,0.016441,-0.017681,0.008139,-0.001484,0.242900,0.000000,0.000000,0.000000,0.018591,-0.019992,0.009204,-0.001678,1.315000,0.000000,0.000000,0.000000,-0.015651,0.016832,-0.007749,0.001413,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.235024,0.015810,0.012769,-0.000191,-0.001808,0.001497,-0.000629,0.000112,0.249024,0.017878,0.014439,-0.000216,-0.002045,0.001693,-0.000711,0.000126,1.309844,-0.015050,-0.012155,0.000182,0.001722,-0.001425,0.000599,-0.000106,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.207466,0.038030,0.009220,-0.001132,0.000145,-0.000101,0.000039,-0.000007,0.280188,0.043008,0.010428,-0.001280,0.000164,-0.000114,0.000044,-0.000008,1.283610,-0.036203,-0.008777,0.001078,-0.000138,0.000096,-0.000037,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.161272,0.053337,0.006131,-0.001014,-0.000009,0.000005,-0.000002,0.000000,0.332431,0.060322,0.006936,-0.001146,-0.000010,0.000006,-0.000002,0.000000,1.239636,-0.050774,-0.005837,0.000965,0.000009,-0.000005,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.102822,0.062540,0.003067,-0.001023,0.000001,-0.000000,0.000000,-0.000000,0.398535,0.070732,0.003470,-0.001158,0.000001,-0.000000,0.000000,-0.000000,1.183995,-0.059535,-0.002920,0.000974,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.038238,0.065606,-0.000001,-0.001023,-0.000000,-0.000000,0.000000,-0.000000,0.471580,0.074200,-0.000001,-0.001157,0.000000,-0.000000,0.000000,-0.000000,1.122514,-0.062453,0.000001,0.000974,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.026345,0.062536,-0.003069,-0.001023,-0.000000,0.000001,-0.000000,0.000000,0.544621,0.070726,-0.003472,-0.001157,-0.000000,0.000001,-0.000000,0.000000,1.061036,-0.059530,0.002922,0.000974,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.084789,0.053330,-0.006133,-0.001013,0.000009,-0.000009,0.000008,-0.000007,0.610718,0.060312,-0.006937,-0.001146,0.000011,-0.000010,0.000010,-0.000008,1.005402,-0.050765,0.005838,0.000964,-0.000009,0.000008,-0.000008,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.130975,0.038022,-0.009219,-0.001132,-0.000145,0.000066,-0.000152,0.000112,0.662951,0.042998,-0.010427,-0.001279,-0.000164,0.000074,-0.000172,0.000126,0.961438,-0.036192,0.008776,0.001077,0.000138,-0.000062,0.000145,-0.000106,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.158526,0.015805,-0.012765,-0.000191,0.001808,-0.000007,0.002247,-0.001483,0.694107,0.017873,-0.014436,-0.000215,0.002044,-0.000008,0.002541,-0.001677,0.935213,-0.015044,0.012151,0.000181,-0.001721,0.000006,-0.002139,0.001412,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/15.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/15.csv
new file mode 100644
index 0000000000000000000000000000000000000000..249c0111a252646a988fef9bd75127b407d985b8
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/15.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.163940,0.000000,0.000000,0.000000,0.000723,-0.000777,0.000358,-0.000065,0.700230,0.000000,0.000000,0.000000,-0.020779,0.022346,-0.010287,0.001875,0.930060,0.000000,0.000000,0.000000,0.007170,-0.007711,0.003550,-0.000647,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.164178,0.000695,0.000561,-0.000008,-0.000080,0.000066,-0.000028,0.000005,0.693385,-0.019983,-0.016140,0.000241,0.002286,-0.001892,0.000795,-0.000141,0.932422,0.006896,0.005569,-0.000083,-0.000789,0.000653,-0.000274,0.000049,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.165390,0.001672,0.000405,-0.000050,0.000006,-0.000004,0.000002,-0.000000,0.658550,-0.048074,-0.011658,0.001430,-0.000183,0.000127,-0.000049,0.000009,0.944442,0.016589,0.004023,-0.000494,0.000063,-0.000044,0.000017,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.167420,0.002344,0.000269,-0.000045,-0.000000,0.000000,-0.000000,0.000000,0.600152,-0.067432,-0.007756,0.001281,0.000012,-0.000007,0.000002,-0.000000,0.964594,0.023269,0.002676,-0.000442,-0.000004,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.169989,0.002748,0.000134,-0.000045,0.000000,-0.000000,0.000000,-0.000000,0.526251,-0.079076,-0.003882,0.001294,-0.000001,0.000000,-0.000000,0.000000,0.990095,0.027287,0.001340,-0.000447,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.172827,0.002882,-0.000001,-0.000045,0.000000,-0.000000,0.000000,-0.000000,0.444587,-0.082960,-0.000002,0.001293,0.000000,0.000000,-0.000000,0.000000,1.018275,0.028627,0.000001,-0.000446,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.175663,0.002746,-0.000135,-0.000045,0.000000,0.000000,-0.000000,0.000000,0.362918,-0.079083,0.003880,0.001294,0.000001,-0.000001,0.000000,-0.000000,1.046456,0.027289,-0.001339,-0.000447,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.178228,0.002340,-0.000270,-0.000044,0.000000,-0.000000,0.000000,-0.000000,0.289009,-0.067443,0.007755,0.001282,-0.000012,0.000011,-0.000011,0.000009,1.071959,0.023272,-0.002676,-0.000442,0.000004,-0.000004,0.000004,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.180254,0.001668,-0.000405,-0.000050,-0.000006,0.000003,-0.000007,0.000005,0.230600,-0.048085,0.011659,0.001431,0.000183,-0.000083,0.000192,-0.000141,1.092114,0.016592,-0.004023,-0.000494,-0.000063,0.000029,-0.000066,0.000049,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.181463,0.000693,-0.000560,-0.000008,0.000079,-0.000000,0.000098,-0.000065,0.195757,-0.019988,0.016144,0.000241,-0.002286,0.000008,-0.002842,0.001876,1.104137,0.006897,-0.005571,-0.000083,0.000789,-0.000003,0.000981,-0.000647,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/16.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/16.csv
new file mode 100644
index 0000000000000000000000000000000000000000..6c67ec99fff73c7574874f9a40e1c5f6ff3cd89f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/16.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.181700,0.000000,0.000000,0.000000,-0.023261,0.025195,-0.011636,0.002126,0.188910,0.000000,0.000000,0.000000,0.012121,-0.013477,0.006294,-0.001158,1.106500,0.000000,0.000000,0.000000,0.005567,-0.005909,0.002704,-0.000491,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.174124,-0.022002,-0.017510,0.000593,0.002579,-0.002146,0.000906,-0.000161,0.192691,0.010761,0.008056,-0.000930,-0.001383,0.001173,-0.000501,0.000090,1.108372,0.005513,0.004565,0.000075,-0.000604,0.000495,-0.000206,0.000036,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.136382,-0.051352,-0.011519,0.001919,-0.000208,0.000146,-0.000058,0.000010,0.209957,0.022039,0.003070,-0.001607,0.000113,-0.000078,0.000025,-0.000003,1.118246,0.013944,0.003784,-0.000246,0.000048,-0.000033,0.000014,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.075320,-0.069012,-0.006203,0.001751,0.000016,-0.000013,0.000011,-0.000004,0.233515,0.023546,-0.001549,-0.001550,-0.000015,-0.000010,0.000050,-0.000009,1.135753,0.020863,0.003152,-0.000203,-0.000002,0.000003,-0.000006,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.001867,-0.076127,-0.000898,0.001776,-0.000012,0.000027,-0.000068,0.000014,0.253978,0.015927,-0.005818,-0.001007,0.000388,-0.000116,0.000054,-0.000003,1.159561,0.026535,0.002487,-0.000275,-0.000051,0.000018,-0.000015,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.073421,-0.072817,0.003899,0.001119,-0.000418,0.000095,0.000029,-0.000014,0.263403,0.002541,-0.006937,0.000338,0.000492,-0.000129,-0.000021,0.000013,1.188262,0.030495,0.001355,-0.000526,-0.000113,0.000028,0.000006,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.141528,-0.062783,0.005841,0.000490,0.000006,0.000017,-0.000014,0.000003,0.259700,-0.009029,-0.004300,0.001055,-0.000009,-0.000019,0.000016,-0.000004,1.219504,0.031329,-0.000600,-0.000693,0.000002,0.000005,-0.000004,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.197967,-0.049582,0.007377,0.000518,-0.000005,0.000003,-0.000004,0.000005,0.247410,-0.014526,-0.001222,0.001008,-0.000007,0.000009,-0.000008,0.000004,1.249544,0.028065,-0.002656,-0.000678,0.000006,-0.000006,0.000006,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.239655,-0.033269,0.008970,0.000612,0.000115,-0.000049,0.000116,-0.000087,0.232669,-0.013946,0.001824,0.001067,0.000074,-0.000038,0.000085,-0.000059,1.274276,0.020718,-0.004716,-0.000745,-0.000083,0.000039,-0.000089,0.000064,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.263248,-0.013195,0.010909,-0.000158,-0.001449,-0.000006,-0.001764,0.001180,0.221676,-0.006892,0.005132,0.000630,-0.000893,0.000023,-0.001174,0.000748,1.289463,0.008828,-0.007044,-0.000215,0.001030,-0.000008,0.001294,-0.000849,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/17.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/17.csv
new file mode 100644
index 0000000000000000000000000000000000000000..040c0612d250dadd392125aa183550f1c019eacf
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/17.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/18.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/18.csv
new file mode 100644
index 0000000000000000000000000000000000000000..b94164a94d6713ee8732102b33103f6307e8aa76
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/18.csv
@@ -0,0 +1,8 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/19.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/19.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ed9d9868021603d897fad1c374fb96540b2babc3
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/19.csv
@@ -0,0 +1,20 @@
+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,
+1.000000,-0.066242,0.000000,0.000000,-0.000000,-0.012234,0.013029,-0.005972,0.001086,0.331390,0.000000,0.000000,-0.000000,0.005700,-0.006355,0.002972,-0.000547,1.489300,0.000000,0.000000,0.000000,-0.018053,0.019185,-0.008785,0.001596,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.070334,-0.012024,-0.009895,-0.000086,0.001331,-0.001094,0.000457,-0.000081,0.333159,0.005023,0.003732,-0.000470,-0.000652,0.000555,-0.000238,0.000043,1.483243,-0.017825,-0.014726,-0.000200,0.001960,-0.001608,0.000671,-0.000118,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.091725,-0.030039,-0.007942,0.000619,-0.000106,0.000073,-0.000028,0.000005,0.341152,0.010116,0.001289,-0.000788,0.000054,-0.000040,0.000017,-0.000003,1.451397,-0.044877,-0.012062,0.000842,-0.000156,0.000107,-0.000041,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.129144,-0.044262,-0.006314,0.000531,0.000007,-0.000005,0.000004,-0.000001,0.351797,0.010426,-0.000963,-0.000745,-0.000005,0.000007,-0.000010,0.000003,1.395217,-0.066762,-0.009871,0.000713,0.000010,-0.000007,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.179184,-0.055278,-0.004695,0.000546,0.000001,0.000005,-0.000015,0.000003,0.360510,0.006243,-0.003239,-0.000779,-0.000005,-0.000019,0.000058,-0.000012,1.319303,-0.084342,-0.007699,0.000732,0.000001,0.000005,-0.000016,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.238616,-0.063068,-0.003161,0.000406,-0.000093,0.000021,0.000006,-0.000003,0.362757,-0.002423,-0.005177,-0.000246,0.000354,-0.000080,-0.000025,0.000012,1.227987,-0.097586,-0.005613,0.000587,-0.000096,0.000022,0.000007,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.304509,-0.068423,-0.002261,0.000262,-0.000001,0.000004,-0.000003,0.000001,0.355172,-0.012562,-0.004709,0.000294,-0.000001,-0.000015,0.000012,-0.000003,1.125304,-0.107311,-0.004181,0.000440,0.000000,0.000004,-0.000003,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.374930,-0.072155,-0.001469,0.000264,-0.000000,-0.000001,0.000000,-0.000000,0.338187,-0.021128,-0.003870,0.000272,-0.000001,0.000003,-0.000002,0.000000,1.014255,-0.114343,-0.002848,0.000446,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.448290,-0.074302,-0.000679,0.000263,-0.000000,0.000000,-0.000000,0.000000,0.313463,-0.028049,-0.003051,0.000274,0.000000,-0.000000,0.000000,-0.000000,0.897510,-0.118702,-0.001511,0.000446,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.523009,-0.074872,0.000109,0.000262,-0.000000,-0.000000,0.000000,-0.000000,0.282637,-0.033328,-0.002228,0.000274,-0.000000,0.000000,-0.000000,0.000000,0.777743,-0.120387,-0.000174,0.000445,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.597510,-0.073869,0.000895,0.000262,-0.000000,0.000000,0.000000,-0.000000,0.247354,-0.036962,-0.001406,0.000274,0.000000,-0.000000,0.000000,-0.000000,0.657626,-0.119400,0.001162,0.000445,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.670222,-0.071293,0.001681,0.000262,-0.000000,0.000000,0.000000,-0.000000,0.209260,-0.038951,-0.000583,0.000274,0.000000,0.000000,0.000000,-0.000000,0.539834,-0.115740,0.002498,0.000445,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.739572,-0.067146,0.002466,0.000262,-0.000000,0.000000,0.000000,-0.000000,0.170000,-0.039294,0.000240,0.000274,0.000000,-0.000000,0.000000,-0.000000,0.427037,-0.109408,0.003834,0.000445,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.803990,-0.061427,0.003252,0.000262,0.000000,-0.000000,0.000000,-0.000000,0.131220,-0.037992,0.001063,0.000274,0.000000,-0.000000,0.000000,-0.000000,0.321909,-0.100404,0.005170,0.000445,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.861903,-0.054138,0.004038,0.000262,0.000000,0.000000,0.000000,0.000000,0.094566,-0.035043,0.001886,0.000274,0.000000,0.000000,-0.000000,0.000000,0.227120,-0.088728,0.006506,0.000445,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.911742,-0.045277,0.004823,0.000262,0.000000,-0.000000,0.000000,-0.000000,0.061684,-0.030447,0.002709,0.000275,0.000000,-0.000000,0.000000,-0.000000,0.145344,-0.074380,0.007842,0.000446,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.951933,-0.034845,0.005607,0.000257,-0.000005,0.000004,-0.000004,0.000003,0.034220,-0.024205,0.003531,0.000271,-0.000004,0.000003,-0.000003,0.000003,0.079252,-0.057360,0.009174,0.000437,-0.000008,0.000007,-0.000006,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.980915,-0.022858,0.006403,0.000320,0.000076,-0.000032,0.000076,-0.000057,0.013816,-0.016330,0.004362,0.000318,0.000057,-0.000025,0.000058,-0.000043,0.031502,-0.037697,0.010528,0.000541,0.000126,-0.000053,0.000125,-0.000095,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.996988,-0.008897,0.007426,-0.000195,-0.000960,-0.000007,-0.001158,0.000779,0.002212,-0.006506,0.005367,-0.000063,-0.000717,-0.000002,-0.000875,0.000584,0.004976,-0.014695,0.012256,-0.000310,-0.001588,-0.000011,-0.001917,0.001289,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/2.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..75ccac089d7547a6a0e690bab4678b6a8fc6509b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/2.csv
@@ -0,0 +1,17 @@
+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,
+1.000000,-0.115960,0.000000,0.000000,0.000000,0.001374,-0.001464,0.000671,-0.000122,-0.356680,0.000000,0.000000,-0.000000,-0.006499,0.006902,-0.003159,0.000574,1.335300,0.000000,0.000000,0.000000,-0.001428,0.001518,-0.000695,0.000126,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.115500,0.001351,0.001112,0.000010,-0.000150,0.000123,-0.000051,0.000009,-0.358863,-0.006428,-0.005317,-0.000081,0.000705,-0.000578,0.000241,-0.000043,1.334821,-0.001410,-0.001165,-0.000016,0.000155,-0.000127,0.000053,-0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.113097,0.003374,0.000892,-0.000070,0.000012,-0.000008,0.000003,-0.000001,-0.370363,-0.016225,-0.004385,0.000294,-0.000056,0.000038,-0.000015,0.000003,1.332301,-0.003551,-0.000954,0.000067,-0.000012,0.000008,-0.000003,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.108894,0.004971,0.000709,-0.000060,-0.000001,0.000000,-0.000000,0.000000,-0.390710,-0.024217,-0.003624,0.000248,0.000003,-0.000002,0.000001,-0.000000,1.327856,-0.005283,-0.000781,0.000056,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.103274,0.006208,0.000527,-0.000061,0.000000,-0.000000,0.000000,-0.000000,-0.418301,-0.030714,-0.002872,0.000251,-0.000000,0.000000,-0.000000,0.000000,1.321849,-0.006674,-0.000610,0.000057,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.096600,0.007081,0.000346,-0.000060,0.000000,0.000000,-0.000000,0.000000,-0.451635,-0.035704,-0.002118,0.000251,-0.000000,-0.000000,0.000000,-0.000000,1.314622,-0.007722,-0.000438,0.000057,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.089234,0.007591,0.000165,-0.000060,0.000000,0.000000,-0.000000,0.000000,-0.489206,-0.039188,-0.001366,0.000251,-0.000000,-0.000000,0.000000,-0.000000,1.306519,-0.008427,-0.000267,0.000057,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.081538,0.007741,-0.000014,-0.000059,0.000000,0.000000,-0.000000,0.000000,-0.529509,-0.041168,-0.000615,0.000250,-0.000000,-0.000000,0.000000,-0.000000,1.297882,-0.008789,-0.000095,0.000057,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.073871,0.007536,-0.000189,-0.000057,0.000001,0.000000,-0.000000,0.000000,-0.571043,-0.041649,0.000133,0.000248,-0.000001,-0.000000,0.000000,-0.000000,1.289055,-0.008808,0.000076,0.000057,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.066579,0.006992,-0.000352,-0.000051,0.000002,0.000000,-0.000000,0.000000,-0.612312,-0.040642,0.000872,0.000244,-0.000002,-0.000000,0.000000,-0.000000,1.280379,-0.008486,0.000246,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,
+1.000000,-0.059988,0.006147,-0.000487,-0.000038,0.000004,0.000001,-0.000000,0.000000,-0.651840,-0.038174,0.001592,0.000236,-0.000003,-0.000001,0.000000,-0.000000,1.272193,-0.007830,0.000409,0.000053,-0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.054362,0.005077,-0.000573,-0.000019,0.000005,0.000000,-0.000000,0.000000,-0.688189,-0.034295,0.002279,0.000222,-0.000004,-0.000000,0.000000,-0.000000,1.264824,-0.006861,0.000556,0.000044,-0.000003,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.049872,0.003894,-0.000600,0.000000,0.000004,-0.000000,-0.000000,0.000000,-0.719986,-0.029085,0.002924,0.000209,-0.000003,-0.000000,0.000000,-0.000000,1.258560,-0.005628,0.000670,0.000032,-0.000003,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.046574,0.002708,-0.000579,0.000012,0.000002,-0.000000,0.000000,-0.000000,-0.745940,-0.022620,0.003534,0.000196,-0.000005,0.000003,-0.000002,0.000002,1.253631,-0.004206,0.000746,0.000019,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.044431,0.001593,-0.000534,0.000014,-0.000004,0.000001,-0.000004,0.000003,-0.764832,-0.014967,0.004133,0.000234,0.000050,-0.000021,0.000050,-0.000038,1.250187,-0.002667,0.000793,0.000019,0.000007,-0.000003,0.000008,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.043361,0.000558,-0.000492,0.000045,0.000054,0.000002,0.000061,-0.000043,-0.775391,-0.005868,0.004880,-0.000106,-0.000637,-0.000004,-0.000772,0.000518,1.248338,-0.001005,0.000853,-0.000040,-0.000105,-0.000001,-0.000125,0.000085,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/3.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..72ee7e1983b3a32ebb5a68f3247686711fb16ae9
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/3.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/4.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ed63e7c821c1d6ce10df19ea27161ac539872d77
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/4.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.456950,0.000000,0.000000,-0.000000,-0.030734,0.033050,-0.015215,0.002774,0.185730,0.000000,0.000000,0.000000,-0.018966,0.020395,-0.009389,0.001712,1.305400,0.000000,0.000000,0.000000,0.019376,-0.020836,0.009592,-0.001749,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.467074,-0.029556,-0.023871,0.000357,0.003380,-0.002798,0.001176,-0.000209,0.179482,-0.018238,-0.014731,0.000220,0.002086,-0.001727,0.000726,-0.000129,1.311783,0.018633,0.015049,-0.000225,-0.002131,0.001764,-0.000741,0.000132,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.518595,-0.071101,-0.017241,0.002116,-0.000271,0.000188,-0.000073,0.000013,0.147690,-0.043876,-0.010639,0.001306,-0.000167,0.000116,-0.000045,0.000008,1.344263,0.044824,0.010869,-0.001334,0.000171,-0.000119,0.000046,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.604964,-0.099728,-0.011468,0.001895,0.000017,-0.000010,0.000004,-0.000001,0.094392,-0.061541,-0.007077,0.001169,0.000011,-0.000006,0.000002,-0.000000,1.398713,0.062872,0.007230,-0.001195,-0.000011,0.000006,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.714255,-0.116943,-0.005739,0.001914,-0.000001,0.000000,-0.000000,0.000000,0.026950,-0.072164,-0.003541,0.001181,-0.000001,0.000000,-0.000000,0.000000,1.467613,0.073725,0.003618,-0.001207,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.835024,-0.122682,-0.000000,0.001913,0.000000,0.000000,-0.000000,0.000000,-0.047575,-0.075705,0.000000,0.001180,-0.000000,0.000000,-0.000000,0.000000,1.543750,0.077342,0.000000,-0.001206,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.955793,-0.116944,0.005739,0.001914,0.000001,-0.000002,0.000001,-0.000001,-0.122100,-0.072164,0.003541,0.001181,0.000001,-0.000001,0.000000,-0.000000,1.619887,0.073725,-0.003618,-0.001207,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.065084,-0.099729,0.011468,0.001895,-0.000017,0.000016,-0.000016,0.000013,-0.189542,-0.061541,0.007077,0.001169,-0.000011,0.000010,-0.000010,0.000008,1.688787,0.062872,-0.007230,-0.001195,0.000011,-0.000010,0.000010,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.151454,-0.071102,0.017241,0.002116,0.000271,-0.000123,0.000284,-0.000209,-0.242840,-0.043876,0.010639,0.001306,0.000167,-0.000076,0.000175,-0.000129,1.743237,0.044824,-0.010869,-0.001334,-0.000171,0.000077,-0.000179,0.000132,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.202975,-0.029556,0.023871,0.000357,-0.003381,0.000012,-0.004202,0.002774,-0.274632,-0.018238,0.014731,0.000220,-0.002086,0.000008,-0.002593,0.001712,1.775717,0.018633,-0.015049,-0.000225,0.002131,-0.000008,0.002649,-0.001749,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/5.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/5.csv
new file mode 100644
index 0000000000000000000000000000000000000000..d065633d6cdfd381bcfce911201d67fb10d770de
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/5.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/6.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/6.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3a421ff2eeb38dc3751be8627076b79ad6f05db7
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/6.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-1.008900,0.000000,0.000000,0.000000,0.020267,-0.021794,0.010033,-0.001829,0.173510,0.000000,0.000000,0.000000,0.017204,-0.018500,0.008517,-0.001553,1.257100,0.000000,0.000000,-0.000000,-0.023203,0.024952,-0.011487,0.002094,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.002224,0.019490,0.015741,-0.000235,-0.002229,0.001845,-0.000775,0.000138,0.179177,0.016544,0.013362,-0.000200,-0.001892,0.001566,-0.000658,0.000117,1.249456,-0.022313,-0.018022,0.000269,0.002552,-0.002112,0.000888,-0.000157,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.968250,0.046886,0.011369,-0.001395,0.000179,-0.000124,0.000048,-0.000008,0.208016,0.039799,0.009650,-0.001184,0.000152,-0.000105,0.000041,-0.000007,1.210561,-0.053678,-0.013016,0.001597,-0.000205,0.000142,-0.000055,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.911296,0.065764,0.007563,-0.001250,-0.000011,0.000007,-0.000002,0.000000,0.256361,0.055822,0.006419,-0.001061,-0.000010,0.000006,-0.000002,0.000000,1.145356,-0.075290,-0.008658,0.001431,0.000013,-0.000008,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.839225,0.077117,0.003785,-0.001262,0.000001,-0.000000,0.000000,-0.000000,0.317535,0.065458,0.003212,-0.001071,0.000000,-0.000000,0.000000,-0.000000,1.062846,-0.088286,-0.004332,0.001445,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.759584,0.080903,0.000001,-0.001261,-0.000000,-0.000000,0.000000,-0.000000,0.385134,0.068669,-0.000000,-0.001071,0.000000,-0.000000,0.000000,-0.000000,0.971672,-0.092618,0.000000,0.001444,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.679941,0.077121,-0.003784,-0.001262,-0.000001,0.000001,-0.000000,0.000000,0.452732,0.065456,-0.003213,-0.001071,-0.000000,0.000001,-0.000000,0.000000,0.880498,-0.088285,0.004333,0.001445,0.000001,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.607866,0.065770,-0.007562,-0.001250,0.000011,-0.000011,0.000010,-0.000008,0.513904,0.055820,-0.006419,-0.001061,0.000010,-0.000009,0.000009,-0.000007,0.797990,-0.075288,0.008658,0.001431,-0.000013,0.000012,-0.000012,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.550906,0.046892,-0.011370,-0.001396,-0.000179,0.000081,-0.000187,0.000138,0.562247,0.039796,-0.009650,-0.001184,-0.000152,0.000069,-0.000159,0.000117,0.732787,-0.053676,0.013016,0.001597,0.000205,-0.000093,0.000215,-0.000157,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.516927,0.019493,-0.015743,-0.000235,0.002230,-0.000008,0.002772,-0.001829,0.591083,0.016543,-0.013361,-0.000200,0.001892,-0.000007,0.002352,-0.001553,0.693893,-0.022312,0.018021,0.000269,-0.002552,0.000009,-0.003172,0.002094,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/7.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/7.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3e3f2063595e2c0c7a35044a098b8ee0f437978d
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/7.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-0.510250,0.000000,0.000000,0.000000,0.027717,-0.029710,0.013657,-0.002488,0.596750,0.000000,0.000000,0.000000,-0.017125,0.018357,-0.008438,0.001537,0.686250,0.000000,0.000000,-0.000000,0.014073,-0.015085,0.006934,-0.001263,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.501073,0.026849,0.021823,-0.000150,-0.003038,0.002508,-0.001052,0.000186,0.591080,-0.016589,-0.013483,0.000093,0.001877,-0.001550,0.000650,-0.000115,0.690910,0.013634,0.011082,-0.000075,-0.001542,0.001274,-0.000534,0.000095,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.453946,0.065427,0.016360,-0.001740,0.000243,-0.000168,0.000065,-0.000011,0.561963,-0.040423,-0.010107,0.001075,-0.000150,0.000104,-0.000040,0.000007,0.714842,0.033227,0.008311,-0.000883,0.000123,-0.000085,0.000033,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.373771,0.093371,0.011658,-0.001541,-0.000015,0.000009,-0.000003,0.000001,0.512428,-0.057687,-0.007202,0.000952,0.000009,-0.000006,0.000002,-0.000000,0.755563,0.047427,0.005926,-0.000782,-0.000008,0.000004,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.270292,0.112031,0.006995,-0.001557,0.000001,-0.000000,0.000000,0.000000,0.448497,-0.069214,-0.004322,0.000962,-0.000001,0.000000,-0.000000,-0.000000,0.808129,0.056917,0.003560,-0.000791,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.152823,0.121352,0.002327,-0.001555,0.000000,-0.000000,-0.000000,0.000000,0.375923,-0.074974,-0.001439,0.000961,-0.000000,0.000000,-0.000000,0.000000,0.867815,0.061665,0.001188,-0.000791,-0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.030700,0.121340,-0.002338,-0.001555,0.000000,-0.000000,0.000000,-0.000000,0.300471,-0.074969,0.001443,0.000961,0.000000,0.000000,-0.000000,0.000000,0.929878,0.061669,-0.001184,-0.000791,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.086747,0.111999,-0.007003,-0.001556,-0.000001,0.000001,-0.000001,0.000001,0.227906,-0.069200,0.004326,0.000961,0.000000,-0.000001,0.000000,-0.000000,0.989572,0.056929,-0.003556,-0.000791,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.190187,0.093329,-0.011659,-0.001539,0.000016,-0.000014,0.000014,-0.000011,0.163993,-0.057667,0.007203,0.000951,-0.000010,0.000009,-0.000009,0.000007,1.042154,0.047444,-0.005925,-0.000783,0.000008,-0.000007,0.000007,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.270322,0.065389,-0.016354,-0.001737,-0.000243,0.000109,-0.000252,0.000186,0.114478,-0.040404,0.010105,0.001074,0.000150,-0.000067,0.000156,-0.000115,1.082893,0.033244,-0.008313,-0.000884,-0.000124,0.000055,-0.000128,0.000095,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.317420,0.026831,-0.021809,-0.000148,0.003035,-0.000005,0.003753,-0.002486,0.085376,-0.016579,0.013476,0.000092,-0.001876,0.000003,-0.002319,0.001536,1.106838,0.013642,-0.011088,-0.000076,0.001543,-0.000003,0.001908,-0.001264,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/8.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/8.csv
new file mode 100644
index 0000000000000000000000000000000000000000..77658489fbf513b488b90fe7c4e8865004bfb325
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/8.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/2/9.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/2/9.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0ba483ae75dbf9f9adc29b965936375a3c712ec7
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/2/9.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/1.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ec56e5b1d1ae5c2cbc6c7ee342d4f620aa731d8b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/1.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-0.500000,0.000000,0.000000,0.000000,0.030855,-0.033073,0.015203,-0.002769,0.000000,0.000000,0.000000,0.000000,0.002072,-0.002221,0.001021,-0.000186,1.500000,0.000000,0.000000,0.000000,-0.016752,0.017956,-0.008254,0.001503,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.489784,0.029890,0.024296,-0.000166,-0.003382,0.002792,-0.001171,0.000207,0.000686,0.002008,0.001632,-0.000011,-0.000227,0.000187,-0.000079,0.000014,1.494454,-0.016228,-0.013191,0.000090,0.001836,-0.001516,0.000636,-0.000113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.437316,0.072844,0.018218,-0.001936,0.000271,-0.000187,0.000072,-0.000013,0.004210,0.004893,0.001224,-0.000130,0.000018,-0.000013,0.000005,-0.000001,1.465969,-0.039547,-0.009890,0.001051,-0.000147,0.000102,-0.000039,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.348047,0.103966,0.012986,-0.001714,-0.000017,0.000010,-0.000003,0.000001,0.010208,0.006986,0.000874,-0.000115,-0.000001,0.000001,-0.000000,0.000000,1.417504,-0.056443,-0.007050,0.000931,0.000009,-0.000005,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.232819,0.124759,0.007799,-0.001733,0.000001,-0.000000,0.000000,-0.000000,0.017952,0.008386,0.000526,-0.000116,0.000000,-0.000000,-0.000000,0.000000,1.354949,-0.067729,-0.004233,0.000941,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.101994,0.135158,0.002601,-0.001732,-0.000000,0.000000,-0.000000,0.000000,0.026749,0.009090,0.000177,-0.000116,-0.000000,0.000000,-0.000000,0.000000,1.283927,-0.073373,-0.001411,0.000940,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.034033,0.135162,-0.002597,-0.001733,-0.000000,-0.000000,0.000000,-0.000000,0.035899,0.009095,-0.000173,-0.000117,-0.000000,0.000000,-0.000000,-0.000000,1.210084,-0.073373,0.001411,0.000940,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.164865,0.124769,-0.007796,-0.001734,-0.000001,0.000001,-0.000001,0.000001,0.044705,0.008399,-0.000523,-0.000117,-0.000000,0.000000,-0.000000,0.000000,1.139062,-0.067730,0.004233,0.000941,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.280105,0.103979,-0.012986,-0.001715,0.000017,-0.000016,0.000015,-0.000013,0.052464,0.007002,-0.000873,-0.000116,0.000001,-0.000001,0.000001,-0.000001,1.076506,-0.056443,0.007050,0.000931,-0.000009,0.000009,-0.000008,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.369386,0.072855,-0.018220,-0.001936,-0.000271,0.000121,-0.000281,0.000208,0.058477,0.004907,-0.001227,-0.000131,-0.000018,0.000008,-0.000019,0.000014,1.028042,-0.039548,0.009890,0.001051,0.000147,-0.000066,0.000153,-0.000113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.421862,0.029895,-0.024300,-0.000166,0.003382,-0.000006,0.004182,-0.002770,0.062012,0.002014,-0.001637,-0.000011,0.000228,-0.000000,0.000282,-0.000187,0.999556,-0.016228,0.013191,0.000090,-0.001836,0.000003,-0.002270,0.001503,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/10.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/10.csv
new file mode 100644
index 0000000000000000000000000000000000000000..28c2749db086ffc33b0da8da0f18f5e7318245bb
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/10.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/11.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/11.csv
new file mode 100644
index 0000000000000000000000000000000000000000..f61ce946aab75954a1737c3329a32e7354c7b579
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/11.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,-0.288500,0.000000,0.000000,0.000000,0.017584,-0.018909,0.008705,-0.001587,0.465220,0.000000,0.000000,0.000000,-0.000032,-0.000017,0.000018,-0.000005,1.444600,0.000000,0.000000,-0.000000,0.003087,-0.003282,0.001503,-0.000273,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.282707,0.016910,0.013658,-0.000204,-0.001934,0.001601,-0.000673,0.000119,0.465185,-0.000135,-0.000183,-0.000091,-0.000002,0.000005,-0.000003,0.000001,1.445635,0.003043,0.002511,0.000030,-0.000335,0.000275,-0.000115,0.000020,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.253229,0.040683,0.009867,-0.001209,0.000156,-0.000108,0.000042,-0.000007,0.464777,-0.000771,-0.000450,-0.000087,0.000001,-0.000001,0.000000,-0.000000,1.451064,0.007643,0.002043,-0.000148,0.000027,-0.000018,0.000007,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.203806,0.057073,0.006570,-0.001081,-0.000009,0.000005,-0.000001,0.000000,0.463469,-0.001932,-0.000710,-0.000086,0.000000,-0.000000,0.000000,-0.000000,1.460617,0.011335,0.001658,-0.000126,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.141249,0.066952,0.003307,-0.001089,-0.000000,0.000003,-0.000007,0.000002,0.460741,-0.003610,-0.000967,-0.000085,-0.000000,0.000002,-0.000003,0.000001,1.473483,0.014270,0.001276,-0.000127,0.000000,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.072081,0.070290,0.000020,-0.001110,-0.000003,-0.000014,0.000044,-0.000009,0.456079,-0.005802,-0.001231,-0.000094,-0.000001,-0.000007,0.000020,-0.000004,1.488903,0.016443,0.000898,-0.000124,0.000001,0.000002,-0.000007,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.002863,0.067114,-0.003008,-0.000709,0.000266,-0.000060,-0.000019,0.000009,0.448960,-0.008494,-0.001377,0.000088,0.000121,-0.000027,-0.000009,0.000004,1.506117,0.017850,0.000479,-0.000187,-0.000042,0.000009,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.060731,0.059687,-0.004232,-0.000307,-0.000003,-0.000011,0.000009,-0.000002,0.439266,-0.010659,-0.000702,0.000271,-0.000001,-0.000005,0.000004,-0.000001,1.524229,0.018135,-0.000224,-0.000250,0.000000,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.115871,0.050272,-0.005195,-0.000327,-0.000000,0.000003,-0.000002,0.000001,0.428172,-0.011264,0.000093,0.000263,0.000001,0.000001,-0.000001,0.000000,1.541891,0.016940,-0.000970,-0.000248,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.160622,0.038907,-0.006164,-0.000315,0.000007,-0.000005,0.000005,-0.000004,0.417265,-0.010286,0.000887,0.000266,-0.000001,0.000002,-0.000002,0.000002,1.557612,0.014254,-0.001717,-0.000249,0.000002,-0.000002,0.000002,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.193053,0.025637,-0.007131,-0.000382,-0.000085,0.000036,-0.000086,0.000065,0.408134,-0.007707,0.001704,0.000297,0.000033,-0.000015,0.000034,-0.000024,1.569901,0.010070,-0.002480,-0.000283,-0.000039,0.000017,-0.000040,0.000029,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.211108,0.010013,-0.008343,0.000201,0.001084,0.000007,0.001310,-0.000880,0.402455,-0.003321,0.002635,0.000099,-0.000391,0.000003,-0.000493,0.000323,1.577176,0.004160,-0.003370,-0.000037,0.000473,-0.000001,0.000587,-0.000388,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/12.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/12.csv
new file mode 100644
index 0000000000000000000000000000000000000000..26063ed6527568d88d174ab367fac374d576ea05
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/12.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.214500,0.000000,0.000000,0.000000,0.010067,-0.010835,0.004990,-0.000910,0.401310,0.000000,0.000000,-0.000000,0.012216,-0.013138,0.006049,-0.001103,1.578600,0.000000,0.000000,0.000000,-0.015462,0.016632,-0.007658,0.001396,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.217812,0.009663,0.007791,-0.000133,-0.001108,0.000918,-0.000385,0.000068,0.405334,0.011745,0.009484,-0.000144,-0.001344,0.001112,-0.000467,0.000083,1.573509,-0.014859,-0.011994,0.000188,0.001701,-0.001408,0.000592,-0.000105,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.234626,0.023168,0.005575,-0.000702,0.000094,-0.000062,0.000023,-0.000004,0.425804,0.028245,0.006843,-0.000842,0.000109,-0.000075,0.000029,-0.000005,1.547624,-0.035703,-0.008634,0.001069,-0.000140,0.000095,-0.000036,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.262719,0.032391,0.003681,-0.000616,-0.000004,0.000003,-0.000001,0.000000,0.460108,0.039606,0.004551,-0.000752,-0.000007,0.000004,-0.000001,0.000000,1.504281,-0.050023,-0.005731,0.000950,0.000008,-0.000005,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.298173,0.037900,0.001827,-0.000617,0.000001,-0.000000,0.000000,-0.000000,0.503509,0.046437,0.002277,-0.000760,0.000000,-0.000000,0.000000,-0.000000,1.449482,-0.058618,-0.002861,0.000958,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.337284,0.039704,-0.000022,-0.000616,0.000000,-0.000000,0.000000,-0.000000,0.551463,0.048712,-0.000002,-0.000759,0.000000,-0.000000,0.000000,-0.000000,1.388960,-0.061469,0.000010,0.000957,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.376349,0.037812,-0.001870,-0.000616,-0.000000,0.000000,-0.000000,0.000000,0.599415,0.046431,-0.002280,-0.000760,-0.000000,0.000001,-0.000000,0.000000,1.328458,-0.058578,0.002881,0.000957,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.411675,0.032224,-0.003715,-0.000610,0.000006,-0.000005,0.000005,-0.000004,0.642807,0.039594,-0.004554,-0.000752,0.000007,-0.000006,0.000006,-0.000005,1.273718,-0.049945,0.005748,0.000948,-0.000009,0.000008,-0.000008,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.439575,0.022963,-0.005573,-0.000681,-0.000088,0.000040,-0.000092,0.000067,0.677097,0.028228,-0.006845,-0.000840,-0.000108,0.000049,-0.000113,0.000083,1.230466,-0.035603,0.008635,0.001059,0.000136,-0.000061,0.000142,-0.000104,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.456212,0.009542,-0.007708,-0.000113,0.001091,-0.000004,0.001356,-0.000895,0.697551,0.011734,-0.009477,-0.000141,0.001342,-0.000005,0.001668,-0.001101,1.204669,-0.014798,0.011953,0.000178,-0.001692,0.000006,-0.002104,0.001389,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/13.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/13.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3fbe05c1386a1547128879f21721af960bef469e
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/13.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.459480,0.000000,0.000000,-0.000000,-0.006753,0.007262,-0.003343,0.000609,0.701570,0.000000,0.000000,-0.000000,-0.009620,0.010345,-0.004762,0.000868,1.199600,0.000000,0.000000,0.000000,0.021282,-0.022886,0.010536,-0.001921,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.457255,-0.006494,-0.005245,0.000078,0.000743,-0.000615,0.000258,-0.000046,0.698401,-0.009252,-0.007472,0.000111,0.001058,-0.000876,0.000368,-0.000065,1.206611,0.020466,0.016530,-0.000247,-0.002341,0.001938,-0.000814,0.000144,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.445934,-0.015624,-0.003789,0.000465,-0.000060,0.000041,-0.000016,0.000003,0.682273,-0.022258,-0.005398,0.000662,-0.000085,0.000059,-0.000023,0.000004,1.242286,0.049234,0.011938,-0.001465,0.000188,-0.000130,0.000051,-0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.426954,-0.021917,-0.002521,0.000416,0.000004,-0.000002,0.000001,-0.000000,0.655234,-0.031223,-0.003592,0.000593,0.000005,-0.000003,0.000001,-0.000000,1.302093,0.069057,0.007941,-0.001312,-0.000012,0.000007,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.402935,-0.025702,-0.001262,0.000421,-0.000000,0.000000,-0.000000,0.000000,0.621015,-0.036617,-0.001800,0.000599,-0.000000,0.000000,-0.000000,0.000000,1.377773,0.080978,0.003974,-0.001325,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.376391,-0.026965,-0.000001,0.000420,0.000000,0.000000,-0.000000,0.000000,0.583198,-0.038420,-0.000003,0.000599,0.000000,0.000000,0.000000,0.000000,1.461400,0.084952,0.000000,-0.001324,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.349845,-0.025706,0.001260,0.000421,0.000000,-0.000000,0.000000,-0.000000,0.545374,-0.036629,0.001794,0.000600,0.000000,-0.000000,0.000000,-0.000000,1.545027,0.080978,-0.003974,-0.001325,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.325821,-0.021924,0.002520,0.000417,-0.000004,0.000004,-0.000003,0.000003,0.511138,-0.031243,0.003590,0.000594,-0.000005,0.000005,-0.000005,0.000004,1.620707,0.069057,-0.007941,-0.001312,0.000012,-0.000011,0.000011,-0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.306833,-0.015632,0.003790,0.000465,0.000060,-0.000027,0.000062,-0.000046,0.484078,-0.022279,0.005400,0.000664,0.000085,-0.000038,0.000089,-0.000065,1.680514,0.049234,-0.011938,-0.001465,-0.000188,0.000085,-0.000197,0.000144,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.295506,-0.006498,0.005248,0.000079,-0.000743,0.000003,-0.000924,0.000610,0.467933,-0.009262,0.007480,0.000112,-0.001060,0.000004,-0.001317,0.000869,1.716189,0.020466,-0.016530,-0.000247,0.002341,-0.000009,0.002910,-0.001921,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/14.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/14.csv
new file mode 100644
index 0000000000000000000000000000000000000000..01b44adb4426f91b44c92901c0b28204da8c6786
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/14.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.293280,0.000000,0.000000,0.000000,-0.011264,0.012113,-0.005576,0.001017,0.464760,0.000000,0.000000,-0.000000,0.026131,-0.028101,0.012937,-0.002359,1.723200,0.000000,0.000000,0.000000,-0.009511,0.010228,-0.004708,0.000858,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.289569,-0.010834,-0.008751,0.000129,0.001239,-0.001025,0.000431,-0.000076,0.473368,0.025127,0.020292,-0.000305,-0.002874,0.002379,-0.001000,0.000177,1.720067,-0.009147,-0.007388,0.000110,0.001046,-0.000866,0.000364,-0.000065,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.270681,-0.026070,-0.006325,0.000774,-0.000099,0.000069,-0.000027,0.000005,0.517164,0.060437,0.014650,-0.001801,0.000231,-0.000160,0.000062,-0.000011,1.704121,-0.022007,-0.005338,0.000654,-0.000084,0.000058,-0.000023,0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.239007,-0.036579,-0.004214,0.000693,0.000006,-0.000004,0.000001,-0.000000,0.590572,0.084754,0.009737,-0.001613,-0.000015,0.000009,-0.000003,0.000001,1.677385,-0.030873,-0.003553,0.000586,0.000005,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.198911,-0.042912,-0.002116,0.000701,0.000000,0.000000,-0.000000,0.000000,0.683442,0.099360,0.004862,-0.001628,0.000001,-0.000000,0.000000,-0.000000,1.643548,-0.036210,-0.001781,0.000592,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.154585,-0.045039,-0.000011,0.000702,0.000000,0.000000,-0.000000,0.000000,0.786037,0.104203,-0.000017,-0.001625,0.000001,-0.000000,-0.000000,0.000000,1.606149,-0.037997,-0.000005,0.000592,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.110238,-0.042951,0.002100,0.000704,0.000000,-0.000001,0.000000,-0.000000,0.888599,0.099297,-0.004889,-0.001623,-0.000000,0.000001,-0.000001,0.000001,1.568739,-0.036230,0.001773,0.000593,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.070091,-0.036640,0.004208,0.000698,-0.000006,0.000006,-0.000006,0.000005,0.981384,0.084654,-0.009746,-0.001606,0.000015,-0.000014,0.000013,-0.000011,1.534875,-0.030904,0.003550,0.000588,-0.000005,0.000005,-0.000005,0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.038355,-0.026129,0.006333,0.000779,0.000100,-0.000045,0.000104,-0.000077,1.054690,0.060340,-0.014637,-0.001793,-0.000230,0.000104,-0.000241,0.000177,1.508108,-0.022037,0.005342,0.000657,0.000084,-0.000038,0.000088,-0.000065,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.019419,-0.010864,0.008773,0.000132,-0.001243,0.000005,-0.001545,0.001020,1.098410,0.025078,-0.020256,-0.000300,0.002868,-0.000010,0.003565,-0.002353,1.492139,-0.009162,0.007399,0.000111,-0.001048,0.000004,-0.001303,0.000860,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/15.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/15.csv
new file mode 100644
index 0000000000000000000000000000000000000000..1b1fdc7b38d0abff6aa9f671c26fc1e8b3f2e945
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/15.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.015698,0.000000,0.000000,0.000000,-0.016377,0.017620,-0.008113,0.001479,1.107000,0.000000,0.000000,0.000000,-0.026136,0.028078,-0.012920,0.002355,1.489000,0.000000,0.000000,0.000000,-0.001705,0.001830,-0.000842,0.000153,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.010307,-0.015731,-0.012693,0.000206,0.001802,-0.001492,0.000627,-0.000111,1.098377,-0.025191,-0.020386,0.000254,0.002872,-0.002375,0.000998,-0.000177,1.488437,-0.001646,-0.001335,0.000014,0.000187,-0.000155,0.000065,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.017086,-0.037769,-0.009114,0.001141,-0.000145,0.000100,-0.000039,0.000007,1.054370,-0.060841,-0.014891,0.001756,-0.000228,0.000160,-0.000062,0.000011,1.485556,-0.003989,-0.000984,0.000113,-0.000015,0.000010,-0.000004,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.062905,-0.052841,-0.006004,0.001018,0.000007,-0.000006,0.000002,-0.000000,0.980276,-0.085761,-0.010091,0.001584,0.000020,-0.000008,0.000003,-0.000001,1.480688,-0.005644,-0.000674,0.000103,0.000002,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.120729,-0.061787,-0.002944,0.001017,-0.000003,0.000000,0.000000,-0.000000,0.886022,-0.101137,-0.005265,0.001626,0.000007,-0.000000,0.000001,-0.000000,1.474475,-0.006677,-0.000357,0.000108,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.184446,-0.064637,0.000089,0.001006,-0.000002,-0.000000,0.000000,-0.000000,0.781254,-0.106758,-0.000342,0.001657,0.000009,-0.000001,0.000001,-0.000000,1.467550,-0.007062,-0.000026,0.000112,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.247989,-0.061447,0.003098,0.001000,-0.000001,-0.000001,0.000000,-0.000000,0.675819,-0.102435,0.004685,0.001697,0.000012,-0.000002,0.000002,-0.000001,1.460574,-0.006775,0.000315,0.000114,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.305341,-0.052262,0.006079,0.000982,-0.000011,0.000009,-0.000008,0.000007,0.579777,-0.087936,0.009827,0.001721,-0.000006,0.000013,-0.000014,0.000011,1.454229,-0.005802,0.000658,0.000113,-0.000001,0.000001,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.350547,-0.037165,0.009057,0.001089,0.000140,-0.000063,0.000148,-0.000109,0.503393,-0.063082,0.015114,0.001947,0.000248,-0.000112,0.000256,-0.000187,1.449198,-0.004146,0.001002,0.000125,0.000016,-0.000007,0.000017,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.377451,-0.015416,0.012464,0.000169,-0.001760,0.000006,-0.002186,0.001444,0.457576,-0.026355,0.021232,0.000386,-0.003027,0.000013,-0.003772,0.002486,1.446191,-0.001726,0.001393,0.000022,-0.000198,0.000001,-0.000246,0.000162,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/16.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/16.csv
new file mode 100644
index 0000000000000000000000000000000000000000..48e20b0370ddec6731a7a958ab69cc8f64aa8666
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/16.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.382730,0.000000,0.000000,-0.000000,0.001781,-0.001915,0.000881,-0.000161,0.448540,0.000000,0.000000,0.000000,0.004218,-0.004537,0.002089,-0.000381,1.445600,0.000000,0.000000,0.000000,0.019076,-0.020514,0.009444,-0.001722,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.382143,0.001715,0.001386,-0.000019,-0.000196,0.000162,-0.000068,0.000012,0.449929,0.004055,0.003274,-0.000050,-0.000464,0.000384,-0.000161,0.000029,1.451884,0.018344,0.014816,-0.000222,-0.002098,0.001737,-0.000730,0.000129,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.379152,0.004131,0.001005,-0.000121,0.000016,-0.000011,0.000004,-0.000001,0.456995,0.009747,0.002359,-0.000292,0.000037,-0.000026,0.000010,-0.000002,1.483860,0.044128,0.010699,-0.001314,0.000168,-0.000117,0.000045,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.374128,0.005806,0.000674,-0.000109,-0.000001,0.000001,-0.000000,0.000000,0.468829,0.013659,0.001564,-0.000261,-0.000002,0.000001,-0.000000,0.000000,1.537463,0.061892,0.007116,-0.001176,-0.000011,0.000006,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.367758,0.006825,0.000344,-0.000111,-0.000000,-0.000000,0.000000,-0.000000,0.483789,0.015998,0.000775,-0.000263,0.000000,-0.000000,0.000000,0.000000,1.605288,0.072572,0.003559,-0.001188,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.360699,0.007181,0.000011,-0.000112,-0.000000,-0.000000,0.000000,-0.000000,0.500299,0.016759,-0.000013,-0.000262,0.000000,0.000000,-0.000000,0.000000,1.680231,0.076127,-0.000003,-0.001187,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.353619,0.006867,-0.000327,-0.000113,-0.000000,0.000000,-0.000000,0.000000,0.516784,0.015950,-0.000796,-0.000260,0.000000,0.000000,-0.000000,0.000000,1.755168,0.072560,-0.003564,-0.001187,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.347193,0.005872,-0.000668,-0.000113,0.000001,-0.000001,0.000001,-0.000001,0.531679,0.013580,-0.001572,-0.000256,0.000003,-0.000002,0.000002,-0.000002,1.822977,0.061873,-0.007118,-0.001175,0.000011,-0.000010,0.000010,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.342101,0.004196,-0.001013,-0.000127,-0.000016,0.000007,-0.000017,0.000012,0.543432,0.009669,-0.002350,-0.000285,-0.000037,0.000017,-0.000039,0.000028,1.876559,0.044109,-0.010697,-0.001312,-0.000168,0.000076,-0.000176,0.000129,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.339059,0.001747,-0.001410,-0.000023,0.000200,-0.000001,0.000249,-0.000164,0.550435,0.004015,-0.003244,-0.000046,0.000459,-0.000002,0.000570,-0.000376,1.908520,0.018334,-0.014808,-0.000221,0.002097,-0.000008,0.002607,-0.001721,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/17.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/17.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3f315cbf1247fc8ee98ffc8de0e1ea2c87ea0dd3
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/17.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/18.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/18.csv
new file mode 100644
index 0000000000000000000000000000000000000000..c0bbb1322001b81b064d1d38f0a5dadbd5dce7b6
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/18.csv
@@ -0,0 +1,8 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/19.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/19.csv
new file mode 100644
index 0000000000000000000000000000000000000000..8276d51aef9895979197a6b1d8ec0b0d320e5712
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/19.csv
@@ -0,0 +1,20 @@
+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,
+1.000000,0.383330,0.000000,0.000000,0.000000,-0.003940,0.003930,-0.001745,0.000311,0.091948,0.000000,0.000000,0.000000,0.011068,-0.011957,0.005516,-0.001007,1.979800,0.000000,0.000000,-0.000000,-0.021059,0.022273,-0.010177,0.001846,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.381886,-0.004404,-0.003989,-0.000486,0.000408,-0.000308,0.000124,-0.000021,0.095568,0.010535,0.008433,-0.000221,-0.001221,0.001017,-0.000428,0.000076,1.972683,-0.021008,-0.017503,-0.000421,0.002275,-0.001858,0.000774,-0.000136,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.373212,-0.013146,-0.004655,-0.000188,-0.000010,0.000019,-0.000006,0.000001,0.113758,0.024899,0.005784,-0.000840,0.000103,-0.000068,0.000027,-0.000005,1.934806,-0.053779,-0.014953,0.000803,-0.000178,0.000123,-0.000047,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.355227,-0.022997,-0.005165,-0.000132,0.000023,-0.000001,0.000000,-0.000000,0.143658,0.034146,0.003502,-0.000739,-0.000000,0.000004,-0.000002,0.000000,1.866782,-0.081601,-0.012921,0.000660,0.000013,-0.000006,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.326955,-0.033634,-0.005428,-0.000044,0.000021,-0.000001,0.000001,-0.000000,0.180569,0.038943,0.001304,-0.000722,0.000006,-0.000000,-0.000000,0.000000,1.772928,-0.105433,-0.010902,0.000678,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.287871,-0.044535,-0.005431,0.000044,0.000024,0.000002,0.000000,-0.000000,0.220101,0.039411,-0.000824,-0.000695,0.000009,0.000002,-0.000001,0.000000,1.657271,-0.125201,-0.008863,0.000681,0.000001,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.237975,-0.055159,-0.005137,0.000155,0.000029,-0.000001,0.000001,-0.000000,0.258004,0.035723,-0.002839,-0.000646,0.000015,0.000001,-0.000000,0.000000,1.523889,-0.140879,-0.006812,0.000686,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.177863,-0.064852,-0.004497,0.000272,0.000030,-0.000001,0.000001,-0.000000,0.290257,0.028171,-0.004678,-0.000575,0.000023,0.000003,-0.000001,0.000000,1.376885,-0.152441,-0.004748,0.000690,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.108816,-0.072910,-0.003500,0.000394,0.000030,-0.000003,0.000001,-0.000000,0.313201,0.017194,-0.006241,-0.000460,0.000036,0.000002,0.000001,0.000000,1.220387,-0.159865,-0.002676,0.000690,-0.000000,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.032828,-0.078616,-0.002153,0.000500,0.000023,-0.000003,0.000001,-0.000000,0.323733,0.003493,-0.007369,-0.000273,0.000064,0.000011,-0.000005,0.000001,1.058536,-0.163149,-0.000611,0.000684,-0.000004,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.047421,-0.081342,-0.000537,0.000570,0.000010,-0.000006,0.000001,-0.000000,0.319655,-0.011779,-0.007758,0.000012,0.000063,-0.000002,0.000009,-0.000003,0.895455,-0.162340,0.001407,0.000661,-0.000006,-0.000000,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.128726,-0.080692,0.001183,0.000560,-0.000017,-0.000008,0.000003,-0.000001,0.300197,-0.026979,-0.007280,0.000338,0.000100,-0.000001,-0.000003,0.000000,0.735177,-0.157570,0.003349,0.000633,-0.000007,0.000001,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.207695,-0.076734,0.002723,0.000459,-0.000028,-0.000000,0.000001,-0.000000,0.266373,-0.040143,-0.005707,0.000688,0.000067,-0.000014,0.000003,-0.000000,0.581583,-0.148998,0.005213,0.000613,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.281274,-0.070020,0.003938,0.000356,-0.000022,0.000003,-0.000000,0.000000,0.221267,-0.049282,-0.003350,0.000856,0.000024,-0.000007,0.000002,-0.000000,0.438409,-0.136740,0.007042,0.000608,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.347019,-0.061151,0.004898,0.000290,-0.000013,0.000001,-0.000000,0.000000,0.169509,-0.053344,-0.000686,0.000910,0.000008,-0.000002,0.000000,-0.000000,0.309319,-0.120835,0.008862,0.000606,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.402994,-0.050531,0.005703,0.000250,-0.000007,0.000001,0.000000,-0.000000,0.116396,-0.051960,0.002081,0.000931,0.000003,-0.000001,0.000000,-0.000000,0.197952,-0.101293,0.010680,0.000606,0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.447579,-0.038400,0.006417,0.000226,-0.000007,0.000005,-0.000004,0.000004,0.067451,-0.044997,0.004881,0.000929,-0.000007,0.000007,-0.000007,0.000006,0.107944,-0.078118,0.012491,0.000595,-0.000011,0.000009,-0.000009,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.479339,-0.024891,0.007115,0.000289,0.000080,-0.000034,0.000080,-0.000061,0.028262,-0.032444,0.007710,0.001032,0.000126,-0.000057,0.000133,-0.000097,0.042911,-0.051345,0.014335,0.000738,0.000172,-0.000073,0.000171,-0.000129,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.496760,-0.009588,0.008045,-0.000263,-0.001024,-0.000009,-0.001229,0.000830,0.004664,-0.013596,0.010938,0.000219,-0.001566,0.000008,-0.001952,0.001286,0.006779,-0.020019,0.016694,-0.000420,-0.002163,-0.000015,-0.002612,0.001756,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/2.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0bb2ea059b769bbfd113876b792a9b8dd913a5b2
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/2.csv
@@ -0,0 +1,17 @@
+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,
+1.000000,0.432080,0.000000,0.000000,0.000000,0.001632,-0.001731,0.000792,-0.000144,0.062700,0.000000,0.000000,0.000000,-0.002773,0.002943,-0.001347,0.000245,0.994010,0.000000,0.000000,0.000000,0.001836,-0.001949,0.000892,-0.000162,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.432629,0.001617,0.001340,0.000023,-0.000177,0.000145,-0.000060,0.000011,0.061768,-0.002745,-0.002273,-0.000037,0.000301,-0.000246,0.000103,-0.000018,0.994627,0.001818,0.001506,0.000025,-0.000199,0.000163,-0.000068,0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.435528,0.004097,0.001115,-0.000071,0.000014,-0.000010,0.000004,-0.000001,0.056851,-0.006944,-0.001884,0.000123,-0.000024,0.000016,-0.000006,0.000001,0.997885,0.004600,0.001249,-0.000081,0.000016,-0.000011,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.440676,0.006140,0.000932,-0.000059,-0.000001,0.000000,-0.000000,0.000000,0.048133,-0.010389,-0.001568,0.000103,0.000002,-0.000001,0.000000,-0.000000,1.003661,0.006883,0.001039,-0.000068,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.447688,0.007824,0.000752,-0.000060,0.000000,-0.000000,0.000000,-0.000000,0.036280,-0.013213,-0.001255,0.000105,-0.000000,0.000000,-0.000000,0.000000,1.011515,0.008756,0.000833,-0.000069,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.456203,0.009146,0.000570,-0.000060,-0.000000,-0.000000,-0.000000,-0.000000,0.021916,-0.015410,-0.000942,0.000104,0.000000,0.000000,-0.000000,0.000000,1.021035,0.010214,0.000625,-0.000069,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.465859,0.010105,0.000389,-0.000061,-0.000000,-0.000000,-0.000000,-0.000000,0.005669,-0.016980,-0.000629,0.000104,-0.000000,-0.000000,0.000000,0.000000,1.031805,0.011258,0.000418,-0.000069,-0.000000,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.476293,0.010701,0.000207,-0.000061,-0.000000,-0.000000,0.000000,-0.000000,-0.011835,-0.017924,-0.000315,0.000104,0.000000,0.000000,-0.000000,0.000000,1.043413,0.011887,0.000211,-0.000069,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.487140,0.010932,0.000024,-0.000061,-0.000000,-0.000000,-0.000000,0.000000,-0.029969,-0.018241,-0.000002,0.000104,0.000000,0.000000,0.000000,-0.000000,1.055442,0.012102,0.000004,-0.000069,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.498034,0.010795,-0.000162,-0.000062,-0.000000,-0.000000,0.000000,0.000000,-0.048108,-0.017932,0.000311,0.000105,0.000000,0.000000,-0.000000,0.000000,1.067478,0.011902,-0.000204,-0.000069,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.508605,0.010283,-0.000351,-0.000064,-0.000000,0.000000,-0.000000,-0.000000,-0.065623,-0.016995,0.000625,0.000105,0.000000,0.000000,-0.000000,0.000000,1.079107,0.011286,-0.000412,-0.000069,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.518473,0.009389,-0.000544,-0.000065,-0.000000,0.000000,-0.000000,-0.000000,-0.081888,-0.015430,0.000939,0.000105,0.000000,0.000000,0.000000,0.000000,1.089910,0.010253,-0.000621,-0.000070,-0.000000,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.527252,0.008103,-0.000742,-0.000067,-0.000001,-0.000000,-0.000000,0.000000,-0.096274,-0.013237,0.001254,0.000105,0.000000,-0.000000,0.000000,-0.000000,1.099472,0.008801,-0.000831,-0.000070,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.534546,0.006417,-0.000945,-0.000068,0.000000,-0.000001,0.000001,-0.000001,-0.108152,-0.010413,0.001569,0.000104,-0.000001,0.000001,-0.000001,0.000001,1.107372,0.006929,-0.001041,-0.000070,0.000001,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.539949,0.004319,-0.001157,-0.000082,-0.000015,0.000007,-0.000015,0.000011,-0.116893,-0.006964,0.001888,0.000124,0.000024,-0.000010,0.000024,-0.000018,1.113191,0.004637,-0.001256,-0.000083,-0.000016,0.000007,-0.000016,0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.543016,0.001718,-0.001418,0.000018,0.000189,0.000001,0.000231,-0.000154,-0.121825,-0.002754,0.002280,-0.000037,-0.000302,-0.000001,-0.000367,0.000246,1.116477,0.001836,-0.001519,0.000024,0.000201,0.000001,0.000245,-0.000164,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/3.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..fc737f799a04af778fc516bea1f586d5fdffefc2
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/3.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/4.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..30f8a79199f6649cc1352c874e0e309e5e670e50
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/4.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.455420,0.000000,0.000000,0.000000,-0.033723,0.036265,-0.016695,0.003044,0.599310,0.000000,0.000000,0.000000,-0.021226,0.022826,-0.010508,0.001916,1.132600,0.000000,0.000000,0.000000,0.020713,-0.022274,0.010254,-0.001869,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.444311,-0.032430,-0.026192,0.000391,0.003709,-0.003070,0.001290,-0.000229,0.592318,-0.020412,-0.016486,0.000246,0.002335,-0.001933,0.000812,-0.000144,1.139423,0.019918,0.016087,-0.000240,-0.002278,0.001886,-0.000793,0.000141,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.387780,-0.078015,-0.018917,0.002322,-0.000298,0.000207,-0.000080,0.000014,0.556735,-0.049106,-0.011907,0.001461,-0.000187,0.000130,-0.000051,0.000009,1.174144,0.047917,0.011619,-0.001426,0.000183,-0.000127,0.000049,-0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.293012,-0.109426,-0.012583,0.002079,0.000019,-0.000011,0.000004,-0.000001,0.497085,-0.068877,-0.007921,0.001309,0.000012,-0.000007,0.000002,-0.000000,1.232351,0.067210,0.007729,-0.001277,-0.000012,0.000007,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.173094,-0.128314,-0.006296,0.002100,-0.000001,0.000000,-0.000000,0.000000,0.421602,-0.080768,-0.003964,0.001322,-0.000001,0.000000,-0.000000,0.000000,1.306007,0.078812,0.003868,-0.001290,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.040583,-0.134609,0.000001,0.002099,-0.000000,0.000000,-0.000000,0.000000,0.338191,-0.084733,-0.000001,0.001321,0.000000,0.000000,-0.000000,0.000000,1.387397,0.082680,0.000000,-0.001289,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.091926,-0.128311,0.006297,0.002100,0.000001,-0.000002,0.000001,-0.000001,0.254779,-0.080771,0.003963,0.001322,0.000001,-0.000001,0.000000,-0.000000,1.468789,0.078813,-0.003867,-0.001290,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.211841,-0.109422,0.012584,0.002079,-0.000019,0.000018,-0.000017,0.000014,0.179292,-0.068882,0.007921,0.001309,-0.000012,0.000011,-0.000011,0.000009,1.542445,0.067212,-0.007729,-0.001277,0.000012,-0.000011,0.000011,-0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.306604,-0.078011,0.018917,0.002321,0.000298,-0.000135,0.000312,-0.000229,0.119637,-0.049110,0.011908,0.001462,0.000187,-0.000085,0.000196,-0.000144,1.600654,0.047919,-0.011619,-0.001426,-0.000183,0.000083,-0.000192,0.000141,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.363132,-0.032428,0.026191,0.000391,-0.003709,0.000014,-0.004611,0.003043,0.084051,-0.020415,0.016488,0.000246,-0.002335,0.000009,-0.002903,0.001916,1.635376,0.019919,-0.016088,-0.000240,0.002278,-0.000008,0.002832,-0.001869,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/5.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/5.csv
new file mode 100644
index 0000000000000000000000000000000000000000..b862c0515b8cb74f1541401a5b1980703bb2b5e4
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/5.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/6.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/6.csv
new file mode 100644
index 0000000000000000000000000000000000000000..352a1fd2ab52b356bae0ec88bb26582b248f38cb
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/6.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.512970,0.000000,0.000000,0.000000,0.008436,-0.009087,0.004186,-0.000764,-0.486680,0.000000,0.000000,0.000000,0.054899,-0.059034,0.027176,-0.004954,1.173500,0.000000,0.000000,0.000000,0.008073,-0.008682,0.003997,-0.000729,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.510198,0.008082,0.006506,-0.000125,-0.000930,0.000770,-0.000324,0.000058,-0.468594,0.052800,0.042648,-0.000632,-0.006038,0.004998,-0.002100,0.000373,1.176159,0.007763,0.006269,-0.000094,-0.000888,0.000735,-0.000309,0.000055,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.496160,0.019313,0.004607,-0.000605,0.000076,-0.000052,0.000020,-0.000004,-0.376546,0.127041,0.030817,-0.003776,0.000484,-0.000336,0.000131,-0.000023,1.189690,0.018672,0.004525,-0.000557,0.000071,-0.000050,0.000019,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.472805,0.026853,0.002960,-0.000536,-0.000001,0.000003,-0.000001,0.000000,-0.222208,0.178228,0.020515,-0.003383,-0.000032,0.000018,-0.000006,0.000001,1.212367,0.026178,0.003002,-0.000502,-0.000005,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.443527,0.031171,0.001363,-0.000528,0.000002,-0.000001,0.000000,0.000000,-0.026868,0.209042,0.010283,-0.003419,0.000001,-0.000000,0.000000,-0.000000,1.241042,0.030664,0.001481,-0.000509,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.411520,0.032317,-0.000216,-0.000525,0.000001,0.000001,0.000001,-0.000000,0.189039,0.219355,0.000031,-0.003417,-0.000000,-0.000000,-0.000000,0.000000,1.272677,0.032097,-0.000048,-0.000510,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.379942,0.030324,-0.001762,-0.000497,0.000013,0.000003,-0.000002,0.000000,0.405007,0.209162,-0.010227,-0.003424,-0.000004,0.000002,-0.000001,0.000001,1.304217,0.030475,-0.001570,-0.000503,0.000004,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.351862,0.025369,-0.003161,-0.000434,0.000015,-0.000007,0.000004,-0.000003,0.600517,0.178434,-0.020492,-0.003399,0.000031,-0.000028,0.000028,-0.000023,1.332623,0.025846,-0.003047,-0.000478,0.000009,-0.000005,0.000004,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.330079,0.017772,-0.004449,-0.000471,-0.000065,0.000029,-0.000068,0.000051,0.755068,0.127245,-0.030842,-0.003793,-0.000485,0.000220,-0.000509,0.000373,1.354948,0.018327,-0.004489,-0.000527,-0.000069,0.000031,-0.000072,0.000053,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.317281,0.007289,-0.005926,-0.000039,0.000824,-0.000001,0.001019,-0.000675,0.847278,0.052902,-0.042724,-0.000642,0.006052,-0.000022,0.007523,-0.004966,1.368203,0.007587,-0.006140,-0.000076,0.000865,-0.000003,0.001073,-0.000709,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/7.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/7.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0ac72418192d5eb31016cde75c5d199dae7ff866
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/7.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-0.314790,0.000000,0.000000,-0.000000,-0.012835,0.014124,-0.006568,0.001205,0.865400,0.000000,0.000000,-0.000000,-0.018531,0.019542,-0.008917,0.001616,1.370800,0.000000,0.000000,-0.000000,-0.004537,0.004746,-0.002158,0.000390,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.318863,-0.011690,-0.008978,0.000724,0.001448,-0.001220,0.000519,-0.000093,0.859110,-0.018604,-0.015582,-0.000475,0.001995,-0.001626,0.000676,-0.000119,1.369242,-0.004632,-0.003932,-0.000185,0.000484,-0.000392,0.000162,-0.000028,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.338152,-0.025315,-0.004479,0.001448,-0.000119,0.000086,-0.000036,0.000007,0.825373,-0.048124,-0.013661,0.000599,-0.000157,0.000106,-0.000039,0.000006,1.360718,-0.012301,-0.003667,0.000080,-0.000038,0.000025,-0.000009,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.366561,-0.030144,-0.000386,0.001352,0.000011,-0.000014,0.000019,-0.000006,0.764103,-0.073938,-0.012201,0.000469,0.000008,0.000002,-0.000011,0.000004,1.344809,-0.019466,-0.003509,0.000049,0.000001,0.000003,-0.000007,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.395729,-0.026815,0.003751,0.001419,0.000009,0.000037,-0.000112,0.000023,0.678435,-0.096933,-0.010812,0.000438,-0.000007,-0.000028,0.000083,-0.000017,1.321883,-0.026344,-0.003382,0.000027,-0.000004,-0.000016,0.000048,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.417417,-0.015347,0.007235,0.000391,-0.000681,0.000153,0.000049,-0.000023,0.571160,-0.117030,-0.008928,0.001199,0.000504,-0.000111,-0.000042,0.000019,1.292202,-0.032905,-0.002972,0.000467,0.000292,-0.000066,-0.000019,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.425640,-0.001530,0.006102,-0.000631,0.000011,0.000030,-0.000029,0.000006,0.446771,-0.129943,-0.003639,0.001939,-0.000011,-0.000033,0.000054,-0.000011,1.257007,-0.036663,-0.000576,0.000913,-0.000003,-0.000010,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.421681,0.008846,0.004274,-0.000644,-0.000053,0.000018,-0.000023,0.000004,0.315128,-0.131364,0.002362,0.002260,0.000248,-0.000065,0.000013,0.000001,1.220669,-0.035136,0.002046,0.000794,-0.000068,0.000024,-0.000016,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.409259,0.015230,0.001940,-0.001004,-0.000177,0.000038,0.000017,-0.000010,0.188584,-0.119103,0.010210,0.002918,0.000173,-0.000034,-0.000028,0.000022,1.188315,-0.028897,0.004058,0.000505,-0.000124,0.000034,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.393226,0.015611,-0.001710,-0.001343,-0.000084,0.000052,-0.000106,0.000071,0.082743,-0.089415,0.019716,0.003494,0.000363,-0.000177,0.000399,-0.000286,1.163893,-0.019578,0.005200,0.000387,0.000071,-0.000025,0.000066,-0.000051,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.380736,0.007941,-0.005838,-0.000820,0.001047,-0.000031,0.001387,-0.000880,0.016837,-0.038543,0.030585,0.001150,-0.004540,0.000042,-0.005724,0.003745,1.149962,-0.007824,0.006443,-0.000062,-0.000865,-0.000003,-0.001056,0.000705,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/8.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/8.csv
new file mode 100644
index 0000000000000000000000000000000000000000..c287d45b2741785907df88944568748e9ccdfddf
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/8.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/3/9.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/3/9.csv
new file mode 100644
index 0000000000000000000000000000000000000000..d76d2ab5df52501c57acdf5f4c69bc66b49568b8
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/3/9.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/1.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..df339cba74bed0caf61c0256294326fbb390238a
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/1.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,0.000000,0.000000,0.000000,0.000000,0.013231,-0.014184,0.006520,-0.001188,0.000000,0.000000,0.000000,0.000000,0.027638,-0.029626,0.013619,-0.002481,1.500000,0.000000,0.000000,0.000000,-0.028437,0.030481,-0.014012,0.002552,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.004380,0.012814,0.010414,-0.000074,-0.001450,0.001198,-0.000502,0.000089,0.009150,0.026771,0.021759,-0.000151,-0.003029,0.002501,-0.001049,0.000186,1.490585,-0.027547,-0.022392,0.000153,0.003117,-0.002573,0.001080,-0.000191,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.026868,0.031216,0.007799,-0.000833,0.000116,-0.000080,0.000031,-0.000005,0.056138,0.065231,0.016307,-0.001736,0.000242,-0.000168,0.000065,-0.000011,1.442230,-0.067133,-0.016789,0.001784,-0.000249,0.000173,-0.000067,0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.065112,0.044529,0.005549,-0.000737,-0.000007,0.000004,-0.000002,0.000000,0.136069,0.093080,0.011615,-0.001538,-0.000015,0.000009,-0.000003,0.000001,1.359961,-0.095813,-0.011967,0.001580,0.000016,-0.000009,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.114449,0.053401,0.003322,-0.000743,0.000001,-0.000000,-0.000000,0.000000,0.239217,0.111665,0.006964,-0.001553,0.000002,-0.000000,-0.000000,0.000000,1.253771,-0.114972,-0.007185,0.001598,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.170430,0.057821,0.001099,-0.000740,0.000000,-0.000000,0.000000,-0.000000,0.356293,0.120938,0.002312,-0.001549,0.000000,-0.000000,-0.000000,0.000000,1.133211,-0.124551,-0.002394,0.001596,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.228610,0.057800,-0.001120,-0.000740,0.000000,-0.000000,0.000000,-0.000000,0.477994,0.120915,-0.002335,-0.001549,0.000000,-0.000000,0.000000,-0.000000,1.007862,-0.124550,0.002395,0.001596,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.284550,0.053340,-0.003339,-0.000740,-0.000000,0.000001,-0.000000,0.000000,0.595025,0.111599,-0.006981,-0.001550,-0.000001,0.000001,-0.000001,0.000001,0.887303,-0.114970,0.007185,0.001598,0.000001,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.333811,0.044443,-0.005555,-0.000732,0.000007,-0.000007,0.000007,-0.000005,0.698094,0.092992,-0.011619,-0.001533,0.000015,-0.000014,0.000014,-0.000011,0.781115,-0.095811,0.011967,0.001580,-0.000016,0.000015,-0.000014,0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.371969,0.031135,-0.007788,-0.000827,-0.000116,0.000052,-0.000120,0.000089,0.777938,0.065151,-0.016296,-0.001731,-0.000242,0.000108,-0.000251,0.000186,0.698848,-0.067131,0.016789,0.001784,0.000249,-0.000112,0.000259,-0.000191,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.394394,0.012775,-0.010384,-0.000070,0.001445,-0.000002,0.001787,-0.001183,0.824863,0.026732,-0.021730,-0.000148,0.003024,-0.000005,0.003739,-0.002476,0.650495,-0.027546,0.022391,0.000153,-0.003117,0.000005,-0.003853,0.002552,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/10.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/10.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3f9918cc9b7250795c0898b13980cb35ca368bfc
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/10.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/11.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/11.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ac05a403b845c8fb349c37493f251ea2a82d585f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/11.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,-1.121200,0.000000,0.000000,0.000000,0.019920,-0.021278,0.009766,-0.001777,0.545980,0.000000,0.000000,0.000000,0.001037,-0.001103,0.000506,-0.000092,1.471700,0.000000,0.000000,0.000000,0.013680,-0.014627,0.006716,-0.001222,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.114569,0.019447,0.015913,0.000025,-0.002175,0.001791,-0.000750,0.000133,0.546327,0.001020,0.000841,0.000009,-0.000113,0.000093,-0.000039,0.000007,1.476247,0.013325,0.010883,-0.000009,-0.001495,0.001232,-0.000516,0.000091,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.080186,0.048031,0.012383,-0.001122,0.000173,-0.000120,0.000046,-0.000008,0.548145,0.002557,0.000681,-0.000050,0.000009,-0.000006,0.000002,-0.000000,1.499759,0.032789,0.008384,-0.000794,0.000120,-0.000082,0.000032,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-1.020802,0.069745,0.009381,-0.000985,-0.000013,0.000006,-0.000002,0.000000,0.551338,0.003785,0.000551,-0.000041,0.000000,0.000000,0.000000,-0.000000,1.540202,0.047395,0.006258,-0.000695,-0.000007,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.942670,0.085520,0.006383,-0.001008,-0.000003,-0.000000,-0.000000,0.000000,0.555634,0.004766,0.000431,-0.000039,0.000001,-0.000000,0.000000,-0.000000,1.593156,0.057812,0.004157,-0.000701,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.851778,0.095249,0.003338,-0.001023,-0.000004,0.000001,-0.000000,0.000000,0.560793,0.005515,0.000318,-0.000038,-0.000001,-0.000001,0.000000,-0.000000,1.654426,0.064028,0.002060,-0.000697,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.754218,0.098839,0.000243,-0.001041,-0.000005,0.000001,-0.000000,0.000000,0.566586,0.006031,0.000194,-0.000046,-0.000003,-0.000001,0.000000,-0.000000,1.719817,0.066059,-0.000028,-0.000695,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.656181,0.096186,-0.002906,-0.001059,-0.000005,0.000001,-0.000000,0.000000,0.572762,0.006269,0.000036,-0.000060,-0.000004,-0.000001,0.000000,-0.000000,1.785153,0.063919,-0.002110,-0.000693,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.563963,0.087182,-0.006107,-0.001076,-0.000005,0.000001,-0.000001,0.000000,0.579002,0.006140,-0.000175,-0.000082,-0.000007,-0.000001,-0.000000,0.000000,1.846269,0.057620,-0.004189,-0.000693,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.483968,0.071727,-0.009349,-0.001077,0.000009,-0.000010,0.000010,-0.000008,0.584877,0.005513,-0.000469,-0.000115,-0.000009,-0.000002,0.000002,-0.000001,1.899008,0.047166,-0.006261,-0.000684,0.000008,-0.000007,0.000007,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.422667,0.049783,-0.012655,-0.001235,-0.000184,0.000081,-0.000188,0.000140,0.589796,0.004188,-0.000881,-0.000169,-0.000026,0.000010,-0.000019,0.000014,1.939230,0.032588,-0.008353,-0.000781,-0.000118,0.000052,-0.000122,0.000091,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.386925,0.020286,-0.016546,-0.000041,0.002281,-0.000001,0.002813,-0.001866,0.592912,0.001843,-0.001446,-0.000076,0.000220,-0.000002,0.000281,-0.000183,1.962587,0.013229,-0.010811,-0.000001,0.001483,0.000000,0.001825,-0.001212,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/12.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/12.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0b61bcef3f890d6b61e03a3fa8bba5111db54074
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/12.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.380000,0.000000,0.000000,-0.000000,-0.009646,0.010375,-0.004776,0.000871,0.593550,0.000000,0.000000,0.000000,-0.002559,0.002751,-0.001266,0.000231,1.967100,0.000000,0.000000,0.000000,-0.024258,0.026086,-0.012009,0.002189,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.383177,-0.009273,-0.007487,0.000115,0.001061,-0.000878,0.000369,-0.000066,0.592706,-0.002464,-0.001992,0.000027,0.000281,-0.000233,0.000098,-0.000017,1.959109,-0.023328,-0.018842,0.000281,0.002668,-0.002209,0.000928,-0.000165,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.399335,-0.022292,-0.005397,0.000667,-0.000085,0.000059,-0.000023,0.000004,0.588407,-0.005938,-0.001446,0.000174,-0.000022,0.000016,-0.000006,0.000001,1.918443,-0.056123,-0.013610,0.001670,-0.000214,0.000149,-0.000058,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.426402,-0.031241,-0.003578,0.000597,0.000005,-0.000003,0.000001,-0.000000,0.581185,-0.008349,-0.000971,0.000156,0.000002,-0.000001,0.000000,-0.000000,1.850267,-0.078723,-0.009055,0.001495,0.000014,-0.000008,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.460621,-0.036596,-0.001776,0.000601,-0.000001,0.000000,-0.000000,0.000000,0.572022,-0.009819,-0.000497,0.000159,0.000000,0.000000,-0.000000,0.000000,1.763992,-0.092319,-0.004534,0.001510,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.498393,-0.038348,0.000023,0.000599,-0.000001,-0.000000,0.000000,0.000000,0.561865,-0.010335,-0.000018,0.000161,0.000001,0.000000,-0.000000,-0.000000,1.668648,-0.096858,-0.000005,0.001510,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.536120,-0.036508,0.001815,0.000596,-0.000001,-0.000000,0.000000,-0.000000,0.551673,-0.009886,0.000469,0.000163,0.000001,-0.000000,0.000000,-0.000000,1.573295,-0.092338,0.004526,0.001512,0.000001,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.570217,-0.031094,0.003595,0.000587,-0.000006,0.000005,-0.000005,0.000004,0.542420,-0.008457,0.000960,0.000163,-0.000001,0.000001,-0.000001,0.000001,1.486995,-0.078754,0.009052,0.001497,-0.000014,0.000013,-0.000013,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.597131,-0.022144,0.005380,0.000654,0.000084,-0.000038,0.000088,-0.000065,0.535086,-0.006045,0.001459,0.000183,0.000023,-0.000011,0.000024,-0.000018,1.418787,-0.056153,0.013613,0.001672,0.000214,-0.000097,0.000224,-0.000165,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.613170,-0.009197,0.007431,0.000107,-0.001051,0.000004,-0.001306,0.000862,0.530703,-0.002517,0.002031,0.000033,-0.000288,0.000001,-0.000359,0.000237,1.378097,-0.023344,0.018853,0.000282,-0.002670,0.000010,-0.003319,0.002191,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/13.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/13.csv
new file mode 100644
index 0000000000000000000000000000000000000000..e5fad3dfcd0ede4d0f4048c97af592a1c315a81f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/13.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.616320,0.000000,0.000000,0.000000,0.012454,-0.013383,0.006159,-0.001123,0.529840,0.000000,0.000000,0.000000,-0.012008,0.012915,-0.005946,0.001084,1.370100,0.000000,0.000000,-0.000000,0.026806,-0.028830,0.013273,-0.002420,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.612213,0.011996,0.009702,-0.000128,-0.001369,0.001132,-0.000476,0.000084,0.525885,-0.011545,-0.009323,0.000142,0.001321,-0.001094,0.000460,-0.000082,1.378929,0.025770,0.020808,-0.000318,-0.002949,0.002441,-0.001026,0.000182,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.591271,0.028938,0.007065,-0.000841,0.000110,-0.000076,0.000030,-0.000005,0.505764,-0.027763,-0.006725,0.000829,-0.000106,0.000074,-0.000029,0.000005,1.423838,0.061962,0.015005,-0.001852,0.000237,-0.000164,0.000064,-0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.556051,0.040744,0.004773,-0.000753,-0.000007,0.000004,-0.000001,0.000000,0.472048,-0.038920,-0.004463,0.000743,0.000007,-0.000004,0.000001,-0.000000,1.499078,0.086847,0.009951,-0.001659,-0.000015,0.000009,-0.000003,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.511291,0.048016,0.002494,-0.000763,-0.000001,-0.000000,0.000000,-0.000000,0.429412,-0.045602,-0.002216,0.000751,0.000000,0.000000,0.000000,-0.000000,1.594208,0.101741,0.004936,-0.001675,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.461546,0.050710,0.000197,-0.000770,-0.000003,-0.000000,-0.000000,-0.000000,0.382345,-0.047780,0.000039,0.000752,-0.000000,-0.000000,0.000000,-0.000000,1.699211,0.106592,-0.000083,-0.001671,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.411413,0.048780,-0.002137,-0.000791,-0.000011,-0.000002,0.000001,0.000000,0.335355,-0.045448,0.002290,0.000747,-0.000003,-0.000002,0.000001,-0.000000,1.804049,0.101415,-0.005091,-0.001666,0.000003,0.000003,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.365574,0.042082,-0.004585,-0.000837,-0.000006,-0.000006,0.000008,-0.000006,0.292940,-0.038645,0.004501,0.000723,-0.000010,0.000007,-0.000006,0.000005,1.898712,0.086260,-0.010045,-0.001625,0.000022,-0.000013,0.000013,-0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.328925,0.030352,-0.007198,-0.000968,-0.000121,0.000055,-0.000125,0.000091,0.259516,-0.027478,0.006696,0.000804,0.000104,-0.000047,0.000109,-0.000080,1.973313,0.061318,-0.014955,-0.001792,-0.000230,0.000104,-0.000243,0.000179,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.306838,0.012732,-0.010237,-0.000212,0.001467,-0.000007,0.001831,-0.001206,0.239624,-0.011399,0.009216,0.000126,-0.001302,0.000004,-0.001617,0.001068,2.017694,0.025426,-0.020561,-0.000275,0.002902,-0.000010,0.003604,-0.002380,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/14.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/14.csv
new file mode 100644
index 0000000000000000000000000000000000000000..158f68cf1b5893c37ddf38d9ab4effef23040ef1
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/14.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.302470,0.000000,0.000000,0.000000,-0.001642,0.001752,-0.000804,0.000146,0.235720,0.000000,0.000000,0.000000,0.012996,-0.013983,0.006439,-0.001174,2.026400,0.000000,0.000000,0.000000,-0.010051,0.010798,-0.004969,0.000906,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.303018,-0.001607,-0.001318,-0.000005,0.000179,-0.000147,0.000062,-0.000011,0.239997,0.012480,0.010068,-0.000166,-0.001430,0.001184,-0.000498,0.000088,2.023084,-0.009687,-0.007839,0.000098,0.001104,-0.000913,0.000384,-0.000068,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.305865,-0.003986,-0.001038,0.000088,-0.000015,0.000010,-0.000004,0.000001,0.261724,0.029951,0.007219,-0.000909,0.000115,-0.000080,0.000031,-0.000005,2.006162,-0.023396,-0.005727,0.000675,-0.000088,0.000061,-0.000024,0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.310809,-0.005825,-0.000806,0.000075,0.000001,-0.000000,0.000000,0.000000,0.298046,0.041871,0.004736,-0.000815,-0.000007,0.000004,-0.000002,0.000000,1.977668,-0.032985,-0.003888,0.000604,0.000006,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.317365,-0.007209,-0.000576,0.000079,0.000002,0.000001,-0.000001,0.000000,0.343835,0.048887,0.002278,-0.000818,0.000003,0.000001,-0.000000,0.000000,1.941402,-0.038936,-0.002059,0.000612,0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.325068,-0.008112,-0.000318,0.000096,0.000007,0.000003,-0.000000,-0.000000,0.394185,0.051004,-0.000153,-0.000802,0.000004,0.000000,0.000000,-0.000000,1.901021,-0.041209,-0.000208,0.000623,0.000005,0.000002,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.333393,-0.008422,0.000033,0.000145,0.000015,0.000000,0.000001,-0.000001,0.444238,0.048309,-0.002530,-0.000781,0.000006,0.000001,0.000000,-0.000000,1.860233,-0.039730,0.001704,0.000655,0.000010,-0.000000,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.341621,-0.007857,0.000565,0.000210,0.000013,-0.000003,0.000000,0.000001,0.489243,0.040934,-0.004829,-0.000748,0.000012,-0.000009,0.000007,-0.000005,1.822872,-0.034317,0.003728,0.000691,0.000003,0.000003,-0.000004,0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.348692,-0.006055,0.001261,0.000264,0.000030,-0.000015,0.000029,-0.000020,0.524605,0.029038,-0.007101,-0.000838,-0.000112,0.000050,-0.000115,0.000085,1.792980,-0.024758,0.005870,0.000790,0.000099,-0.000045,0.000102,-0.000074,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.353199,-0.002664,0.002093,0.000106,-0.000319,0.000004,-0.000405,0.000264,0.545612,0.012029,-0.009732,-0.000124,0.001372,-0.000004,0.001703,-0.001125,1.774963,-0.010386,0.008351,0.000173,-0.001197,0.000006,-0.001493,0.000984,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/15.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/15.csv
new file mode 100644
index 0000000000000000000000000000000000000000..9b38eb5a6f24c3fece19eea83ab002ae3322f3e0
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/15.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.354120,0.000000,0.000000,0.000000,0.005222,-0.005618,0.002587,-0.000472,0.549730,0.000000,0.000000,0.000000,-0.024915,0.026793,-0.012334,0.002249,1.771400,0.000000,0.000000,-0.000000,0.005635,-0.006062,0.002791,-0.000509,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.352401,0.005018,0.004050,-0.000064,-0.000575,0.000476,-0.000200,0.000035,0.541522,-0.023960,-0.019351,0.000289,0.002740,-0.002268,0.000953,-0.000169,1.773255,0.005414,0.004370,-0.000069,-0.000620,0.000513,-0.000216,0.000038,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.343661,0.012053,0.002912,-0.000363,0.000046,-0.000032,0.000012,-0.000002,0.499756,-0.057639,-0.013976,0.001715,-0.000220,0.000153,-0.000059,0.000010,1.782686,0.013006,0.003143,-0.000391,0.000050,-0.000035,0.000013,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.329034,0.016874,0.001923,-0.000324,-0.000002,0.000002,-0.000001,0.000000,0.429739,-0.080848,-0.009298,0.001536,0.000014,-0.000008,0.000003,-0.000000,1.798470,0.018210,0.002076,-0.000349,-0.000002,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.310561,0.019747,0.000950,-0.000323,0.000001,-0.000000,0.000000,-0.000000,0.341137,-0.094808,-0.004655,0.001551,-0.000001,0.000000,-0.000000,0.000000,1.818406,0.021310,0.001025,-0.000349,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.290187,0.020679,-0.000017,-0.000321,0.000000,-0.000000,-0.000000,0.000000,0.243224,-0.099467,-0.000004,0.001550,0.000000,0.000000,-0.000000,0.000000,1.840393,0.022316,-0.000019,-0.000347,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.269846,0.019683,-0.000979,-0.000320,0.000000,0.000000,-0.000000,0.000000,0.145304,-0.094823,0.004649,0.001552,0.000001,-0.000001,0.000001,-0.000001,1.862344,0.021239,-0.001058,-0.000346,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.251462,0.016765,-0.001937,-0.000317,0.000003,-0.000003,0.000003,-0.000002,0.056681,-0.080873,0.009295,0.001537,-0.000014,0.000013,-0.000013,0.000010,1.882180,0.018089,-0.002091,-0.000341,0.000003,-0.000003,0.000003,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.236949,0.011941,-0.002901,-0.000353,-0.000045,0.000021,-0.000048,0.000035,-0.013363,-0.057665,0.013979,0.001717,0.000220,-0.000100,0.000231,-0.000169,1.897837,0.012882,-0.003130,-0.000381,-0.000049,0.000022,-0.000051,0.000038,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.228299,0.004960,-0.004008,-0.000058,0.000567,-0.000002,0.000705,-0.000465,-0.055150,-0.023973,0.019361,0.000290,-0.002742,0.000010,-0.003409,0.002250,1.907168,0.005350,-0.004323,-0.000062,0.000612,-0.000002,0.000760,-0.000502,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/16.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/16.csv
new file mode 100644
index 0000000000000000000000000000000000000000..cab626891810e31e9417e98104f149fec8f1546c
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/16.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.226600,0.000000,0.000000,0.000000,0.002912,-0.003111,0.001428,-0.000260,-0.063362,0.000000,0.000000,0.000000,-0.002732,0.002941,-0.001354,0.000247,1.909000,0.000000,0.000000,-0.000000,0.003290,-0.003557,0.001641,-0.000300,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.225631,0.002842,0.002324,0.000003,-0.000318,0.000262,-0.000110,0.000019,-0.064261,-0.002622,-0.002114,0.000036,0.000301,-0.000249,0.000105,-0.000019,1.910075,0.003126,0.002498,-0.000071,-0.000364,0.000303,-0.000128,0.000023,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.220609,0.007013,0.001805,-0.000165,0.000025,-0.000017,0.000007,-0.000001,-0.068823,-0.006285,-0.001510,0.000193,-0.000024,0.000017,-0.000007,0.000001,1.915462,0.007359,0.001689,-0.000259,0.000030,-0.000020,0.000008,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.211942,0.010175,0.001362,-0.000149,-0.000006,-0.000002,0.000000,0.000000,-0.076437,-0.008766,-0.000977,0.000177,0.000002,-0.000001,0.000000,-0.000000,1.924267,0.010016,0.000981,-0.000228,0.000003,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.200562,0.012420,0.000865,-0.000185,-0.000010,0.000000,0.000000,-0.000000,-0.086001,-0.010183,-0.000440,0.000179,-0.000001,-0.000000,0.000000,-0.000000,1.935041,0.011317,0.000336,-0.000197,0.000009,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.187472,0.013559,0.000257,-0.000219,-0.000008,0.000000,-0.000000,0.000000,-0.096446,-0.010530,0.000090,0.000172,-0.000004,-0.000001,0.000000,0.000000,1.946506,0.011433,-0.000204,-0.000165,0.000006,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.173883,0.013383,-0.000448,-0.000249,-0.000005,0.000002,-0.000001,0.000000,-0.106719,-0.009852,0.000578,0.000153,-0.000004,0.000001,-0.000000,-0.000000,1.957576,0.010550,-0.000671,-0.000148,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.161200,0.011728,-0.001212,-0.000256,0.000001,-0.000002,0.000002,-0.000002,-0.115844,-0.008252,0.001017,0.000142,-0.000003,0.000002,-0.000001,0.000001,1.967308,0.008770,-0.001106,-0.000143,0.000002,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.150941,0.008532,-0.001994,-0.000285,-0.000034,0.000015,-0.000035,0.000026,-0.122938,-0.005796,0.001444,0.000157,0.000021,-0.000010,0.000022,-0.000017,1.974830,0.006131,-0.001540,-0.000160,-0.000023,0.000010,-0.000024,0.000017,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.144716,0.003599,-0.002886,-0.000070,0.000417,-0.000002,0.000521,-0.000343,-0.127116,-0.002382,0.001935,0.000015,-0.000270,0.000001,-0.000334,0.000221,1.979242,0.002511,-0.002043,-0.000011,0.000284,-0.000000,0.000350,-0.000232,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/17.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/17.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3255d01988459b109900fad0976a40f8c8134920
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/17.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/18.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/18.csv
new file mode 100644
index 0000000000000000000000000000000000000000..00962fa45b19b5c5bdbe186bec52eb35720a2ac7
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/18.csv
@@ -0,0 +1,8 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/19.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/19.csv
new file mode 100644
index 0000000000000000000000000000000000000000..5371c9fed5312de0859302e628d823293ee2ca60
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/19.csv
@@ -0,0 +1,20 @@
+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,
+1.000000,-0.352530,0.000000,0.000000,0.000000,0.004957,-0.005269,0.002413,-0.000438,0.043803,0.000000,0.000000,-0.000000,0.001340,-0.001495,0.000699,-0.000129,2.072600,0.000000,0.000000,0.000000,-0.021258,0.022472,-0.010266,0.001862,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.350868,0.004890,0.004036,0.000050,-0.000539,0.000442,-0.000184,0.000033,0.044219,0.001180,0.000875,-0.000111,-0.000153,0.000131,-0.000056,0.000010,2.065411,-0.021227,-0.017702,-0.000445,0.002295,-0.001874,0.000780,-0.000137,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.342140,0.012289,0.003290,-0.000236,0.000043,-0.000029,0.000011,-0.000002,0.046094,0.002373,0.000303,-0.000182,0.000014,-0.000009,0.000004,-0.000001,2.027100,-0.054439,-0.015194,0.000787,-0.000181,0.000124,-0.000047,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.326775,0.018239,0.002674,-0.000200,-0.000002,0.000002,-0.000001,0.000000,0.048597,0.002463,-0.000206,-0.000163,0.000003,0.000001,-0.000000,0.000000,1.958157,-0.082799,-0.013221,0.000639,0.000012,-0.000006,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.306061,0.022987,0.002073,-0.000200,0.000001,-0.000000,0.000000,0.000000,0.050694,0.001578,-0.000669,-0.000143,0.000007,0.000001,-0.000000,0.000000,1.862784,-0.107295,-0.011268,0.000656,0.000001,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.281201,0.026536,0.001478,-0.000195,0.000002,0.000000,0.000000,-0.000000,0.051468,-0.000157,-0.001047,-0.000105,0.000014,0.000002,0.000001,-0.000000,1.744878,-0.127858,-0.009293,0.000661,0.000002,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.253379,0.028919,0.000915,-0.000176,0.000008,0.000001,-0.000001,0.000000,0.050177,-0.002492,-0.001243,-0.000013,0.000031,-0.000000,-0.000000,-0.000000,1.608390,-0.144452,-0.007296,0.000670,0.000002,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.223714,0.030250,0.000430,-0.000150,0.000004,-0.000001,-0.000000,0.000000,0.046459,-0.004896,-0.001105,0.000099,0.000021,-0.000007,0.000002,-0.000000,1.457314,-0.157026,-0.005273,0.000679,0.000002,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.193182,0.030670,-0.000006,-0.000143,0.000001,0.000000,0.000000,-0.000000,0.040573,-0.006749,-0.000727,0.000143,0.000004,-0.000003,0.000001,-0.000000,1.295696,-0.165529,-0.003227,0.000684,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.162661,0.030231,-0.000430,-0.000139,0.000001,-0.000000,-0.000000,0.000000,0.033242,-0.007767,-0.000291,0.000144,-0.000001,0.000000,0.000000,-0.000000,1.127625,-0.169927,-0.001171,0.000686,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.132997,0.028960,-0.000839,-0.000134,0.000001,-0.000000,-0.000000,0.000000,0.025327,-0.007920,0.000136,0.000140,-0.000002,-0.000001,-0.000000,-0.000000,0.957214,-0.170208,0.000890,0.000688,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.105009,0.026884,-0.001235,-0.000129,0.000003,0.000001,0.000000,0.000000,0.017680,-0.007242,0.000532,0.000119,-0.000010,-0.000002,-0.000000,0.000000,0.788584,-0.166362,0.002956,0.000690,0.000001,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.079485,0.024045,-0.001591,-0.000103,0.000012,0.000003,-0.000001,0.000000,0.011077,-0.005873,0.000806,0.000056,-0.000021,-0.000002,0.000001,-0.000000,0.625869,-0.158377,0.005031,0.000693,0.000001,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.057121,0.020608,-0.001818,-0.000048,0.000013,-0.000000,-0.000000,0.000000,0.006045,-0.004175,0.000852,-0.000021,-0.000014,0.000003,-0.000000,0.000000,0.473218,-0.146230,0.007116,0.000696,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.038367,0.016876,-0.001892,-0.000006,0.000007,-0.000002,0.000000,-0.000000,0.002690,-0.002578,0.000727,-0.000057,-0.000004,0.000002,-0.000000,0.000000,0.334801,-0.129907,0.009207,0.000697,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.023383,0.013096,-0.001881,0.000010,0.000002,-0.000001,0.000000,-0.000000,0.000779,-0.001305,0.000541,-0.000065,-0.000001,0.000000,-0.000000,0.000000,0.214798,-0.109400,0.011301,0.000699,0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.012157,0.009370,-0.001843,0.000015,0.000001,-0.000001,0.000001,-0.000001,-0.000051,-0.000420,0.000343,-0.000066,0.000000,-0.000000,0.000000,-0.000000,0.117398,-0.084705,0.013388,0.000686,-0.000012,0.000010,-0.000010,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.004615,0.005729,-0.001802,0.000002,-0.000016,0.000006,-0.000015,0.000012,-0.000194,0.000067,0.000143,-0.000068,-0.000002,0.000001,-0.000003,0.000002,0.046764,-0.055866,0.015509,0.000841,0.000187,-0.000080,0.000187,-0.000142,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.000699,0.002091,-0.001804,0.000120,0.000212,0.000004,0.000246,-0.000169,-0.000055,0.000140,-0.000068,-0.000058,0.000027,-0.000002,0.000040,-0.000024,0.007401,-0.021843,0.018190,-0.000426,-0.002367,-0.000016,-0.002861,0.001922,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/2.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..95a5a292b2b5d9be0b4956fe9f6d247b8a15c7aa
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/2.csv
@@ -0,0 +1,17 @@
+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,
+1.000000,0.398760,0.000000,0.000000,0.000000,0.006945,-0.007371,0.003373,-0.000613,0.834000,0.000000,0.000000,0.000000,-0.004994,0.005329,-0.002445,0.000445,0.641080,0.000000,0.000000,-0.000000,0.001998,-0.002121,0.000971,-0.000176,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.401095,0.006878,0.005696,0.000095,-0.000753,0.000617,-0.000257,0.000045,0.832335,-0.004886,-0.004006,-0.000018,0.000541,-0.000451,0.000188,-0.000033,0.641752,0.001978,0.001638,0.000027,-0.000217,0.000177,-0.000074,0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.413416,0.017401,0.004725,-0.000306,0.000060,-0.000041,0.000016,-0.000003,0.823671,-0.012146,-0.003196,0.000242,-0.000050,0.000031,-0.000012,0.000002,0.645295,0.005004,0.001358,-0.000088,0.000017,-0.000012,0.000005,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.435267,0.026041,0.003934,-0.000257,-0.000004,0.000002,-0.000001,0.000000,0.808543,-0.017912,-0.002593,0.000187,-0.000001,-0.000001,0.000001,-0.000000,0.651578,0.007487,0.001130,-0.000074,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.464983,0.033130,0.003153,-0.000261,0.000000,-0.000000,0.000000,-0.000000,0.788223,-0.022544,-0.002044,0.000179,-0.000002,0.000000,-0.000000,0.000000,0.660120,0.009522,0.000905,-0.000075,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.501004,0.038651,0.002369,-0.000261,-0.000000,-0.000000,0.000000,0.000000,0.763813,-0.026101,-0.001515,0.000174,-0.000001,0.000000,-0.000000,-0.000000,0.670471,0.011105,0.000679,-0.000075,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.541762,0.042604,0.001584,-0.000262,-0.000000,0.000000,-0.000000,0.000000,0.736371,-0.028610,-0.000995,0.000173,-0.000000,0.000000,-0.000000,0.000000,0.682180,0.012237,0.000453,-0.000075,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.585688,0.044986,0.000798,-0.000262,-0.000000,0.000000,-0.000000,0.000000,0.706939,-0.030081,-0.000477,0.000172,-0.000000,0.000000,-0.000000,0.000000,0.694794,0.012917,0.000227,-0.000075,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.631211,0.045796,0.000011,-0.000262,-0.000000,0.000000,-0.000000,0.000000,0.676553,-0.030519,0.000039,0.000172,-0.000000,0.000000,-0.000000,0.000000,0.707862,0.013145,0.000001,-0.000075,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.676756,0.045031,-0.000777,-0.000263,-0.000000,-0.000000,-0.000000,0.000000,0.646245,-0.029924,0.000555,0.000172,-0.000000,-0.000000,0.000000,-0.000000,0.720933,0.012921,-0.000225,-0.000075,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.720747,0.042689,-0.001566,-0.000263,-0.000000,0.000000,-0.000000,0.000000,0.617047,-0.028300,0.001070,0.000171,-0.000000,0.000000,0.000000,-0.000000,0.733554,0.012245,-0.000451,-0.000075,-0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.761607,0.038766,-0.002357,-0.000264,-0.000000,-0.000000,-0.000000,-0.000000,0.589989,-0.025646,0.001584,0.000171,-0.000000,0.000000,-0.000000,0.000000,0.745272,0.011116,-0.000678,-0.000076,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.797752,0.033260,-0.003150,-0.000265,-0.000000,0.000000,-0.000000,0.000000,0.566098,-0.021964,0.002098,0.000172,0.000000,-0.000000,0.000000,-0.000000,0.755636,0.009535,-0.000904,-0.000076,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.827598,0.026168,-0.003941,-0.000261,0.000004,-0.000003,0.000003,-0.000003,0.546405,-0.017252,0.002611,0.000169,-0.000002,0.000002,-0.000002,0.000002,0.764191,0.007500,-0.001131,-0.000075,0.000001,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.849564,0.017500,-0.004745,-0.000311,-0.000060,0.000026,-0.000061,0.000046,0.531932,-0.011522,0.003131,0.000202,0.000040,-0.000017,0.000040,-0.000030,0.770485,0.005014,-0.001360,-0.000089,-0.000017,0.000007,-0.000017,0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.861960,0.006922,-0.005731,0.000093,0.000758,0.000003,0.000922,-0.000617,0.523775,-0.004552,0.003771,-0.000064,-0.000498,-0.000002,-0.000605,0.000405,0.774037,0.001983,-0.001642,0.000027,0.000217,0.000001,0.000264,-0.000177,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/3.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..d02921ced896cb964d8d8a18d8ebbc82fed64e3e
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/3.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/4.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..55cda2b5236bbf18723a2fcb114d31df8c387a7f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/4.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,1.071500,0.000000,0.000000,0.000000,-0.036266,0.038998,-0.017952,0.003273,0.996220,0.000000,0.000000,0.000000,-0.028208,0.030333,-0.013964,0.002546,1.097700,0.000000,0.000000,-0.000000,0.003218,-0.003460,0.001593,-0.000290,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.059552,-0.034880,-0.028175,0.000417,0.003989,-0.003302,0.001388,-0.000246,0.986927,-0.027131,-0.021915,0.000324,0.003102,-0.002568,0.001079,-0.000191,1.098760,0.003095,0.002500,-0.000037,-0.000354,0.000293,-0.000123,0.000022,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.998742,-0.083929,-0.020362,0.002493,-0.000320,0.000222,-0.000086,0.000015,0.939627,-0.065283,-0.015839,0.001939,-0.000249,0.000173,-0.000067,0.000012,1.104156,0.007448,0.001808,-0.000221,0.000028,-0.000020,0.000008,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.896775,-0.117757,-0.013562,0.002233,0.000021,-0.000012,0.000004,-0.000001,0.860313,-0.091598,-0.010550,0.001736,0.000016,-0.000009,0.000003,-0.000001,1.113206,0.010452,0.001205,-0.000198,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.767701,-0.138138,-0.006809,0.002257,-0.000000,0.000000,0.000000,-0.000000,0.759911,-0.107454,-0.005298,0.001755,-0.000000,0.000000,0.000000,-0.000000,1.124664,0.012264,0.000606,-0.000200,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.625011,-0.144985,-0.000037,0.002259,0.000001,-0.000000,0.000000,-0.000000,0.648913,-0.112785,-0.000031,0.001757,0.000001,-0.000000,0.000000,-0.000000,1.137335,0.012877,0.000006,-0.000200,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.482249,-0.138276,0.006749,0.002265,0.000002,-0.000002,0.000001,-0.000001,0.537855,-0.107572,0.005247,0.001762,0.000002,-0.000002,0.000001,-0.000001,1.150018,0.012288,-0.000596,-0.000201,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.352988,-0.117984,0.013536,0.002247,-0.000019,0.000019,-0.000019,0.000015,0.437293,-0.091791,0.010528,0.001749,-0.000015,0.000015,-0.000015,0.000012,1.161509,0.010492,-0.001200,-0.000200,0.000001,-0.000002,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.250783,-0.084159,0.020387,0.002512,0.000322,-0.000146,0.000337,-0.000247,0.357776,-0.065479,0.015860,0.001955,0.000250,-0.000113,0.000262,-0.000192,1.170601,0.007491,-0.001812,-0.000225,-0.000029,0.000013,-0.000030,0.000022,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.189790,-0.034998,0.028261,0.000429,-0.004004,0.000015,-0.004979,0.003286,0.310319,-0.027231,0.021989,0.000335,-0.003116,0.000012,-0.003874,0.002557,1.176032,0.003117,-0.002516,-0.000039,0.000357,-0.000001,0.000444,-0.000293,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/5.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/5.csv
new file mode 100644
index 0000000000000000000000000000000000000000..2e134634a32b589e8227d137d4449e61bc4b1bc1
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/5.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/6.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/6.csv
new file mode 100644
index 0000000000000000000000000000000000000000..a2684b11961ec21bcb845c8bcc37e68660151ee4
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/6.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.418580,0.000000,0.000000,0.000000,0.014218,-0.015292,0.007040,-0.001283,0.150340,0.000000,0.000000,-0.000000,0.044918,-0.048306,0.022239,-0.004054,0.743980,0.000000,0.000000,-0.000000,-0.000271,0.000292,-0.000135,0.000025,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.413897,0.013670,0.011039,-0.000168,-0.001564,0.001295,-0.000544,0.000097,0.165136,0.043190,0.034878,-0.000526,-0.004941,0.004090,-0.001719,0.000305,0.743891,-0.000259,-0.000208,0.000004,0.000030,-0.000025,0.000010,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.390073,0.032873,0.007963,-0.000981,0.000126,-0.000087,0.000034,-0.000006,0.240412,0.103874,0.025172,-0.003097,0.000397,-0.000275,0.000107,-0.000019,0.743441,-0.000618,-0.000146,0.000020,-0.000002,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.350151,0.046084,0.005286,-0.000879,-0.000008,0.000005,-0.000002,0.000000,0.366570,0.145649,0.016723,-0.002773,-0.000025,0.000015,-0.000005,0.000001,0.742696,-0.000855,-0.000092,0.000018,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.299664,0.054004,0.002631,-0.000886,0.000001,-0.000000,-0.000000,0.000000,0.526155,0.170726,0.008343,-0.002798,0.000003,-0.000000,-0.000000,0.000000,0.741767,-0.000986,-0.000040,0.000017,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.243915,0.056611,-0.000022,-0.000883,0.000001,-0.000000,-0.000000,0.000000,0.702429,0.179028,-0.000038,-0.002791,0.000001,-0.000000,-0.000000,0.000000,0.740758,-0.001015,0.000010,0.000016,-0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.188207,0.053922,-0.002666,-0.000880,0.000000,0.000001,-0.000000,0.000000,0.878630,0.170585,-0.008404,-0.002788,-0.000000,0.000002,-0.000001,0.000001,0.739768,-0.000950,0.000055,0.000015,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.137830,0.045953,-0.005299,-0.000870,0.000008,-0.000007,0.000007,-0.000006,1.038024,0.145422,-0.016746,-0.002758,0.000026,-0.000024,0.000023,-0.000019,0.738888,-0.000797,0.000098,0.000014,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.098043,0.032744,-0.007948,-0.000971,-0.000125,0.000056,-0.000131,0.000096,1.163948,0.103648,-0.025146,-0.003079,-0.000395,0.000179,-0.000414,0.000304,0.738203,-0.000560,0.000139,0.000015,0.000002,-0.000001,0.000002,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.074321,0.013606,-0.010991,-0.000161,0.001556,-0.000006,0.001933,-0.001276,1.239045,0.043076,-0.034794,-0.000515,0.004926,-0.000018,0.006123,-0.004042,0.737799,-0.000230,0.000187,0.000002,-0.000026,0.000000,-0.000032,0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/7.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/7.csv
new file mode 100644
index 0000000000000000000000000000000000000000..fa78f42fe107426b5a0648b203a10d4c25db2ec4
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/7.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-0.069661,0.000000,0.000000,0.000000,0.009933,-0.010657,0.004901,-0.000893,1.253800,0.000000,0.000000,-0.000000,-0.021838,0.023399,-0.010754,0.001959,0.737720,0.000000,0.000000,0.000000,0.028988,-0.031072,0.014283,-0.002602,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.066377,0.009603,0.007792,-0.000071,-0.001090,0.000900,-0.000378,0.000067,1.246565,-0.021174,-0.017224,0.000100,0.002392,-0.001975,0.000828,-0.000147,0.747318,0.028082,0.022827,-0.000155,-0.003177,0.002623,-0.001100,0.000195,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.049553,0.023320,0.005784,-0.000639,0.000088,-0.000060,0.000023,-0.000004,1.209366,-0.051684,-0.012974,0.001354,-0.000191,0.000133,-0.000051,0.000009,0.796613,0.068441,0.017118,-0.001818,0.000254,-0.000176,0.000068,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.021041,0.033133,0.004058,-0.000563,-0.000004,0.000003,-0.000001,0.000000,1.145961,-0.073916,-0.009314,0.001202,0.000014,-0.000007,0.000003,-0.000000,0.880489,0.097687,0.012203,-0.001611,-0.000017,0.000009,-0.000003,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.015585,0.039555,0.002366,-0.000561,0.000003,-0.000000,0.000000,-0.000000,1.063943,-0.088904,-0.005665,0.001223,0.000001,0.000000,0.000000,-0.000000,0.988758,0.117225,0.007326,-0.001630,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.056949,0.042617,0.000702,-0.000549,0.000003,-0.000000,0.000000,-0.000000,0.970598,-0.096559,-0.001985,0.001231,0.000003,-0.000000,0.000000,-0.000000,1.111680,0.126988,0.002438,-0.001629,0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.099722,0.042386,-0.000927,-0.000537,0.000003,-0.000000,0.000000,-0.000000,0.873287,-0.096825,0.001725,0.001243,0.000003,-0.000000,0.000000,-0.000000,1.239477,0.126977,-0.002449,-0.001629,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.140646,0.038928,-0.002527,-0.000530,0.000001,0.000000,-0.000000,0.000000,0.779433,-0.089635,0.005471,0.001255,0.000004,-0.000001,0.000001,-0.000000,1.362376,0.117194,-0.007334,-0.001628,-0.000000,0.000001,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.176519,0.032293,-0.004103,-0.000518,0.000007,-0.000005,0.000005,-0.000004,0.696527,-0.074919,0.009246,0.001254,-0.000009,0.000011,-0.000011,0.000009,1.470608,0.097646,-0.012206,-0.001609,0.000017,-0.000015,0.000014,-0.000012,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.204193,0.022536,-0.005677,-0.000582,-0.000083,0.000037,-0.000086,0.000064,0.632109,-0.052646,0.013092,0.001427,0.000199,-0.000089,0.000205,-0.000151,1.554444,0.068403,-0.017113,-0.001816,-0.000254,0.000114,-0.000264,0.000195,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.220401,0.009218,-0.007505,-0.000036,0.001040,-0.000001,0.001284,-0.000851,0.594146,-0.021657,0.017581,0.000149,-0.002455,0.000005,-0.003040,0.002012,1.603709,0.028063,-0.022813,-0.000153,0.003175,-0.000005,0.003924,-0.002599,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/8.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/8.csv
new file mode 100644
index 0000000000000000000000000000000000000000..d4bf253daeabc881f3882e167fc4ab1837f14dad
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/8.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/4/9.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/4/9.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0bed7ff7ac7ba1306e9455a1db1c912f5b93f5ea
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/4/9.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/1.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..37d95924bce117233795ab1417e89c6e6ac9c818
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/1.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,0.500000,0.000000,0.000000,0.000000,0.029116,-0.031208,0.014346,-0.002613,0.000000,0.000000,0.000000,0.000000,0.026325,-0.028218,0.012971,-0.002363,1.500000,0.000000,0.000000,0.000000,-0.022297,0.023900,-0.010986,0.002001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.509640,0.028205,0.022927,-0.000156,-0.003191,0.002635,-0.001105,0.000196,0.008716,0.025501,0.020728,-0.000142,-0.002885,0.002382,-0.000999,0.000177,1.492618,-0.021600,-0.017557,0.000120,0.002444,-0.002018,0.000846,-0.000150,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.559150,0.068739,0.017192,-0.001826,0.000255,-0.000177,0.000068,-0.000012,0.053478,0.062145,0.015540,-0.001652,0.000231,-0.000160,0.000062,-0.000011,1.454704,-0.052638,-0.013164,0.001399,-0.000196,0.000135,-0.000052,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.643390,0.098109,0.012256,-0.001618,-0.000016,0.000009,-0.000003,0.000001,0.129634,0.088691,0.011075,-0.001463,-0.000015,0.000008,-0.000003,0.000001,1.390197,-0.075126,-0.009383,0.001239,0.000012,-0.000007,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.752127,0.117733,0.007361,-0.001636,0.000001,-0.000000,0.000000,-0.000000,0.227929,0.106421,0.006648,-0.001479,0.000001,-0.000000,0.000000,-0.000000,1.306934,-0.090148,-0.005634,0.001253,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.875586,0.127549,0.002455,-0.001635,-0.000000,0.000000,-0.000000,-0.000000,0.339519,0.115281,0.002213,-0.001478,0.000000,-0.000000,-0.000000,0.000000,1.212405,-0.097659,-0.001877,0.001252,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.003955,0.127554,-0.002450,-0.001635,0.000000,-0.000000,0.000000,-0.000000,0.455536,0.115276,-0.002219,-0.001477,0.000000,-0.000000,0.000000,-0.000000,1.114120,-0.097659,0.001878,0.001252,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.127423,0.117747,-0.007357,-0.001636,-0.000001,0.000001,-0.000001,0.000001,0.567115,0.106406,-0.006652,-0.001478,-0.000001,0.000001,-0.000001,0.000001,1.019591,-0.090147,0.005634,0.001253,0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.236177,0.098127,-0.012255,-0.001619,0.000016,-0.000015,0.000015,-0.000012,0.665392,0.088671,-0.011076,-0.001462,0.000015,-0.000014,0.000013,-0.000011,0.936330,-0.075124,0.009383,0.001239,-0.000012,0.000011,-0.000011,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.320434,0.068755,-0.017194,-0.001828,-0.000255,0.000114,-0.000265,0.000196,0.741528,0.062127,-0.015538,-0.001651,-0.000231,0.000103,-0.000240,0.000177,0.871825,-0.052637,0.013164,0.001399,0.000196,-0.000087,0.000203,-0.000150,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.369957,0.028213,-0.022933,-0.000157,0.003192,-0.000005,0.003946,-0.002614,0.786277,0.025493,-0.020722,-0.000141,0.002884,-0.000005,0.003566,-0.002362,0.833912,-0.021599,0.017556,0.000120,-0.002444,0.000004,-0.003021,0.002001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/10.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/10.csv
new file mode 100644
index 0000000000000000000000000000000000000000..a24af28d406ee5788e995aabff350f8d0fe1dfad
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/10.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/11.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/11.csv
new file mode 100644
index 0000000000000000000000000000000000000000..1b5a671da94fed968386a967ab2d53788899ac4d
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/11.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,-0.689570,0.000000,0.000000,0.000000,0.032046,-0.034285,0.015747,-0.002867,0.041886,0.000000,0.000000,-0.000000,-0.005083,0.005372,-0.002454,0.000445,1.845300,0.000000,0.000000,-0.000000,0.001277,-0.001413,0.000659,-0.000121,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.678929,0.031174,0.025431,-0.000058,-0.003505,0.002890,-0.001211,0.000214,0.040166,-0.005079,-0.004238,-0.000109,0.000548,-0.000448,0.000186,-0.000033,1.845702,0.001148,0.000871,-0.000085,-0.000145,0.000123,-0.000052,0.000009,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.623994,0.076527,0.019463,-0.001898,0.000280,-0.000193,0.000075,-0.000013,0.030994,-0.013040,-0.003647,0.000185,-0.000043,0.000030,-0.000011,0.000002,1.847570,0.002418,0.000383,-0.000157,0.000012,-0.000008,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.529753,0.110272,0.014368,-0.001666,-0.000017,0.000010,-0.000004,0.000001,0.014468,-0.019858,-0.003184,0.000151,0.000003,-0.000001,0.000000,-0.000000,1.850220,0.002735,-0.000062,-0.000147,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.406789,0.133976,0.009330,-0.001680,0.000004,0.000000,-0.000000,0.000000,-0.008421,-0.025765,-0.002719,0.000157,0.000001,0.000000,-0.000001,0.000000,1.852745,0.002170,-0.000502,-0.000146,0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.265158,0.147611,0.004311,-0.001666,0.000004,-0.000000,-0.000000,0.000000,-0.036746,-0.030726,-0.002239,0.000164,0.000005,0.000002,0.000002,0.000000,1.854268,0.000732,-0.000935,-0.000142,0.000003,0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.114899,0.151249,-0.000663,-0.001647,0.000010,0.000004,-0.000001,0.000000,-0.069538,-0.034671,-0.001670,0.000239,0.000041,0.000010,-0.000003,0.000000,1.853928,-0.001539,-0.001314,-0.000094,0.000028,0.000008,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.034054,0.145039,-0.005514,-0.001580,0.000022,0.000002,-0.000001,0.000000,-0.105592,-0.037096,-0.000641,0.000462,0.000065,0.000001,-0.000002,-0.000000,1.851016,-0.004303,-0.001365,0.000075,0.000052,0.000002,-0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.172022,0.129367,-0.010108,-0.001480,0.000026,-0.000002,-0.000003,0.000001,-0.142804,-0.036742,0.001109,0.000686,0.000037,-0.000015,0.000003,-0.000000,1.845475,-0.006600,-0.000832,0.000270,0.000036,-0.000012,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.289823,0.104798,-0.014423,-0.001401,0.000023,-0.000016,0.000014,-0.000012,-0.177727,-0.032379,0.003273,0.000729,-0.000005,0.000005,-0.000005,0.000004,1.838340,-0.007360,0.000097,0.000326,0.000001,0.000001,-0.000002,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.378807,0.071762,-0.018685,-0.001599,-0.000256,0.000113,-0.000263,0.000196,-0.206105,-0.023643,0.005491,0.000805,0.000093,-0.000043,0.000099,-0.000072,1.831404,-0.006180,0.001093,0.000353,0.000029,-0.000014,0.000032,-0.000023,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.430074,0.028927,-0.023722,0.000103,0.003223,0.000004,0.003953,-0.002631,-0.223375,-0.009998,0.008008,0.000206,-0.001160,0.000007,-0.001452,0.000954,1.826695,-0.002854,0.002192,0.000177,-0.000354,0.000006,-0.000456,0.000294,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/12.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/12.csv
new file mode 100644
index 0000000000000000000000000000000000000000..66d4c7175c73e8b2225ab99de740e5eeb198a034
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/12.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.439930,0.000000,0.000000,0.000000,-0.016394,0.017598,-0.008095,0.001475,-0.226810,0.000000,0.000000,0.000000,0.024693,-0.026591,0.012249,-0.002234,1.825700,0.000000,0.000000,-0.000000,-0.006790,0.007270,-0.003341,0.000608,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.434514,-0.015830,-0.012831,0.000133,0.001800,-0.001488,0.000625,-0.000111,-0.218693,0.023672,0.019066,-0.000352,-0.002720,0.002254,-0.000948,0.000168,1.823448,-0.006592,-0.005369,0.000023,0.000743,-0.000613,0.000257,-0.000046,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.406812,-0.038360,-0.009466,0.001073,-0.000144,0.000100,-0.000039,0.000007,-0.177553,0.056627,0.013542,-0.001764,0.000218,-0.000152,0.000059,-0.000010,1.811851,-0.016131,-0.004072,0.000413,-0.000059,0.000041,-0.000016,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.359983,-0.054332,-0.006547,0.000962,0.000013,-0.000004,0.000002,-0.000000,-0.109032,0.078816,0.008714,-0.001584,-0.000012,0.000010,-0.000003,0.000001,1.792031,-0.023142,-0.002957,0.000367,0.000006,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.300077,-0.064495,-0.003593,0.001011,0.000017,0.000004,-0.000000,-0.000000,-0.023091,0.091478,0.003954,-0.001573,0.000015,0.000003,0.000002,-0.000001,1.766305,-0.027931,-0.001817,0.000397,0.000014,0.000004,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.233021,-0.068560,-0.000424,0.001104,0.000021,-0.000007,0.000001,0.000000,0.070787,0.094750,-0.000626,-0.001461,0.000042,-0.000005,-0.000004,0.000002,1.736972,-0.030298,-0.000501,0.000493,0.000032,-0.000006,-0.000002,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.165156,-0.066042,0.002956,0.001134,0.000000,-0.000002,0.000001,-0.000001,0.163484,0.089242,-0.004842,-0.001379,0.000004,-0.000002,0.000001,0.000000,1.706691,-0.029728,0.001098,0.000551,0.000003,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.103203,-0.056733,0.006348,0.001122,-0.000010,0.000009,-0.000009,0.000007,0.246508,0.075432,-0.008960,-0.001361,0.000013,-0.000012,0.000012,-0.000009,1.678613,-0.025876,0.002753,0.000548,-0.000005,0.000005,-0.000004,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.053938,-0.040666,0.009766,0.001250,0.000157,-0.000071,0.000164,-0.000120,0.311622,0.053425,-0.013108,-0.001526,-0.000202,0.000091,-0.000211,0.000155,1.656037,-0.018725,0.004420,0.000607,0.000073,-0.000033,0.000077,-0.000056,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.024417,-0.016971,0.013681,0.000238,-0.001948,0.000008,-0.002425,0.001599,0.350246,0.022100,-0.017892,-0.000213,0.002517,-0.000007,0.003123,-0.002064,1.642400,-0.007868,0.006321,0.000137,-0.000908,0.000005,-0.001134,0.000746,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/13.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/13.csv
new file mode 100644
index 0000000000000000000000000000000000000000..4386d9e6411259ecdc19163c4a0316922c2c8784
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/13.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.018599,0.000000,0.000000,0.000000,0.009400,-0.010107,0.004652,-0.000848,0.357810,0.000000,0.000000,0.000000,-0.022813,0.024533,-0.011294,0.002059,1.639700,0.000000,0.000000,0.000000,0.013335,-0.014341,0.006602,-0.001204,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.021696,0.009042,0.007305,-0.000107,-0.001034,0.000856,-0.000360,0.000064,0.350295,-0.021939,-0.017719,0.000265,0.002509,-0.002077,0.000873,-0.000155,1.644093,0.012822,0.010355,-0.000156,-0.001467,0.001214,-0.000510,0.000091,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.037462,0.021763,0.005284,-0.000645,0.000083,-0.000058,0.000022,-0.000004,0.312051,-0.052778,-0.012798,0.001571,-0.000201,0.000140,-0.000054,0.000009,1.666441,0.030838,0.007473,-0.000920,0.000118,-0.000082,0.000032,-0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.063908,0.030548,0.003526,-0.000577,-0.000005,0.000003,-0.000001,0.000000,0.247940,-0.074029,-0.008514,0.001406,0.000013,-0.000007,0.000003,-0.000000,1.703895,0.043240,0.004964,-0.000824,-0.000008,0.000004,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.097401,0.035856,0.001780,-0.000583,0.000000,-0.000000,-0.000000,-0.000000,0.166812,-0.086809,-0.004261,0.001421,-0.000001,0.000000,-0.000000,0.000000,1.751272,0.050682,0.002474,-0.000832,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.134455,0.037667,0.000030,-0.000584,-0.000001,-0.000000,0.000000,-0.000000,0.077161,-0.091071,-0.000001,0.001420,0.000000,0.000000,-0.000000,0.000000,1.803597,0.053137,-0.000019,-0.000830,0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.171567,0.035971,-0.001728,-0.000589,-0.000002,0.000000,-0.000000,0.000000,-0.012491,-0.086812,0.004260,0.001421,0.000001,-0.000001,0.000001,-0.000000,1.855885,0.050611,-0.002507,-0.000828,0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.205219,0.030743,-0.003500,-0.000589,0.000004,-0.000005,0.000005,-0.000004,-0.093622,-0.074031,0.008515,0.001407,-0.000013,0.000012,-0.000012,0.000009,1.903162,0.043118,-0.004980,-0.000816,0.000008,-0.000007,0.000007,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.231873,0.021968,-0.005303,-0.000663,-0.000085,0.000038,-0.000088,0.000065,-0.157734,-0.052777,0.012799,0.001570,0.000201,-0.000091,0.000211,-0.000155,1.940487,0.030710,-0.007461,-0.000908,-0.000116,0.000053,-0.000122,0.000090,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.247805,0.009149,-0.007382,-0.000119,0.001048,-0.000004,0.001304,-0.000860,-0.195975,-0.021937,0.017718,0.000264,-0.002509,0.000009,-0.003119,0.002059,1.962731,0.012755,-0.010306,-0.000149,0.001458,-0.000005,0.001812,-0.001196,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/14.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/14.csv
new file mode 100644
index 0000000000000000000000000000000000000000..d3b931a80f261281eb6d4efcb7e61c780be875b0
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/14.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.250940,0.000000,0.000000,0.000000,0.002861,-0.003067,0.001410,-0.000257,-0.203490,0.000000,0.000000,0.000000,0.028180,-0.030334,0.013970,-0.002548,1.967100,0.000000,0.000000,0.000000,-0.014367,0.015395,-0.007076,0.001289,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.251887,0.002772,0.002254,-0.000015,-0.000314,0.000259,-0.000109,0.000019,-0.194221,0.027039,0.021795,-0.000380,-0.003103,0.002570,-0.001081,0.000192,1.962341,-0.013928,-0.011328,0.000068,0.001574,-0.001299,0.000545,-0.000096,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.256754,0.006757,0.001691,-0.000179,0.000025,-0.000017,0.000006,-0.000001,-0.147190,0.064785,0.015554,-0.001994,0.000248,-0.000174,0.000068,-0.000011,1.937876,-0.033985,-0.008525,0.000892,-0.000126,0.000086,-0.000032,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.265036,0.009644,0.001199,-0.000168,-0.000009,-0.000002,0.000001,0.000000,-0.068714,0.090363,0.010103,-0.001782,-0.000005,0.000015,-0.000005,0.000001,1.896191,-0.048585,-0.006101,0.000816,0.000032,0.000005,-0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.275701,0.011500,0.000639,-0.000202,-0.000003,0.000004,-0.000001,0.000000,0.029976,0.105254,0.004819,-0.001724,0.000021,-0.000003,-0.000001,0.000000,1.842357,-0.058194,-0.003433,0.000963,0.000032,-0.000007,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.287637,0.012171,0.000036,-0.000198,0.000002,-0.000001,0.000000,0.000000,0.138342,0.109785,-0.000265,-0.001677,0.000004,-0.000002,0.000001,-0.000000,1.781717,-0.062079,-0.000421,0.001026,0.000004,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.299648,0.011655,-0.000551,-0.000195,0.000000,0.000000,-0.000000,0.000000,0.246187,0.104231,-0.005287,-0.001674,-0.000001,0.000001,-0.000001,0.000001,1.720245,-0.059837,0.002665,0.001029,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.310557,0.009969,-0.001134,-0.000193,0.000002,-0.000002,0.000002,-0.000001,0.343457,0.088638,-0.010299,-0.001657,0.000015,-0.000014,0.000014,-0.000011,1.664102,-0.051422,0.005746,0.001019,-0.000009,0.000009,-0.000008,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.319200,0.007122,-0.001721,-0.000215,-0.000027,0.000012,-0.000029,0.000021,0.420142,0.063063,-0.015348,-0.001853,-0.000240,0.000108,-0.000251,0.000184,1.619443,-0.036869,0.008850,0.001135,0.000142,-0.000064,0.000149,-0.000109,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.324364,0.002965,-0.002393,-0.000038,0.000340,-0.000001,0.000422,-0.000279,0.465806,0.026175,-0.021156,-0.000296,0.002990,-0.000010,0.003714,-0.002453,1.592675,-0.015389,0.012404,0.000217,-0.001766,0.000008,-0.002199,0.001450,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/15.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/15.csv
new file mode 100644
index 0000000000000000000000000000000000000000..208e4c96e348d3c9acf6bfb936ab1db526bdd17c
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/15.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.325380,0.000000,0.000000,0.000000,-0.030618,0.032999,-0.015206,0.002774,0.474770,0.000000,0.000000,0.000000,-0.041573,0.044451,-0.020411,0.003715,1.587400,0.000000,0.000000,0.000000,0.003344,-0.003703,0.001727,-0.000317,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.315329,-0.029296,-0.023556,0.000486,0.003376,-0.002799,0.001178,-0.000209,0.460952,-0.040496,-0.033074,0.000026,0.004544,-0.003746,0.001570,-0.000278,1.588450,0.002997,0.002266,-0.000231,-0.000380,0.000321,-0.000137,0.000024,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.264510,-0.069835,-0.016554,0.002238,-0.000270,0.000188,-0.000071,0.000012,0.389498,-0.099646,-0.025479,0.002413,-0.000366,0.000256,-0.000107,0.000020,1.593312,0.006275,0.000970,-0.000416,0.000031,-0.000020,0.000005,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.180217,-0.096714,-0.010400,0.002029,0.000020,-0.000005,-0.000012,0.000002,0.266589,-0.144051,-0.019060,0.002075,0.000016,-0.000036,0.000073,-0.000015,1.600157,0.007019,-0.000225,-0.000404,-0.000004,-0.000008,0.000026,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.075137,-0.111426,-0.004371,0.001900,-0.000108,0.000007,0.000013,-0.000004,0.105592,-0.175726,-0.012311,0.002725,0.000416,-0.000085,-0.000031,0.000014,1.606556,0.005424,-0.001254,-0.000156,0.000167,-0.000032,-0.000013,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.038851,-0.114813,0.000867,0.001664,-0.000013,0.000013,-0.000006,0.000001,-0.079406,-0.191025,-0.002672,0.003392,0.000002,-0.000023,0.000016,-0.000004,1.610697,0.002914,-0.001123,0.000122,0.000003,-0.000010,0.000007,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.151138,-0.108101,0.005847,0.001664,0.000003,-0.000002,0.000001,-0.000001,-0.269719,-0.186227,0.007453,0.003363,-0.000002,0.000001,-0.000001,-0.000001,1.612609,0.001027,-0.000770,0.000114,-0.000001,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.251726,-0.091410,0.010839,0.001653,-0.000015,0.000014,-0.000014,0.000011,-0.445134,-0.161246,0.017514,0.003327,-0.000030,0.000027,-0.000026,0.000021,1.612979,-0.000172,-0.000427,0.000115,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.330648,-0.064766,0.015880,0.001854,0.000245,-0.000110,0.000256,-0.000188,-0.585547,-0.116233,0.027633,0.003690,0.000450,-0.000205,0.000475,-0.000347,1.612495,-0.000679,-0.000077,0.000121,0.000006,-0.000003,0.000007,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.377477,-0.026799,0.021693,0.000262,-0.003053,0.000009,-0.003788,0.002504,-0.670084,-0.048703,0.039182,0.000781,-0.005607,0.000027,-0.006993,0.004607,1.611865,-0.000452,0.000298,0.000090,-0.000068,0.000003,-0.000094,0.000058,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/16.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/16.csv
new file mode 100644
index 0000000000000000000000000000000000000000..af605932b8cd1dfa8336df081ce99b21cea9b82d
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/16.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.386650,0.000000,0.000000,-0.000000,-0.001463,0.001573,-0.000724,0.000132,-0.686790,0.000000,0.000000,-0.000000,0.009173,-0.009864,0.004541,-0.000828,1.611700,0.000000,0.000000,0.000000,-0.007918,0.008515,-0.003920,0.000715,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.387132,-0.001407,-0.001136,0.000017,0.000161,-0.000133,0.000056,-0.000010,-0.683768,0.008821,0.007125,-0.000106,-0.001009,0.000835,-0.000351,0.000062,1.609092,-0.007614,-0.006150,0.000092,0.000871,-0.000721,0.000303,-0.000054,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.389584,-0.003384,-0.000820,0.000101,-0.000013,0.000009,-0.000003,0.000001,-0.668391,0.021222,0.005146,-0.000631,0.000081,-0.000056,0.000022,-0.000004,1.595819,-0.018317,-0.004442,0.000545,-0.000070,0.000049,-0.000019,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.393694,-0.004746,-0.000546,0.000090,0.000001,-0.000000,0.000000,-0.000000,-0.642612,0.029766,0.003423,-0.000566,-0.000005,0.000003,-0.000001,0.000000,1.573568,-0.025692,-0.002954,0.000488,0.000004,-0.000003,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.398895,-0.005565,-0.000273,0.000091,-0.000000,0.000000,-0.000000,0.000000,-0.609991,0.034905,0.001713,-0.000571,0.000000,-0.000000,0.000000,-0.000000,1.545413,-0.030127,-0.001478,0.000493,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.404642,-0.005838,0.000000,0.000091,-0.000000,0.000000,-0.000000,0.000000,-0.573943,0.036619,0.000000,-0.000571,-0.000000,-0.000000,0.000000,-0.000000,1.514300,-0.031605,0.000000,0.000493,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.410388,-0.005564,0.000273,0.000091,0.000000,-0.000000,0.000000,-0.000000,-0.537895,0.034907,-0.001713,-0.000571,-0.000000,0.000000,-0.000000,0.000000,1.483187,-0.030127,0.001478,0.000493,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.415588,-0.004745,0.000546,0.000090,-0.000001,0.000001,-0.000001,0.000001,-0.505272,0.029769,-0.003423,-0.000566,0.000005,-0.000005,0.000005,-0.000004,1.455031,-0.025692,0.002955,0.000488,-0.000004,0.000004,-0.000004,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.419697,-0.003383,0.000820,0.000101,0.000013,-0.000006,0.000014,-0.000010,-0.479491,0.021224,-0.005146,-0.000632,-0.000081,0.000037,-0.000085,0.000062,1.432781,-0.018317,0.004442,0.000545,0.000070,-0.000032,0.000073,-0.000054,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.422148,-0.001406,0.001136,0.000017,-0.000161,0.000001,-0.000200,0.000132,-0.464112,0.008823,-0.007126,-0.000106,0.001009,-0.000004,0.001254,-0.000828,1.419508,-0.007614,0.006150,0.000092,-0.000871,0.000003,-0.001083,0.000715,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/17.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/17.csv
new file mode 100644
index 0000000000000000000000000000000000000000..63d15f06f21d0b892d90acc2d5d8c769f06907d3
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/17.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/18.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/18.csv
new file mode 100644
index 0000000000000000000000000000000000000000..61cfa2a8e1be78cf900a1e88756223751e37a22f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/18.csv
@@ -0,0 +1,8 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/19.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/19.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3dffdfa3c2d505470f1bce4b450efe7f7aa5508b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/19.csv
@@ -0,0 +1,20 @@
+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,
+1.000000,-0.520030,0.000000,0.000000,0.000000,0.014075,-0.014919,0.006822,-0.001238,-0.194220,0.000000,0.000000,0.000000,-0.002858,0.003136,-0.001457,0.000267,1.350200,0.000000,0.000000,0.000000,-0.015941,0.016926,-0.007748,0.001407,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.515289,0.013975,0.011596,0.000219,-0.001527,0.001247,-0.000520,0.000092,-0.195132,-0.002622,-0.002029,0.000143,0.000320,-0.000271,0.000115,-0.000020,1.344844,-0.015770,-0.013051,-0.000205,0.001727,-0.001418,0.000591,-0.000104,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.490208,0.035471,0.009683,-0.000614,0.000115,-0.000083,0.000032,-0.000005,-0.199496,-0.005779,-0.001095,0.000295,-0.000030,0.000018,-0.000007,0.000001,1.316614,-0.039850,-0.010801,0.000703,-0.000141,0.000094,-0.000036,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.445610,0.053191,0.008061,-0.000541,-0.000014,0.000004,-0.000002,0.000000,-0.206092,-0.007145,-0.000287,0.000259,-0.000002,-0.000001,0.000000,-0.000000,1.266589,-0.059610,-0.009009,0.000574,0.000005,-0.000005,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.384911,0.067648,0.006380,-0.000575,-0.000006,-0.000000,0.000000,0.000000,-0.213269,-0.006958,0.000466,0.000242,-0.000005,0.000000,0.000000,-0.000000,1.198545,-0.075904,-0.007288,0.000568,-0.000005,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.311465,0.078655,0.004615,-0.000599,-0.000003,0.000001,0.000000,0.000000,-0.219523,-0.005315,0.001169,0.000227,-0.000003,-0.000000,-0.000000,0.000000,1.115916,-0.088798,-0.005619,0.000541,-0.000010,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.228795,0.086084,0.002816,-0.000592,0.000010,0.000003,-0.000003,0.000001,-0.223445,-0.002308,0.001829,0.000211,-0.000005,-0.000001,-0.000000,0.000000,1.022029,-0.098456,-0.004061,0.000497,-0.000013,-0.000000,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.140476,0.089985,0.001106,-0.000550,0.000011,0.000001,-0.000002,0.000001,-0.223720,0.001955,0.002416,0.000174,-0.000014,-0.000002,0.000001,-0.000000,0.919998,-0.105132,-0.002633,0.000460,-0.000006,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.049923,0.090591,-0.000480,-0.000509,0.000010,0.000001,-0.000001,0.000000,-0.219191,0.007244,0.002838,0.000102,-0.000024,-0.000003,0.000002,-0.000001,0.812689,-0.109037,-0.001281,0.000443,-0.000004,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.039690,0.088150,-0.001937,-0.000458,0.000018,0.000002,-0.000002,0.000001,-0.209034,0.013122,0.002988,-0.000003,-0.000029,-0.000001,0.000002,-0.000001,0.702810,-0.110285,0.000025,0.000427,-0.000004,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.125464,0.082978,-0.003194,-0.000374,0.000030,0.000004,-0.000004,0.000001,-0.192956,0.018973,0.002807,-0.000118,-0.000032,-0.000002,0.000004,-0.000001,0.592973,-0.108970,0.001282,0.000411,-0.000004,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.204906,0.075594,-0.004128,-0.000248,0.000036,0.000003,-0.000004,0.000001,-0.171325,0.024110,0.002273,-0.000233,-0.000026,0.000000,0.000002,-0.000001,0.485693,-0.105188,0.002493,0.000397,-0.000003,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.276159,0.066731,-0.004672,-0.000122,0.000027,0.000000,-0.000003,0.000001,-0.145200,0.027861,0.001438,-0.000315,-0.000015,0.000001,0.000001,-0.000000,0.383393,-0.099020,0.003669,0.000388,-0.000002,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.338122,0.057119,-0.004902,-0.000045,0.000009,-0.000003,0.000001,-0.000000,-0.116229,0.029742,0.000424,-0.000353,-0.000004,0.000002,-0.000000,0.000000,0.288428,-0.090523,0.004826,0.000384,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.390300,0.047204,-0.005003,-0.000026,0.000003,0.000000,-0.000000,0.000000,-0.086419,0.029519,-0.000651,-0.000362,-0.000001,0.000000,0.000000,-0.000000,0.203115,-0.079720,0.005976,0.000383,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.432478,0.037132,-0.005063,-0.000015,0.000002,-0.000000,-0.000000,0.000000,-0.057914,0.027128,-0.001743,-0.000366,-0.000001,0.000000,-0.000000,0.000000,0.129753,-0.066621,0.007122,0.000382,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.464534,0.026967,-0.005099,-0.000007,0.000004,-0.000003,0.000002,-0.000002,-0.032895,0.022543,-0.002841,-0.000364,0.000003,-0.000003,0.000003,-0.000003,0.070636,-0.051234,0.008261,0.000373,-0.000007,0.000006,-0.000006,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.486396,0.016748,-0.005136,-0.000048,-0.000050,0.000020,-0.000047,0.000037,-0.013557,0.015766,-0.003955,-0.000413,-0.000059,0.000026,-0.000061,0.000045,0.028036,-0.033588,0.009419,0.000466,0.000112,-0.000047,0.000111,-0.000084,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.497919,0.006206,-0.005312,0.000303,0.000638,0.000011,0.000749,-0.000513,-0.002208,0.006461,-0.005255,-0.000032,0.000730,-0.000001,0.000903,-0.000598,0.004423,-0.013067,0.010909,-0.000290,-0.001409,-0.000011,-0.001699,0.001143,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/2.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..d173fd999ff02dc8c7f7c73fcf56ab3fc7b310ae
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/2.csv
@@ -0,0 +1,17 @@
+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,
+1.000000,1.379600,0.000000,0.000000,0.000000,0.001411,-0.001499,0.000686,-0.000125,0.794990,0.000000,0.000000,0.000000,-0.005338,0.005666,-0.002593,0.000471,0.826530,0.000000,0.000000,0.000000,0.012423,-0.013186,0.006034,-0.001096,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.380074,0.001395,0.001153,0.000017,-0.000153,0.000126,-0.000052,0.000009,0.793196,-0.005285,-0.004376,-0.000072,0.000579,-0.000474,0.000198,-0.000035,0.830706,0.012300,0.010185,0.000167,-0.001347,0.001104,-0.000460,0.000081,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.382567,0.003516,0.000948,-0.000065,0.000012,-0.000008,0.000003,-0.000001,0.783731,-0.013365,-0.003626,0.000236,-0.000046,0.000031,-0.000012,0.000002,0.852737,0.031110,0.008442,-0.000550,0.000107,-0.000073,0.000028,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.386973,0.005240,0.000780,-0.000055,-0.000001,0.000000,-0.000000,0.000000,0.766951,-0.019994,-0.003017,0.000198,0.000003,-0.000002,0.000001,-0.000000,0.891796,0.046540,0.007021,-0.000462,-0.000007,0.000004,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.392939,0.006635,0.000615,-0.000055,0.000000,-0.000000,0.000000,-0.000000,0.744139,-0.025430,-0.002418,0.000200,-0.000000,0.000000,-0.000000,0.000000,0.944892,0.059184,0.005619,-0.000469,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.400133,0.007699,0.000449,-0.000055,0.000000,0.000000,0.000000,0.000000,0.716491,-0.029665,-0.001817,0.000200,0.000000,0.000000,0.000000,-0.000000,1.009226,0.069016,0.004213,-0.000468,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.408226,0.008433,0.000286,-0.000054,0.000000,0.000000,-0.000000,0.000000,0.685209,-0.032699,-0.001217,0.000200,0.000000,0.000000,0.000000,-0.000000,1.081987,0.076037,0.002808,-0.000468,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.416891,0.008842,0.000124,-0.000053,0.000000,0.000000,-0.000000,0.000000,0.651494,-0.034531,-0.000615,0.000201,0.000000,0.000000,0.000000,-0.000000,1.160364,0.080248,0.001402,-0.000468,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.425804,0.008931,-0.000035,-0.000052,0.000000,0.000000,-0.000000,0.000000,0.616549,-0.035159,-0.000012,0.000201,0.000000,-0.000000,0.000000,-0.000000,1.241545,0.081647,-0.000003,-0.000468,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.434648,0.008706,-0.000189,-0.000051,0.000000,0.000000,-0.000000,0.000000,0.581579,-0.034579,0.000592,0.000202,0.000000,-0.000000,0.000000,-0.000000,1.322722,0.080237,-0.001408,-0.000468,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.443115,0.008178,-0.000338,-0.000049,0.000000,-0.000000,-0.000000,0.000000,0.547794,-0.032789,0.001199,0.000203,0.000000,-0.000000,0.000000,-0.000000,1.401082,0.076017,-0.002812,-0.000468,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.450906,0.007357,-0.000482,-0.000047,0.000000,-0.000000,0.000000,-0.000000,0.516407,-0.029783,0.001807,0.000203,0.000000,0.000000,0.000000,0.000000,1.473819,0.068988,-0.004216,-0.000468,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.457733,0.006252,-0.000622,-0.000046,0.000000,0.000000,-0.000000,0.000000,0.488635,-0.025558,0.002418,0.000204,0.000000,-0.000000,0.000000,-0.000000,1.538122,0.059152,-0.005620,-0.000468,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.463318,0.004872,-0.000756,-0.000044,0.000001,-0.000001,0.000001,-0.000000,0.465699,-0.020111,0.003028,0.000201,-0.000003,0.000003,-0.000002,0.000002,1.591186,0.046508,-0.007020,-0.000461,0.000007,-0.000006,0.000006,-0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.467391,0.003230,-0.000889,-0.000052,-0.000011,0.000005,-0.000011,0.000008,0.448815,-0.013452,0.003646,0.000239,0.000046,-0.000020,0.000047,-0.000035,1.630216,0.031085,-0.008437,-0.000549,-0.000107,0.000046,-0.000108,0.000081,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.469670,0.001268,-0.001054,0.000022,0.000138,0.000001,0.000167,-0.000112,0.439287,-0.005321,0.004405,-0.000071,-0.000583,-0.000003,-0.000709,0.000474,1.652228,0.012289,-0.010177,0.000168,0.001345,0.000006,0.001635,-0.001095,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/3.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..c81a4970d17f91a154fa5354c471b51815df9261
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/3.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/4.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..08a8ed073bdf6418774838ff202c1b995b76eb39
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/4.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,1.444200,0.000000,0.000000,0.000000,-0.030286,0.032567,-0.014992,0.002733,0.079829,0.000000,0.000000,0.000000,0.005651,-0.006078,0.002798,-0.000510,1.597800,0.000000,0.000000,-0.000000,0.023786,-0.025579,0.011775,-0.002147,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.434223,-0.029128,-0.023528,0.000348,0.003331,-0.002757,0.001159,-0.000206,0.081691,0.005434,0.004389,-0.000066,-0.000622,0.000515,-0.000216,0.000038,1.605636,0.022874,0.018474,-0.000276,-0.002616,0.002166,-0.000910,0.000161,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.383443,-0.070085,-0.017003,0.002082,-0.000267,0.000186,-0.000072,0.000013,0.091163,0.013072,0.003169,-0.000389,0.000050,-0.000035,0.000013,-0.000002,1.645508,0.055025,0.013342,-0.001638,0.000210,-0.000146,0.000057,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.298295,-0.098332,-0.011324,0.001865,0.000017,-0.000010,0.000004,-0.000001,0.107041,0.018332,0.002106,-0.000349,-0.000003,0.000002,-0.000001,0.000000,1.712348,0.077177,0.008874,-0.001467,-0.000013,0.000008,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.190515,-0.115349,-0.005685,0.001884,-0.000000,0.000000,0.000000,-0.000000,0.127128,0.021492,0.001052,-0.000352,0.000000,-0.000000,0.000000,-0.000000,1.796924,0.090496,0.004439,-0.001481,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.071365,-0.121066,-0.000031,0.001886,0.000001,0.000000,0.000000,-0.000000,0.149321,0.022542,-0.000002,-0.000351,0.000000,-0.000000,0.000000,-0.000000,1.890379,0.094932,-0.000003,-0.001480,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.952155,-0.115466,0.005633,0.001891,0.000002,-0.000002,0.000001,-0.000001,0.171509,0.021483,-0.001056,-0.000351,-0.000000,0.000000,-0.000000,0.000000,1.983828,0.090486,-0.004444,-0.001481,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.844214,-0.098526,0.011301,0.001877,-0.000016,0.000016,-0.000016,0.000013,0.191585,0.018318,-0.002108,-0.000348,0.000003,-0.000003,0.000003,-0.000002,2.068390,0.077160,-0.008876,-0.001466,0.000014,-0.000013,0.000012,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.758863,-0.070284,0.017023,0.002099,0.000269,-0.000122,0.000281,-0.000206,0.207449,0.013059,-0.003167,-0.000388,-0.000050,0.000023,-0.000052,0.000038,2.135211,0.055006,-0.013340,-0.001636,-0.000210,0.000095,-0.000220,0.000161,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.707924,-0.029230,0.023602,0.000360,-0.003345,0.000013,-0.004159,0.002745,0.216911,0.005428,-0.004384,-0.000065,0.000621,-0.000002,0.000772,-0.000509,2.175068,0.022864,-0.018467,-0.000275,0.002615,-0.000010,0.003250,-0.002146,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/5.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/5.csv
new file mode 100644
index 0000000000000000000000000000000000000000..c8d0749024d3f3d0dccde5e2bc6739e980434dc6
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/5.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/6.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/6.csv
new file mode 100644
index 0000000000000000000000000000000000000000..2237b972c884e087e1053c974b275dfe042ae9ee
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/6.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.247700,0.000000,0.000000,0.000000,0.017754,-0.019092,0.008789,-0.001602,0.412460,0.000000,0.000000,0.000000,0.001893,-0.002035,0.000937,-0.000171,1.445800,0.000000,0.000000,0.000000,-0.025227,0.027129,-0.012489,0.002277,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.253549,0.017073,0.013790,-0.000206,-0.001953,0.001616,-0.000679,0.000121,0.413084,0.001820,0.001470,-0.000022,-0.000208,0.000172,-0.000072,0.000013,1.437489,-0.024260,-0.019594,0.000293,0.002775,-0.002297,0.000965,-0.000171,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.283310,0.041073,0.009959,-0.001222,0.000157,-0.000109,0.000042,-0.000007,0.416256,0.004379,0.001062,-0.000130,0.000017,-0.000012,0.000005,-0.000001,1.395200,-0.058362,-0.014152,0.001737,-0.000223,0.000155,-0.000060,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.333203,0.057610,0.006625,-0.001095,-0.000010,0.000006,-0.000002,0.000000,0.421575,0.006141,0.000706,-0.000117,-0.000001,0.000001,-0.000000,0.000000,1.324305,-0.081860,-0.009414,0.001555,0.000014,-0.000008,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.396337,0.067554,0.003315,-0.001106,0.000001,-0.000000,0.000000,-0.000000,0.428305,0.007201,0.000353,-0.000118,0.000000,-0.000000,0.000000,-0.000000,1.234596,-0.095991,-0.004711,0.001571,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.466100,0.070868,-0.000000,-0.001105,0.000000,-0.000000,0.000000,-0.000000,0.435741,0.007554,-0.000000,-0.000118,0.000000,-0.000000,0.000000,-0.000000,1.135465,-0.100701,0.000000,0.001570,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.535864,0.067554,-0.003315,-0.001106,-0.000000,0.000001,-0.000000,0.000000,0.443177,0.007201,-0.000353,-0.000118,-0.000000,0.000000,-0.000000,0.000000,1.036334,-0.095991,0.004711,0.001571,0.000001,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.598997,0.057609,-0.006625,-0.001095,0.000010,-0.000009,0.000009,-0.000007,0.449907,0.006140,-0.000706,-0.000117,0.000001,-0.000001,0.000001,-0.000001,0.946624,-0.081860,0.009414,0.001555,-0.000014,0.000013,-0.000013,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.648890,0.041073,-0.009959,-0.001222,-0.000157,0.000071,-0.000164,0.000121,0.455225,0.004378,-0.001062,-0.000130,-0.000017,0.000008,-0.000017,0.000013,0.875730,-0.058362,0.014152,0.001737,0.000223,-0.000101,0.000233,-0.000171,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.678651,0.017074,-0.013790,-0.000206,0.001953,-0.000007,0.002428,-0.001602,0.458397,0.001820,-0.001470,-0.000022,0.000208,-0.000001,0.000259,-0.000171,0.833440,-0.024260,0.019594,0.000293,-0.002775,0.000010,-0.003449,0.002277,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/7.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/7.csv
new file mode 100644
index 0000000000000000000000000000000000000000..f2ebf12dacea26beea51f2d1b0f2f9f2d08fe4eb
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/7.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,0.684500,0.000000,0.000000,-0.000000,0.011864,-0.012717,0.005846,-0.001065,0.459020,0.000000,0.000000,0.000000,-0.020736,0.022227,-0.010217,0.001861,0.825130,0.000000,0.000000,0.000000,0.040625,-0.043545,0.020017,-0.003646,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.688428,0.011493,0.009342,-0.000064,-0.001300,0.001074,-0.000450,0.000080,0.452154,-0.020088,-0.016328,0.000112,0.002273,-0.001877,0.000787,-0.000139,0.838581,0.039354,0.031988,-0.000218,-0.004452,0.003676,-0.001542,0.000273,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.708602,0.028009,0.007005,-0.000744,0.000104,-0.000072,0.000028,-0.000005,0.416894,-0.048954,-0.012243,0.001301,-0.000182,0.000126,-0.000049,0.000008,0.907659,0.095906,0.023985,-0.002549,0.000356,-0.000246,0.000095,-0.000017,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.742927,0.039975,0.004993,-0.000659,-0.000007,0.000004,-0.000001,0.000000,0.356902,-0.069868,-0.008726,0.001152,0.000012,-0.000007,0.000002,-0.000000,1.025190,0.136879,0.017096,-0.002258,-0.000023,0.000013,-0.000005,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.787232,0.047970,0.002998,-0.000667,0.000000,-0.000000,0.000000,-0.000000,0.279467,-0.083839,-0.005240,0.001165,-0.000001,0.000000,-0.000000,0.000000,1.176894,0.164250,0.010265,-0.002282,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.837534,0.051968,0.001000,-0.000666,-0.000000,0.000000,-0.000000,-0.000000,0.191553,-0.090825,-0.001746,0.001164,0.000000,-0.000000,0.000000,-0.000000,1.349127,0.177936,0.003421,-0.002281,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.889836,0.051970,-0.000998,-0.000666,0.000000,-0.000000,0.000000,-0.000000,0.100147,-0.090825,0.001746,0.001164,-0.000000,0.000000,-0.000000,0.000000,1.528203,0.177936,-0.003421,-0.002281,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.940141,0.047975,-0.002997,-0.000667,-0.000000,0.000001,-0.000000,0.000000,0.012233,-0.083839,0.005240,0.001165,0.000001,-0.000001,0.000000,-0.000000,1.700437,0.164250,-0.010265,-0.002282,-0.000001,0.000002,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.984454,0.039984,-0.004992,-0.000659,0.000007,-0.000006,0.000006,-0.000005,-0.065202,-0.069868,0.008726,0.001152,-0.000012,0.000011,-0.000010,0.000008,1.852140,0.136879,-0.017096,-0.002258,0.000023,-0.000021,0.000020,-0.000017,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.018787,0.028019,-0.007005,-0.000745,-0.000104,0.000047,-0.000108,0.000080,-0.125194,-0.048954,0.012243,0.001301,0.000182,-0.000081,0.000189,-0.000139,1.969671,0.095906,-0.023985,-0.002549,-0.000356,0.000159,-0.000370,0.000273,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.038970,0.011499,-0.009346,-0.000065,0.001301,-0.000002,0.001609,-0.001065,-0.160454,-0.020088,0.016328,0.000112,-0.002273,0.000004,-0.002810,0.001861,2.038749,0.039354,-0.031988,-0.000218,0.004452,-0.000007,0.005505,-0.003646,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/8.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/8.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3e6acbbe77c3c0c9904a3d5c5ef181f324da9fda
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/8.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/5/9.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/5/9.csv
new file mode 100644
index 0000000000000000000000000000000000000000..60c51acba62d64ee5b2448d58c73563db54431af
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/5/9.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/1.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..5056662c1b001146968346d153611165edf5688a
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/1.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,1.000000,0.000000,0.000000,0.000000,0.006815,-0.007319,0.003367,-0.000614,0.000000,0.000000,0.000000,0.000000,0.001874,-0.001996,0.000915,-0.000166,1.500000,0.000000,0.000000,0.000000,-0.012214,0.013091,-0.006018,0.001096,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.002250,0.006573,0.005323,-0.000062,-0.000749,0.000619,-0.000260,0.000046,0.000627,0.001843,0.001518,0.000014,-0.000204,0.000167,-0.000070,0.000012,1.495955,-0.011834,-0.009621,0.000064,0.001339,-0.001105,0.000464,-0.000082,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.013741,0.015898,0.003905,-0.000452,0.000061,-0.000041,0.000016,-0.000003,0.003909,0.004610,0.001222,-0.000094,0.000016,-0.000011,0.000004,-0.000001,1.475179,-0.028850,-0.007220,0.000764,-0.000107,0.000074,-0.000029,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.033125,0.022466,0.002685,-0.000396,-0.000000,0.000003,-0.000001,0.000000,0.009655,0.006801,0.000972,-0.000083,-0.000003,-0.000001,0.000000,0.000000,1.439817,-0.041192,-0.005155,0.000677,0.000007,-0.000004,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.057882,0.026659,0.001517,-0.000380,0.000005,-0.000000,-0.000000,0.000000,0.017341,0.008480,0.000698,-0.000100,-0.000005,-0.000000,0.000000,-0.000000,1.394152,-0.049454,-0.003103,0.000686,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.085684,0.028573,0.000405,-0.000363,0.000003,-0.000000,0.000000,-0.000000,0.026414,0.009557,0.000371,-0.000117,-0.000004,-0.000000,-0.000000,0.000000,1.342281,-0.053601,-0.001044,0.000687,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.114300,0.028303,-0.000668,-0.000352,0.000003,-0.000001,0.000000,-0.000000,0.036220,0.009931,-0.000005,-0.000135,-0.000005,0.000000,-0.000000,0.000000,1.288324,-0.053627,0.001019,0.000688,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.141586,0.025921,-0.001711,-0.000345,0.000001,-0.000000,0.000000,0.000000,0.046007,0.009497,-0.000439,-0.000154,-0.000004,0.000001,-0.000000,0.000000,1.236404,-0.049523,0.003085,0.000690,0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.165453,0.021469,-0.002739,-0.000340,0.000004,-0.000003,0.000003,-0.000003,0.054908,0.008147,-0.000914,-0.000160,0.000001,-0.000001,0.000001,-0.000001,1.190656,-0.041285,0.005150,0.000682,-0.000007,0.000006,-0.000006,0.000005,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.183844,0.014971,-0.003776,-0.000385,-0.000055,0.000025,-0.000057,0.000042,0.061982,0.005839,-0.001402,-0.000179,-0.000023,0.000010,-0.000024,0.000017,1.155202,-0.028935,0.007233,0.000771,0.000108,-0.000048,0.000112,-0.000082,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.194609,0.006121,-0.004984,-0.000023,0.000690,-0.000001,0.000852,-0.000565,0.066221,0.002437,-0.001964,-0.000034,0.000280,-0.000001,0.000348,-0.000230,1.134359,-0.011876,0.009652,0.000067,-0.001344,0.000002,-0.001662,0.001100,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/10.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/10.csv
new file mode 100644
index 0000000000000000000000000000000000000000..8efe3c671876f775fdc0d463e81bf0c75cb620df
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/10.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/11.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/11.csv
new file mode 100644
index 0000000000000000000000000000000000000000..9bc940e836df1223b9068ab3e880fe55525bb2d0
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/11.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,-0.218370,0.000000,0.000000,0.000000,0.009757,-0.010464,0.004812,-0.000877,-0.225090,0.000000,0.000000,0.000000,-0.000875,0.001001,-0.000473,0.000088,1.247200,0.000000,0.000000,0.000000,-0.002267,0.002452,-0.001132,0.000207,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.215142,0.009441,0.007667,-0.000060,-0.001068,0.000884,-0.000371,0.000066,-0.225350,-0.000721,-0.000497,0.000116,0.000102,-0.000089,0.000039,-0.000007,1.246460,-0.002150,-0.001716,0.000052,0.000251,-0.000209,0.000088,-0.000016,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.198584,0.022977,0.005735,-0.000610,0.000089,-0.000059,0.000023,-0.000004,-0.226407,-0.001223,0.000001,0.000157,-0.000012,0.000006,-0.000003,0.000000,1.242760,-0.005048,-0.001151,0.000180,-0.000021,0.000014,-0.000005,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.170431,0.032792,0.004117,-0.000519,0.000002,0.000004,-0.000001,0.000000,-0.227480,-0.000780,0.000430,0.000132,-0.000006,-0.000002,0.000000,-0.000000,1.236730,-0.006849,-0.000658,0.000159,-0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.134037,0.039489,0.002595,-0.000491,0.000009,0.000000,-0.000000,0.000000,-0.227706,0.000444,0.000775,0.000094,-0.000013,-0.000002,0.000001,-0.000000,1.229381,-0.007694,-0.000192,0.000149,-0.000005,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.092435,0.043241,0.001174,-0.000458,0.000006,-0.000001,0.000000,-0.000000,-0.226405,0.002222,0.000973,0.000036,-0.000016,-0.000001,0.000000,-0.000000,1.221639,-0.007653,0.000222,0.000126,-0.000007,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.048471,0.044238,-0.000169,-0.000440,0.000003,-0.000001,-0.000000,0.000000,-0.223190,0.004210,0.000980,-0.000033,-0.000018,-0.000000,-0.000000,0.000000,1.214327,-0.006860,0.000554,0.000093,-0.000010,-0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.004840,0.042586,-0.001482,-0.000437,-0.000000,-0.000000,-0.000000,0.000000,-0.218052,0.005995,0.000765,-0.000113,-0.000021,-0.000000,-0.000000,0.000000,1.208103,-0.005518,0.000764,0.000044,-0.000014,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.035826,0.038309,-0.002799,-0.000443,-0.000003,-0.000000,-0.000001,0.000000,-0.211426,0.007103,0.000301,-0.000194,-0.000017,0.000004,-0.000001,0.000000,1.203378,-0.003916,0.000808,-0.000015,-0.000013,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.070890,0.031367,-0.004150,-0.000456,0.000001,-0.000002,0.000004,-0.000004,-0.204231,0.007068,-0.000356,-0.000237,-0.000006,0.000000,0.000001,-0.000001,1.200243,-0.002389,0.000701,-0.000052,-0.000006,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.097651,0.021692,-0.005551,-0.000524,-0.000079,0.000035,-0.000082,0.000060,-0.197761,0.005624,-0.001103,-0.000273,-0.000027,0.000012,-0.000027,0.000020,1.198499,-0.001160,0.000522,-0.000064,-0.000000,0.000000,-0.000000,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.113203,0.008813,-0.007199,-0.000004,0.000988,-0.000000,0.001217,-0.000808,-0.193536,0.002524,-0.001964,-0.000124,0.000306,-0.000004,0.000393,-0.000254,1.197796,-0.000311,0.000320,-0.000082,-0.000020,-0.000003,-0.000013,0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/12.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/12.csv
new file mode 100644
index 0000000000000000000000000000000000000000..cc716dccf1a5f2f11d195ff48451c00a20586696
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/12.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.116210,0.000000,0.000000,0.000000,0.005626,-0.006049,0.002785,-0.000508,-0.192660,0.000000,0.000000,0.000000,0.009313,-0.010014,0.004610,-0.000840,1.197700,0.000000,0.000000,0.000000,-0.005839,0.006280,-0.002891,0.000527,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.118064,0.005412,0.004372,-0.000064,-0.000619,0.000512,-0.000215,0.000038,-0.189591,0.008958,0.007237,-0.000106,-0.001024,0.000848,-0.000356,0.000063,1.195777,-0.005614,-0.004533,0.000069,0.000642,-0.000532,0.000224,-0.000040,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.127500,0.013026,0.003162,-0.000386,0.000050,-0.000034,0.000013,-0.000002,-0.173972,0.021559,0.005233,-0.000640,0.000082,-0.000057,0.000022,-0.000004,1.185993,-0.013499,-0.003269,0.000403,-0.000052,0.000036,-0.000014,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.143328,0.018282,0.002109,-0.000346,-0.000003,0.000002,-0.000001,0.000000,-0.147776,0.030256,0.003489,-0.000573,-0.000005,0.000003,-0.000001,0.000000,1.169601,-0.018921,-0.002169,0.000361,0.000003,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.163371,0.021454,0.001062,-0.000350,-0.000000,-0.000000,-0.000000,0.000000,-0.114608,0.035503,0.001756,-0.000579,-0.000000,-0.000000,0.000000,-0.000000,1.148874,-0.022169,-0.001078,0.000364,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.185536,0.022525,0.000008,-0.000352,-0.000000,0.000000,-0.000000,0.000000,-0.077928,0.037276,0.000017,-0.000581,-0.000001,-0.000000,-0.000000,0.000000,1.125991,-0.023234,0.000012,0.000362,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.207717,0.021485,-0.001049,-0.000353,-0.000000,0.000000,-0.000000,0.000000,-0.041216,0.035565,-0.001729,-0.000584,-0.000001,0.000001,-0.000000,0.000000,1.103131,-0.022125,0.001096,0.000361,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.227800,0.018329,-0.002105,-0.000349,0.000003,-0.000003,0.000003,-0.000002,-0.007965,0.030355,-0.003479,-0.000579,0.000005,-0.000005,0.000005,-0.000004,1.082463,-0.018852,0.002175,0.000357,-0.000003,0.000003,-0.000003,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.243676,0.013071,-0.003168,-0.000390,-0.000050,0.000023,-0.000052,0.000038,0.018333,0.021656,-0.005245,-0.000647,-0.000083,0.000037,-0.000087,0.000064,1.066142,-0.013431,0.003261,0.000398,0.000051,-0.000023,0.000054,-0.000039,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.253148,0.005434,-0.004389,-0.000066,0.000622,-0.000002,0.000773,-0.000510,0.034028,0.009007,-0.007272,-0.000111,0.001031,-0.000004,0.001281,-0.000846,1.056411,-0.005580,0.004508,0.000066,-0.000638,0.000002,-0.000793,0.000523,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/13.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/13.csv
new file mode 100644
index 0000000000000000000000000000000000000000..9c80c5992573e1a2928993c077b93728009df6d6
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/13.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.255010,0.000000,0.000000,0.000000,0.005572,-0.005999,0.002763,-0.000504,0.037114,0.000000,0.000000,-0.000000,-0.004824,0.005190,-0.002390,0.000436,1.054500,0.000000,0.000000,0.000000,0.009180,-0.009868,0.004542,-0.000828,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.256843,0.005345,0.004308,-0.000076,-0.000614,0.000508,-0.000214,0.000038,0.035526,-0.004634,-0.003739,0.000061,0.000531,-0.000440,0.000185,-0.000033,1.057526,0.008835,0.007141,-0.000100,-0.001009,0.000835,-0.000351,0.000062,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.266139,0.012803,0.003072,-0.000394,0.000050,-0.000034,0.000013,-0.000002,0.027458,-0.011124,-0.002684,0.000336,-0.000043,0.000030,-0.000012,0.000002,1.072939,0.021286,0.005180,-0.000626,0.000081,-0.000056,0.000022,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.281645,0.017854,0.001995,-0.000353,-0.000002,0.000002,-0.000001,0.000000,0.013963,-0.015563,-0.001768,0.000300,0.000002,-0.000002,0.000001,-0.000000,1.098823,0.029916,0.003474,-0.000561,-0.000006,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.301140,0.020783,0.000936,-0.000351,0.000002,0.000000,-0.000000,0.000000,-0.003068,-0.018199,-0.000868,0.000299,-0.000001,0.000000,0.000000,-0.000000,1.131648,0.035168,0.001775,-0.000569,-0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.322510,0.021610,-0.000104,-0.000342,0.000003,0.000000,-0.000000,0.000000,-0.021837,-0.019042,0.000022,0.000295,-0.000001,0.000000,-0.000000,0.000000,1.168022,0.037008,0.000062,-0.000574,-0.000002,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.343677,0.020389,-0.001111,-0.000329,0.000003,0.000000,-0.000000,0.000000,-0.040563,-0.018116,0.000903,0.000293,-0.000000,-0.000000,0.000000,-0.000000,1.204516,0.035403,-0.001671,-0.000582,-0.000002,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.362629,0.017193,-0.002076,-0.000311,0.000007,-0.000003,0.000002,-0.000002,-0.057482,-0.015431,0.001780,0.000290,-0.000002,0.000003,-0.000002,0.000002,1.237665,0.030308,-0.003426,-0.000585,0.000003,-0.000005,0.000005,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.377438,0.012120,-0.003004,-0.000336,-0.000043,0.000020,-0.000047,0.000035,-0.070843,-0.010997,0.002667,0.000326,0.000042,-0.000019,0.000044,-0.000032,1.263961,0.021690,-0.005221,-0.000660,-0.000084,0.000038,-0.000087,0.000064,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.386183,0.004991,-0.004050,-0.000037,0.000566,-0.000001,0.000701,-0.000464,-0.078811,-0.004571,0.003692,0.000055,-0.000523,0.000002,-0.000650,0.000429,1.279700,0.009044,-0.007293,-0.000123,0.001037,-0.000004,0.001291,-0.000851,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/14.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/14.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0d3b9d2d885c3001f6eebdcc2c08e7db9f750d94
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/14.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.387890,0.000000,0.000000,-0.000000,-0.014963,0.016089,-0.007406,0.001350,-0.080377,0.000000,0.000000,0.000000,0.004617,-0.004960,0.002283,-0.000416,1.282800,0.000000,0.000000,-0.000000,-0.006670,0.007182,-0.003308,0.000603,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.382960,-0.014395,-0.011629,0.000169,0.001646,-0.001362,0.000572,-0.000102,-0.078854,0.004448,0.003598,-0.000046,-0.000507,0.000420,-0.000176,0.000031,1.280607,-0.006395,-0.005152,0.000094,0.000735,-0.000609,0.000256,-0.000045,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.357859,-0.034650,-0.008414,0.001026,-0.000132,0.000092,-0.000036,0.000006,-0.071088,0.010734,0.002624,-0.000311,0.000041,-0.000028,0.000011,-0.000002,1.269491,-0.015303,-0.003662,0.000475,-0.000060,0.000041,-0.000016,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.315752,-0.048637,-0.005612,0.000922,0.000010,-0.000005,0.000002,-0.000000,-0.058019,0.015124,0.001778,-0.000279,-0.000004,0.000000,-0.000000,0.000000,1.250969,-0.021314,-0.002371,0.000419,-0.000000,-0.000004,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.262432,-0.057071,-0.002816,0.000936,0.000000,-0.000000,0.000000,-0.000000,-0.041399,0.017829,0.000920,-0.000292,-0.000002,0.000001,0.000000,-0.000000,1.227700,-0.024811,-0.001135,0.000403,-0.000004,0.000001,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.203481,-0.059894,-0.000008,0.000935,-0.000000,0.000000,-0.000000,0.000000,-0.022944,0.018785,0.000034,-0.000297,-0.000000,0.000000,-0.000000,0.000000,1.202154,-0.025885,0.000058,0.000395,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.144514,-0.057105,0.002797,0.000936,0.000000,-0.000001,0.000000,-0.000000,-0.004422,0.017960,-0.000859,-0.000298,-0.000000,0.000000,-0.000000,0.000000,1.176723,-0.024582,0.001244,0.000395,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.091141,-0.048706,0.005598,0.000926,-0.000008,0.000008,-0.000008,0.000006,0.012381,0.015349,-0.001751,-0.000295,0.000003,-0.000003,0.000002,-0.000002,1.153780,-0.020909,0.002428,0.000391,-0.000004,0.000003,-0.000003,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.048957,-0.034729,0.008419,0.001034,0.000133,-0.000060,0.000139,-0.000102,0.025684,0.010960,-0.002650,-0.000329,-0.000042,0.000019,-0.000044,0.000032,1.135689,-0.014878,0.003620,0.000438,0.000057,-0.000026,0.000059,-0.000044,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.023792,-0.014437,0.011660,0.000175,-0.001651,0.000006,-0.002053,0.001355,0.033630,0.004561,-0.003682,-0.000058,0.000522,-0.000002,0.000649,-0.000429,1.124915,-0.006176,0.004992,0.000070,-0.000706,0.000002,-0.000877,0.000579,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/15.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/15.csv
new file mode 100644
index 0000000000000000000000000000000000000000..a60be53a9a4ee0c7d06d804d3114fdb6580e1c32
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/15.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.018846,0.000000,0.000000,0.000000,0.022638,-0.024955,0.011613,-0.002132,0.035193,0.000000,0.000000,-0.000000,-0.025600,0.027900,-0.012920,0.002364,1.122800,0.000000,0.000000,0.000000,0.010375,-0.010930,0.004985,-0.000903,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.026011,0.020534,0.015706,-0.001352,-0.002558,0.002155,-0.000916,0.000163,0.026937,-0.023868,-0.018745,0.000958,0.002857,-0.002388,0.001010,-0.000180,1.126327,0.010437,0.008756,0.000285,-0.001116,0.000910,-0.000379,0.000067,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.059743,0.044083,0.007551,-0.002624,0.000205,-0.000136,0.000034,-0.000002,-0.013419,-0.054195,-0.011237,0.002409,-0.000229,0.000155,-0.000049,0.000006,1.145286,0.027084,0.007735,-0.000318,0.000090,-0.000065,0.000032,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.108855,0.051644,0.000022,-0.002546,-0.000028,-0.000046,0.000162,-0.000033,-0.076558,-0.069832,-0.004440,0.002276,0.000024,0.000024,-0.000097,0.000020,1.179837,0.041778,0.007005,-0.000212,-0.000000,0.000023,-0.000063,0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.158030,0.044449,-0.006509,-0.001039,0.001010,-0.000227,-0.000071,0.000034,-0.148583,-0.072111,0.001735,0.001369,-0.000613,0.000138,0.000043,-0.000021,1.228381,0.054981,0.005930,-0.000784,-0.000375,0.000084,0.000027,-0.000013,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.195676,0.031026,-0.006199,0.000484,-0.000014,-0.000043,0.000035,-0.000009,-0.218043,-0.066181,0.003762,0.000445,0.000009,0.000026,-0.000021,0.000005,1.288231,0.063482,0.002305,-0.001349,0.000005,0.000016,-0.000013,0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.220956,0.019957,-0.004919,0.000396,-0.000005,0.000009,-0.000005,0.000001,-0.279999,-0.057250,0.005200,0.000498,0.000003,-0.000006,0.000003,-0.000001,1.352681,0.064090,-0.001680,-0.001318,0.000001,-0.000002,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.236390,0.011308,-0.003727,0.000399,0.000001,-0.000001,-0.000000,0.000000,-0.331550,-0.045357,0.006688,0.000489,-0.000007,0.000007,-0.000006,0.000005,1.413773,0.056779,-0.005627,-0.001307,0.000010,-0.000010,0.000010,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.244371,0.005053,-0.002531,0.000396,-0.000002,-0.000002,0.000004,0.000000,-0.369732,-0.030511,0.008190,0.000577,0.000106,-0.000046,0.000107,-0.000081,1.463620,0.041599,-0.009602,-0.001441,-0.000165,0.000076,-0.000175,0.000128,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.247289,0.001183,-0.001320,0.000444,0.000048,0.000016,0.000006,-0.000026,-0.391389,-0.012126,0.010015,-0.000132,-0.001334,-0.000005,-0.001625,0.001086,1.494039,0.017633,-0.014106,-0.000383,0.002049,-0.000014,0.002568,-0.001687,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/16.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/16.csv
new file mode 100644
index 0000000000000000000000000000000000000000..6757671b38eeca1f12f43590322d4aa64b9e2a42
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/16.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.247640,0.000000,0.000000,0.000000,0.000432,-0.000471,0.000218,-0.000040,-0.395510,0.000000,0.000000,0.000000,0.005427,-0.005831,0.002683,-0.000489,1.500100,0.000000,0.000000,-0.000000,-0.004799,0.005161,-0.002376,0.000433,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.247779,0.000402,0.000315,-0.000017,-0.000048,0.000040,-0.000017,0.000003,-0.393719,0.005231,0.004233,-0.000053,-0.000596,0.000493,-0.000207,0.000037,1.498519,-0.004615,-0.003728,0.000055,0.000528,-0.000437,0.000184,-0.000033,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.248457,0.000910,0.000188,-0.000040,0.000004,-0.000003,0.000001,-0.000000,-0.384582,0.012633,0.003092,-0.000364,0.000048,-0.000033,0.000013,-0.000002,1.490473,-0.011105,-0.002693,0.000330,-0.000042,0.000029,-0.000011,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.249517,0.001174,0.000079,-0.000034,0.000001,0.000000,-0.000000,-0.000000,-0.369196,0.017810,0.002098,-0.000327,-0.000004,0.000002,-0.000001,0.000000,1.476983,-0.015578,-0.001793,0.000296,0.000003,-0.000002,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.250738,0.001237,-0.000013,-0.000027,0.000002,0.000000,0.000000,-0.000000,-0.349618,0.021014,0.001101,-0.000337,-0.000002,-0.000000,0.000000,0.000000,1.459910,-0.018269,-0.000897,0.000299,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.251938,0.001141,-0.000078,-0.000017,0.000002,-0.000000,0.000000,-0.000000,-0.327842,0.022197,0.000078,-0.000346,-0.000002,0.000000,-0.000000,0.000000,1.441042,-0.019166,0.000001,0.000299,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.252988,0.000945,-0.000114,-0.000008,0.000002,-0.000000,0.000000,-0.000000,-0.305916,0.021306,-0.000974,-0.000355,-0.000002,0.000001,-0.000000,0.000000,1.422176,-0.018267,0.000898,0.000299,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.253812,0.000699,-0.000130,-0.000003,0.000001,-0.000000,0.000000,-0.000000,-0.285941,0.018286,-0.002048,-0.000359,0.000002,-0.000003,0.000003,-0.000002,1.405106,-0.015574,0.001794,0.000296,-0.000003,0.000003,-0.000002,0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.254379,0.000434,-0.000134,-0.000001,-0.000001,0.000000,-0.000001,0.000001,-0.270062,0.013108,-0.003146,-0.000403,-0.000051,0.000023,-0.000053,0.000039,1.391621,-0.011099,0.002693,0.000330,0.000042,-0.000019,0.000044,-0.000033,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.254677,0.000160,-0.000137,0.000008,0.000016,0.000000,0.000019,-0.000013,-0.260546,0.005472,-0.004410,-0.000078,0.000628,-0.000003,0.000782,-0.000516,1.383580,-0.004612,0.003726,0.000055,-0.000527,0.000002,-0.000656,0.000433,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/17.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/17.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ec13e2e64a3685c222df3ca9be3d768bbfdf4786
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/17.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/18.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/18.csv
new file mode 100644
index 0000000000000000000000000000000000000000..4b1decf8b570ca75a1dc0e639ce88861e386da87
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/18.csv
@@ -0,0 +1,8 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/19.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/19.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ab7ba9830c9a439ab57c09b1a3ac2836a3066a5f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/19.csv
@@ -0,0 +1,20 @@
+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,
+1.000000,0.122340,0.000000,0.000000,0.000000,0.011833,-0.012613,0.005784,-0.001052,-0.410230,0.000000,0.000000,-0.000000,0.005073,-0.005395,0.002471,-0.000449,1.555600,0.000000,0.000000,0.000000,-0.016533,0.017519,-0.008012,0.001454,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.126292,0.011606,0.009535,0.000064,-0.001287,0.001061,-0.000443,0.000078,-0.408530,0.005002,0.004127,0.000051,-0.000550,0.000453,-0.000189,0.000033,1.550029,-0.016426,-0.013642,-0.000275,0.001786,-0.001466,0.000610,-0.000108,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.146906,0.028916,0.007616,-0.000591,0.000115,-0.000068,0.000024,-0.000004,-0.399602,0.012574,0.003376,-0.000231,0.000047,-0.000030,0.000011,-0.000002,1.520508,-0.041812,-0.011512,0.000655,-0.000151,0.000096,-0.000035,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.182915,0.042617,0.006141,-0.000458,0.000003,0.000003,-0.000001,0.000000,-0.383858,0.018724,0.002792,-0.000186,-0.000001,0.000001,-0.000001,0.000000,1.467755,-0.063166,-0.009901,0.000509,0.000005,-0.000004,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.231220,0.053545,0.004800,-0.000433,0.000008,0.000000,-0.000001,0.000000,-0.362528,0.023749,0.002234,-0.000185,0.000002,0.000000,-0.000000,0.000000,1.395199,-0.081436,-0.008369,0.000509,-0.000002,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.289141,0.061879,0.003548,-0.000404,0.000006,0.000000,-0.000000,0.000000,-0.336728,0.027670,0.001691,-0.000177,0.000002,0.000000,-0.000000,0.000000,1.305902,-0.096652,-0.006849,0.000505,-0.000001,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.354170,0.067788,0.002374,-0.000379,0.000006,0.000001,-0.000001,0.000000,-0.307541,0.030531,0.001175,-0.000166,0.000003,0.000001,-0.000001,0.000000,1.202905,-0.108838,-0.005337,0.000504,-0.000000,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.423960,0.071424,0.001275,-0.000353,0.000007,0.000000,-0.000000,0.000000,-0.275998,0.032396,0.000697,-0.000153,0.000004,0.000000,-0.000000,0.000000,1.089233,-0.118002,-0.003827,0.000504,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.496312,0.072942,0.000256,-0.000326,0.000007,0.000001,-0.000001,0.000000,-0.243054,0.033346,0.000261,-0.000138,0.000004,0.000000,-0.000000,-0.000000,0.967907,-0.124144,-0.002315,0.000505,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.569191,0.072506,-0.000677,-0.000296,0.000008,0.000001,-0.000001,0.000000,-0.209581,0.033468,-0.000132,-0.000124,0.000003,-0.000000,0.000000,-0.000000,0.841954,-0.127257,-0.000797,0.000507,0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.640732,0.070295,-0.001519,-0.000265,0.000008,0.000001,-0.000001,0.000000,-0.176366,0.032844,-0.000489,-0.000115,0.000001,-0.000000,0.000000,-0.000000,0.714408,-0.127327,0.000728,0.000510,0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.709251,0.066495,-0.002264,-0.000232,0.000008,0.000000,-0.000001,0.000000,-0.144125,0.031526,-0.000827,-0.000111,0.000001,-0.000000,0.000000,-0.000000,0.588319,-0.124339,0.002261,0.000512,0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.773260,0.061305,-0.002912,-0.000203,0.000006,-0.000000,-0.000000,0.000000,-0.113535,0.029542,-0.001156,-0.000109,0.000000,-0.000000,0.000000,-0.000000,0.466754,-0.118278,0.003801,0.000514,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.831456,0.054896,-0.003487,-0.000183,0.000004,-0.000000,-0.000000,0.000000,-0.085258,0.026905,-0.001481,-0.000108,0.000000,-0.000000,0.000000,-0.000000,0.352792,-0.109131,0.005346,0.000516,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.882685,0.047388,-0.004013,-0.000169,0.000003,-0.000000,-0.000000,0.000000,-0.059941,0.023620,-0.001805,-0.000108,-0.000000,-0.000000,-0.000000,-0.000000,0.249523,-0.096891,0.006894,0.000516,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.925893,0.038862,-0.004509,-0.000162,0.000001,0.000000,-0.000000,0.000000,-0.038234,0.019686,-0.002129,-0.000108,-0.000000,0.000000,-0.000000,0.000000,0.160042,-0.081554,0.008444,0.000517,0.000000,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.960086,0.029366,-0.004984,-0.000153,0.000005,-0.000003,0.000003,-0.000003,-0.020785,0.015104,-0.002452,-0.000106,0.000002,-0.000002,0.000002,-0.000001,0.087448,-0.063117,0.009989,0.000508,-0.000009,0.000008,-0.000007,0.000006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.984317,0.018941,-0.005459,-0.000202,-0.000061,0.000025,-0.000060,0.000046,-0.008239,0.009881,-0.002781,-0.000133,-0.000033,0.000014,-0.000032,0.000025,0.034826,-0.041612,0.011559,0.000624,0.000139,-0.000059,0.000139,-0.000106,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.997548,0.007264,-0.006109,0.000217,0.000773,0.000008,0.000925,-0.000625,-0.001298,0.003837,-0.003206,0.000089,0.000413,0.000003,0.000498,-0.000335,0.005511,-0.016265,0.013547,-0.000320,-0.001762,-0.000012,-0.002130,0.001431,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/2.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..95e8707495a5d4c1c4b4b79b691f934f2d0a3b9e
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/2.csv
@@ -0,0 +1,17 @@
+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,
+1.000000,1.196700,0.000000,0.000000,0.000000,-0.003332,0.003536,-0.001618,0.000294,0.067056,0.000000,0.000000,0.000000,-0.003547,0.003765,-0.001723,0.000313,1.130300,0.000000,0.000000,0.000000,0.009439,-0.010018,0.004585,-0.000833,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.195580,-0.003299,-0.002732,-0.000045,0.000361,-0.000296,0.000123,-0.000022,0.065864,-0.003512,-0.002908,-0.000048,0.000385,-0.000315,0.000131,-0.000023,1.133473,0.009346,0.007739,0.000127,-0.001023,0.000838,-0.000350,0.000062,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.189671,-0.008345,-0.002265,0.000147,-0.000029,0.000020,-0.000008,0.000001,0.059574,-0.008882,-0.002410,0.000157,-0.000031,0.000021,-0.000008,0.000001,1.150212,0.023637,0.006414,-0.000418,0.000081,-0.000056,0.000021,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.179193,-0.012485,-0.001884,0.000124,0.000002,-0.000001,0.000000,-0.000000,0.048422,-0.013288,-0.002005,0.000132,0.000002,-0.000001,0.000000,-0.000000,1.179889,0.035362,0.005335,-0.000351,-0.000005,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.164949,-0.015879,-0.001509,0.000126,-0.000000,0.000000,-0.000000,0.000000,0.033262,-0.016898,-0.001605,0.000134,-0.000000,0.000000,-0.000000,0.000000,1.220233,0.044970,0.004270,-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,
+1.000000,1.147687,-0.018519,-0.001132,0.000126,0.000000,0.000000,-0.000000,0.000000,0.014892,-0.019706,-0.001203,0.000134,0.000000,0.000000,-0.000000,0.000000,1.269117,0.052443,0.003203,-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,
+1.000000,1.128161,-0.020407,-0.000755,0.000126,0.000000,0.000000,0.000000,-0.000000,-0.005883,-0.021712,-0.000802,0.000134,-0.000000,-0.000000,0.000000,-0.000000,1.324407,0.057780,0.002135,-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,
+1.000000,1.107124,-0.021541,-0.000379,0.000126,0.000000,0.000000,0.000000,0.000000,-0.028263,-0.022915,-0.000401,0.000134,-0.000000,0.000000,0.000000,-0.000000,1.383966,0.060983,0.001067,-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,
+1.000000,1.085330,-0.021922,-0.000002,0.000126,0.000000,0.000000,0.000000,0.000000,-0.051445,-0.023315,0.000000,0.000134,-0.000000,-0.000000,0.000000,-0.000000,1.445660,0.062050,-0.000000,-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,
+1.000000,1.063532,-0.021549,0.000375,0.000126,0.000000,0.000000,-0.000000,-0.000000,-0.074626,-0.022914,0.000401,0.000134,-0.000000,-0.000000,0.000000,-0.000000,1.507354,0.060981,-0.001068,-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,
+1.000000,1.042484,-0.020422,0.000752,0.000126,0.000000,0.000000,0.000000,0.000000,-0.097005,-0.021710,0.000803,0.000134,-0.000000,-0.000000,-0.000000,0.000000,1.566911,0.057778,-0.002135,-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,
+1.000000,1.022941,-0.018540,0.001130,0.000126,0.000000,0.000000,-0.000000,0.000000,-0.117778,-0.019704,0.001204,0.000134,-0.000000,0.000000,-0.000000,0.000000,1.622198,0.052440,-0.003203,-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,
+1.000000,1.005656,-0.015903,0.001508,0.000126,0.000000,-0.000000,0.000000,-0.000000,-0.136144,-0.016895,0.001605,0.000134,0.000000,-0.000000,0.000000,-0.000000,1.671079,0.044966,-0.004271,-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,
+1.000000,0.991387,-0.012509,0.001885,0.000124,-0.000002,0.000002,-0.000002,0.000001,-0.151301,-0.013285,0.002005,0.000132,-0.000002,0.000002,-0.000002,0.000001,1.711418,0.035358,-0.005335,-0.000350,0.000005,-0.000005,0.000004,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.980888,-0.008365,0.002268,0.000148,0.000029,-0.000012,0.000029,-0.000022,-0.162450,-0.008880,0.002410,0.000157,0.000031,-0.000013,0.000031,-0.000023,1.741092,0.023634,-0.006414,-0.000418,-0.000081,0.000035,-0.000082,0.000062,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.974963,-0.003308,0.002739,-0.000044,-0.000362,-0.000002,-0.000440,0.000295,-0.168738,-0.003511,0.002907,-0.000048,-0.000384,-0.000002,-0.000467,0.000313,1.757828,0.009344,-0.007738,0.000127,0.001023,0.000005,0.001243,-0.000832,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/3.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..4b0c2a465fc29a84c98fbd02eea0ecbb8a6d995b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/3.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/4.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..468ee4fa7236ae20c17009d3e83644415d1b7131
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/4.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.705950,0.000000,0.000000,0.000000,-0.029395,0.031611,-0.014552,0.002653,-0.082836,0.000000,0.000000,-0.000000,0.004202,-0.004519,0.002080,-0.000379,1.501700,0.000000,0.000000,-0.000000,0.033967,-0.036528,0.016816,-0.003066,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.696266,-0.028268,-0.022831,0.000341,0.003233,-0.002676,0.001125,-0.000200,-0.081452,0.004041,0.003264,-0.000049,-0.000462,0.000383,-0.000161,0.000029,1.512890,0.032665,0.026382,-0.000394,-0.003736,0.003093,-0.001300,0.000231,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.646990,-0.068005,-0.016490,0.002024,-0.000259,0.000180,-0.000070,0.000012,-0.074407,0.009722,0.002357,-0.000289,0.000037,-0.000026,0.000010,-0.000002,1.569831,0.078582,0.019054,-0.002339,0.000300,-0.000208,0.000081,-0.000014,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.564382,-0.095385,-0.010969,0.001812,0.000017,-0.000010,0.000003,-0.000001,-0.062598,0.013636,0.001568,-0.000259,-0.000002,0.000001,-0.000000,0.000000,1.665286,0.110220,0.012675,-0.002094,-0.000019,0.000011,-0.000004,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.459850,-0.111851,-0.005489,0.001831,-0.000001,0.000000,-0.000000,0.000000,-0.047654,0.015990,0.000785,-0.000262,0.000000,-0.000000,0.000000,-0.000000,1.786076,0.129247,0.006343,-0.002115,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.344341,-0.117339,-0.000000,0.001829,0.000000,0.000000,-0.000000,0.000000,-0.031140,0.016775,0.000000,-0.000262,-0.000000,-0.000000,0.000000,-0.000000,1.919551,0.135589,-0.000000,-0.002114,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.228830,-0.111851,0.005489,0.001831,0.000001,-0.000001,0.000001,-0.000001,-0.014626,0.015991,-0.000785,-0.000262,-0.000000,0.000000,-0.000000,0.000000,2.053025,0.129246,-0.006343,-0.002115,-0.000001,0.000002,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.124298,-0.095386,0.010969,0.001812,-0.000017,0.000015,-0.000015,0.000012,0.000319,0.013637,-0.001568,-0.000259,0.000002,-0.000002,0.000002,-0.000002,2.173814,0.110220,-0.012675,-0.002094,0.000019,-0.000018,0.000017,-0.000014,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.041689,-0.068006,0.016490,0.002024,0.000259,-0.000117,0.000272,-0.000200,0.012130,0.009723,-0.002358,-0.000289,-0.000037,0.000017,-0.000039,0.000029,2.269270,0.078581,-0.019054,-0.002339,-0.000300,0.000136,-0.000314,0.000231,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.007588,-0.028269,0.022832,0.000341,-0.003233,0.000012,-0.004019,0.002653,0.019175,0.004042,-0.003264,-0.000049,0.000462,-0.000002,0.000575,-0.000379,2.326210,0.032665,-0.026382,-0.000394,0.003736,-0.000014,0.004644,-0.003066,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/5.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/5.csv
new file mode 100644
index 0000000000000000000000000000000000000000..306dd19bbf4339f7b5780e8475e51c0beb0bf78b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/5.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/6.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/6.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ff745e97939839b7a22a14297c1c7eb7913ba573
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/6.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.052516,0.000000,0.000000,0.000000,-0.001278,0.001374,-0.000633,0.000115,-0.354410,0.000000,0.000000,0.000000,0.019321,-0.020777,0.009565,-0.001744,1.779000,0.000000,0.000000,-0.000000,-0.025253,0.027156,-0.012501,0.002279,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.052937,-0.001229,-0.000993,0.000015,0.000141,-0.000116,0.000049,-0.000009,-0.348045,0.018580,0.015006,-0.000224,-0.002125,0.001759,-0.000739,0.000131,1.770681,-0.024285,-0.019614,0.000293,0.002778,-0.002299,0.000966,-0.000171,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.055080,-0.002959,-0.000718,0.000088,-0.000011,0.000008,-0.000003,0.000001,-0.315657,0.044698,0.010838,-0.001330,0.000171,-0.000118,0.000046,-0.000008,1.728349,-0.058421,-0.014166,0.001739,-0.000223,0.000155,-0.000060,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.058675,-0.004152,-0.000479,0.000079,0.000001,-0.000000,0.000000,-0.000000,-0.261361,0.062695,0.007210,-0.001191,-0.000011,0.000006,-0.000002,0.000000,1.657383,-0.081943,-0.009423,0.001557,0.000014,-0.000008,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.063227,-0.004872,-0.000241,0.000080,0.000000,0.000000,0.000000,-0.000000,-0.192654,0.073518,0.003608,-0.001203,0.000001,-0.000000,0.000000,-0.000000,1.567583,-0.096088,-0.004716,0.001573,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.068260,-0.005115,-0.000002,0.000080,0.000000,-0.000000,0.000000,-0.000000,-0.116731,0.077127,0.000001,-0.001202,-0.000000,-0.000000,0.000000,-0.000000,1.468351,-0.100803,-0.000000,0.001571,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.073296,-0.004879,0.000238,0.000080,0.000000,-0.000000,0.000000,-0.000000,-0.040806,0.073520,-0.003607,-0.001203,-0.000001,0.000001,-0.000000,0.000000,1.369120,-0.096089,0.004715,0.001573,0.000001,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.077857,-0.004162,0.000478,0.000079,-0.000001,0.000001,-0.000001,0.000001,0.027904,0.062698,-0.007210,-0.001191,0.000011,-0.000010,0.000010,-0.000008,1.279319,-0.081943,0.009423,0.001557,-0.000014,0.000013,-0.000013,0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.081463,-0.002969,0.000719,0.000089,0.000011,-0.000005,0.000012,-0.000009,0.082204,0.044701,-0.010839,-0.001330,-0.000171,0.000077,-0.000179,0.000131,1.208352,-0.058422,0.014166,0.001739,0.000223,-0.000101,0.000234,-0.000171,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.083614,-0.001234,0.000997,0.000015,-0.000141,0.000001,-0.000176,0.000116,0.114595,0.018582,-0.015008,-0.000224,0.002125,-0.000008,0.002642,-0.001744,1.166019,-0.024285,0.019614,0.000293,-0.002778,0.000010,-0.003453,0.002279,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/7.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/7.csv
new file mode 100644
index 0000000000000000000000000000000000000000..5f6f8a72634a099e498ae652c0ced3b8b6d88286
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/7.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-0.084037,0.000000,0.000000,0.000000,0.010617,-0.011381,0.005232,-0.000953,0.120960,0.000000,0.000000,0.000000,-0.024369,0.026122,-0.012008,0.002187,1.157700,0.000000,0.000000,0.000000,0.010878,-0.011660,0.005360,-0.000976,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.080522,0.010282,0.008356,-0.000059,-0.001164,0.000961,-0.000403,0.000071,0.112892,-0.023605,-0.019186,0.000133,0.002671,-0.002205,0.000925,-0.000164,1.161302,0.010538,0.008566,-0.000059,-0.001192,0.000984,-0.000413,0.000073,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.062478,0.025048,0.006258,-0.000668,0.000093,-0.000064,0.000025,-0.000004,0.071461,-0.057517,-0.014379,0.001531,-0.000214,0.000148,-0.000057,0.000010,1.179799,0.025681,0.006422,-0.000683,0.000095,-0.000066,0.000026,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.031790,0.035731,0.004453,-0.000591,-0.000006,0.000003,-0.000001,0.000000,0.000982,-0.082074,-0.010243,0.001355,0.000013,-0.000008,0.000003,-0.000000,1.211271,0.036653,0.004578,-0.000604,-0.000006,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.007799,0.042853,0.002667,-0.000596,0.000001,-0.000000,0.000000,0.000000,-0.089973,-0.098468,-0.006144,0.001369,-0.000001,0.000000,-0.000000,0.000000,1.251893,0.043982,0.002749,-0.000611,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.052723,0.046398,0.000880,-0.000595,0.000000,0.000000,-0.000000,0.000000,-0.193217,-0.106652,-0.002041,0.001367,-0.000000,-0.000000,0.000000,-0.000000,1.298014,0.047648,0.000917,-0.000611,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.099406,0.046374,-0.000903,-0.000594,0.000000,-0.000000,0.000000,-0.000000,-0.300543,-0.106634,0.002059,0.001366,-0.000000,0.000000,-0.000000,0.000000,1.345968,0.047649,-0.000915,-0.000611,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.144283,0.042786,-0.002684,-0.000594,-0.000000,0.000001,-0.000000,0.000000,-0.403752,-0.098418,0.006157,0.001367,0.000001,-0.000001,0.000001,-0.000000,1.392091,0.043986,-0.002748,-0.000611,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.183792,0.035639,-0.004460,-0.000586,0.000006,-0.000005,0.000005,-0.000004,-0.494647,-0.082008,0.010247,0.001351,-0.000014,0.000013,-0.000012,0.000010,1.432718,0.036658,-0.004577,-0.000605,0.000006,-0.000006,0.000005,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.214387,0.024959,-0.006248,-0.000661,-0.000093,0.000041,-0.000096,0.000071,-0.565060,-0.057455,0.014371,0.001526,0.000213,-0.000095,0.000222,-0.000164,1.464195,0.025687,-0.006423,-0.000683,-0.000095,0.000043,-0.000099,0.000073,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.232361,0.010238,-0.008323,-0.000055,0.001158,-0.000002,0.001431,-0.000948,-0.606442,-0.023575,0.019163,0.000130,-0.002667,0.000004,-0.003297,0.002184,1.482697,0.010541,-0.008568,-0.000059,0.001193,-0.000002,0.001475,-0.000977,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/8.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/8.csv
new file mode 100644
index 0000000000000000000000000000000000000000..26fde602cd730e274f704a0b698e6f51fc9d56c3
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/8.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/6/9.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/6/9.csv
new file mode 100644
index 0000000000000000000000000000000000000000..6e1adf202a9da44c2a126ec33ee3416607b03589
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/6/9.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/1.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..6f6ec3fc12b02b2658bf6a7b397036b0c45d83e7
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/1.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,1.500000,0.000000,0.000000,0.000000,0.025888,-0.027753,0.012759,-0.002324,0.000000,0.000000,0.000000,0.000000,-0.007078,0.007587,-0.003488,0.000635,1.500000,0.000000,0.000000,0.000000,-0.026526,0.028433,-0.013070,0.002381,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.508569,0.025068,0.020370,-0.000147,-0.002837,0.002344,-0.000983,0.000174,-0.002344,-0.006857,-0.005573,0.000038,0.000776,-0.000641,0.000269,-0.000048,1.491217,-0.025696,-0.020887,0.000143,0.002907,-0.002400,0.001007,-0.000178,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.552557,0.061054,0.015249,-0.001629,0.000228,-0.000157,0.000061,-0.000011,-0.014379,-0.016709,-0.004178,0.000444,-0.000062,0.000043,-0.000017,0.000003,1.446113,-0.062621,-0.015661,0.001664,-0.000233,0.000161,-0.000062,0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.627352,0.087084,0.010853,-0.001438,-0.000013,0.000008,-0.000003,0.000001,-0.034856,-0.023847,-0.002978,0.000393,0.000004,-0.000002,0.000001,-0.000000,1.369372,-0.089374,-0.011163,0.001474,0.000015,-0.000008,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.723842,0.104448,0.006506,-0.001451,0.000001,-0.000000,0.000000,-0.000000,-0.061284,-0.028614,-0.001788,0.000397,-0.000000,0.000000,-0.000000,0.000000,1.270318,-0.107246,-0.006702,0.001490,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.833346,0.113110,0.002158,-0.001448,0.000000,-0.000000,-0.000000,0.000000,-0.091289,-0.030998,-0.000596,0.000397,-0.000000,0.000000,0.000000,-0.000000,1.157860,-0.116181,-0.002234,0.001489,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.947166,0.113082,-0.002186,-0.001448,0.000000,-0.000000,0.000000,-0.000000,-0.122487,-0.031000,0.000595,0.000397,0.000000,0.000000,-0.000000,0.000000,1.040935,-0.116180,0.002234,0.001489,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,2.056614,0.104366,-0.006530,-0.001449,-0.000001,0.000001,-0.000001,0.000000,-0.152495,-0.028619,0.001787,0.000398,0.000000,-0.000000,0.000000,-0.000000,0.928478,-0.107245,0.006702,0.001490,0.000001,-0.000001,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,2.153001,0.086963,-0.010866,-0.001433,0.000014,-0.000013,0.000013,-0.000011,-0.178930,-0.023854,0.002977,0.000394,-0.000004,0.000004,-0.000004,0.000003,0.829426,-0.089373,0.011163,0.001474,-0.000015,0.000014,-0.000013,0.000011,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,2.227669,0.060928,-0.015238,-0.001618,-0.000227,0.000101,-0.000235,0.000174,-0.199414,-0.016716,0.004179,0.000445,0.000062,-0.000028,0.000065,-0.000048,0.752686,-0.062620,0.015660,0.001664,0.000233,-0.000104,0.000242,-0.000178,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,2.271555,0.025002,-0.020322,-0.000140,0.002828,-0.000005,0.003498,-0.002317,-0.211455,-0.006861,0.005576,0.000039,-0.000776,0.000001,-0.000960,0.000636,0.707582,-0.025695,0.020886,0.000143,-0.002907,0.000005,-0.003594,0.002381,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/10.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/10.csv
new file mode 100644
index 0000000000000000000000000000000000000000..7086168ac03f126861e39fcaedd48b23c8bc2c1a
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/10.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/11.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/11.csv
new file mode 100644
index 0000000000000000000000000000000000000000..6459963c08849080fd67428dc1cdc901f1292487
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/11.csv
@@ -0,0 +1,13 @@
+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,
+1.000000,-0.557510,0.000000,0.000000,-0.000000,0.014632,-0.015642,0.007182,-0.001307,-1.005700,0.000000,0.000000,0.000000,0.004810,-0.005144,0.002362,-0.000430,0.979100,0.000000,0.000000,0.000000,0.018919,-0.020225,0.009286,-0.001690,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.552645,0.014259,0.011650,-0.000004,-0.001599,0.001317,-0.000552,0.000098,-1.004102,0.004683,0.003823,-0.000005,-0.000526,0.000434,-0.000182,0.000032,0.985390,0.018437,0.015064,-0.000005,-0.002067,0.001704,-0.000714,0.000126,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.527476,0.035112,0.008992,-0.000845,0.000128,-0.000088,0.000034,-0.000006,-0.995844,0.011512,0.002939,-0.000279,0.000043,-0.000029,0.000011,-0.000002,1.017935,0.045401,0.011628,-0.001092,0.000165,-0.000114,0.000044,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.484149,0.050796,0.006731,-0.000740,-0.000008,0.000005,-0.000002,0.000000,-0.981648,0.016633,0.002194,-0.000243,-0.000002,0.000001,-0.000001,0.000000,1.073959,0.065681,0.008703,-0.000957,-0.000010,0.000006,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.427366,0.062021,0.004490,-0.000749,0.000000,-0.000000,0.000000,-0.000000,-0.963066,0.020287,0.001459,-0.000245,0.000000,-0.000000,-0.000000,0.000000,1.147380,0.080195,0.005806,-0.000968,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.361604,0.068755,0.002245,-0.000748,-0.000000,-0.000000,0.000000,-0.000000,-0.941564,0.022472,0.000726,-0.000244,0.000000,-0.000000,0.000000,-0.000000,1.232412,0.088903,0.002903,-0.000968,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.291352,0.071000,0.000000,-0.000748,-0.000000,0.000000,-0.000000,-0.000000,-0.918610,0.023192,-0.000006,-0.000244,0.000000,-0.000000,0.000000,-0.000000,1.323250,0.091805,0.000000,-0.000968,-0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.221100,0.068755,-0.002245,-0.000748,0.000000,-0.000000,0.000000,-0.000000,-0.895667,0.022450,-0.000737,-0.000244,0.000000,-0.000000,0.000000,-0.000000,1.414088,0.088903,-0.002903,-0.000968,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.155338,0.062021,-0.004490,-0.000749,-0.000000,0.000001,-0.000000,0.000000,-0.874198,0.020244,-0.001468,-0.000244,-0.000000,0.000000,-0.000000,0.000000,1.499120,0.080195,-0.005806,-0.000968,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.098556,0.050796,-0.006731,-0.000740,0.000008,-0.000007,0.000007,-0.000006,-0.855666,0.016576,-0.002198,-0.000241,0.000003,-0.000002,0.000002,-0.000002,1.572541,0.065681,-0.008703,-0.000957,0.000010,-0.000010,0.000009,-0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.055228,0.035112,-0.008992,-0.000845,-0.000128,0.000057,-0.000132,0.000098,-0.841528,0.011456,-0.002935,-0.000275,-0.000042,0.000018,-0.000043,0.000032,1.628565,0.045401,-0.011628,-0.001092,-0.000165,0.000073,-0.000170,0.000126,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.030059,0.014259,-0.011650,-0.000004,0.001599,0.000000,0.001968,-0.001307,-0.833317,0.004652,-0.003801,-0.000001,0.000522,0.000000,0.000642,-0.000426,1.661110,0.018437,-0.015064,-0.000005,0.002067,0.000000,0.002544,-0.001690,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/12.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/12.csv
new file mode 100644
index 0000000000000000000000000000000000000000..30caf19b7412a523592a556ab86a16680cb97ef5
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/12.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.025194,0.000000,0.000000,0.000000,-0.003824,0.004112,-0.001893,0.000345,-0.831730,0.000000,0.000000,0.000000,0.011425,-0.012291,0.005660,-0.001032,1.667400,0.000000,0.000000,0.000000,-0.020229,0.021754,-0.010015,0.001826,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.026454,-0.003677,-0.002970,0.000044,0.000421,-0.000348,0.000146,-0.000026,-0.827969,0.010975,0.008856,-0.000142,-0.001257,0.001041,-0.000438,0.000078,1.660736,-0.019454,-0.015712,0.000235,0.002225,-0.001842,0.000774,-0.000137,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.032863,-0.008845,-0.002145,0.000263,-0.000034,0.000023,-0.000009,0.000002,-0.808856,0.026358,0.006369,-0.000791,0.000103,-0.000070,0.000027,-0.000005,1.626825,-0.046799,-0.011348,0.001393,-0.000179,0.000124,-0.000048,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.043608,-0.012407,-0.001427,0.000236,0.000002,-0.000001,0.000000,-0.000000,-0.776863,0.036916,0.004221,-0.000703,-0.000006,0.000004,-0.000001,0.000000,1.569977,-0.065642,-0.007548,0.001247,0.000011,-0.000007,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.057204,-0.014548,-0.000714,0.000238,-0.000000,0.000000,-0.000000,0.000000,-0.736432,0.043240,0.002102,-0.000707,0.000001,-0.000000,0.000000,-0.000000,1.498041,-0.076973,-0.003777,0.001260,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.072228,-0.015262,0.000000,0.000238,-0.000000,0.000000,-0.000000,0.000000,-0.691796,0.045326,-0.000016,-0.000705,0.000000,-0.000000,0.000000,-0.000000,1.418550,-0.080750,0.000000,0.001259,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.087252,-0.014548,0.000714,0.000238,0.000000,-0.000000,0.000000,-0.000000,-0.647191,0.043180,-0.002130,-0.000705,-0.000000,0.000001,-0.000000,0.000000,1.339059,-0.076973,0.003777,0.001260,0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.100848,-0.012406,0.001427,0.000236,-0.000002,0.000002,-0.000002,0.000002,-0.606845,0.036808,-0.004239,-0.000698,0.000006,-0.000006,0.000006,-0.000005,1.267123,-0.065641,0.007548,0.001247,-0.000011,0.000011,-0.000010,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.111592,-0.008845,0.002145,0.000263,0.000034,-0.000015,0.000035,-0.000026,-0.574972,0.026234,-0.006365,-0.000779,-0.000100,0.000045,-0.000105,0.000077,1.210275,-0.046799,0.011348,0.001393,0.000179,-0.000081,0.000187,-0.000137,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.118001,-0.003677,0.002969,0.000044,-0.000421,0.000002,-0.000523,0.000345,-0.555965,0.010903,-0.008807,-0.000130,0.001247,-0.000005,0.001550,-0.001023,1.176364,-0.019454,0.015712,0.000235,-0.002225,0.000008,-0.002766,0.001826,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/13.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/13.csv
new file mode 100644
index 0000000000000000000000000000000000000000..e645750ea646267bc0a6fcd863e54372c35c2956
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/13.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.119260,0.000000,0.000000,0.000000,0.011524,-0.012392,0.005705,-0.001040,-0.552230,0.000000,0.000000,0.000000,-0.007812,0.008399,-0.003866,0.000705,1.169700,0.000000,0.000000,0.000000,0.008385,-0.009017,0.004151,-0.000757,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.115464,0.011082,0.008951,-0.000134,-0.001268,0.001049,-0.000441,0.000078,-0.554804,-0.007516,-0.006073,0.000087,0.000859,-0.000711,0.000299,-0.000053,1.172462,0.008064,0.006513,-0.000097,-0.000922,0.000763,-0.000321,0.000057,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.096146,0.026660,0.006465,-0.000793,0.000102,-0.000071,0.000027,-0.000005,-0.567911,-0.018096,-0.004397,0.000535,-0.000069,0.000048,-0.000019,0.000003,1.186518,0.019398,0.004704,-0.000577,0.000074,-0.000051,0.000020,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.063761,0.037394,0.004300,-0.000711,-0.000007,0.000004,-0.000001,0.000000,-0.589906,-0.025411,-0.002939,0.000479,0.000005,-0.000003,0.000001,-0.000000,1.210082,0.027209,0.003129,-0.000517,-0.000005,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.022782,0.043849,0.002152,-0.000718,0.000000,-0.000000,0.000000,-0.000000,-0.617775,-0.029844,-0.001491,0.000484,0.000000,0.000000,-0.000000,0.000000,1.239900,0.031906,0.001566,-0.000522,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.022502,0.046001,0.000000,-0.000717,-0.000000,-0.000000,0.000000,-0.000000,-0.648626,-0.031373,-0.000037,0.000485,0.000001,0.000000,0.000000,-0.000000,1.272850,0.033471,0.000000,-0.000522,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.067787,0.043850,-0.002152,-0.000718,-0.000000,0.000001,-0.000000,0.000000,-0.679550,-0.029989,0.001424,0.000489,0.000002,-0.000000,0.000000,-0.000000,1.305799,0.031906,-0.001566,-0.000522,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.108768,0.037396,-0.004300,-0.000711,0.000007,-0.000006,0.000006,-0.000005,-0.707625,-0.025668,0.002899,0.000492,-0.000002,0.000004,-0.000004,0.000003,1.335617,0.027209,-0.003129,-0.000517,0.000005,-0.000004,0.000004,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.141154,0.026661,-0.006465,-0.000793,-0.000102,0.000046,-0.000107,0.000078,-0.729900,-0.018382,0.004413,0.000560,0.000074,-0.000032,0.000074,-0.000055,1.359181,0.019399,-0.004704,-0.000577,-0.000074,0.000033,-0.000078,0.000057,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.160473,0.011083,-0.008951,-0.000134,0.001268,-0.000005,0.001576,-0.001040,-0.743248,-0.007677,0.006185,0.000112,-0.000881,0.000003,-0.001099,0.000724,1.373238,0.008064,-0.006513,-0.000097,0.000922,-0.000003,0.001147,-0.000757,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/14.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/14.csv
new file mode 100644
index 0000000000000000000000000000000000000000..252c464746dd458874ecf0612f1f08f931b610fa
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/14.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.164270,0.000000,0.000000,0.000000,-0.033552,0.036082,-0.016611,0.003028,-0.745880,0.000000,0.000000,0.000000,0.035189,-0.037840,0.017419,-0.003176,1.376000,0.000000,0.000000,0.000000,-0.003099,0.003333,-0.001534,0.000280,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.153218,-0.032264,-0.026057,0.000391,0.003691,-0.003055,0.001284,-0.000228,-0.734287,0.033844,0.027337,-0.000405,-0.003870,0.003203,-0.001346,0.000239,1.374979,-0.002980,-0.002407,0.000036,0.000341,-0.000282,0.000119,-0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.096979,-0.077609,-0.018814,0.002312,-0.000296,0.000206,-0.000080,0.000014,-0.675285,0.081434,0.019756,-0.002419,0.000311,-0.000216,0.000084,-0.000015,1.369785,-0.007168,-0.001738,0.000213,-0.000027,0.000019,-0.000007,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.002711,-0.108841,-0.012508,0.002070,0.000019,-0.000011,0.000004,-0.000001,-0.576350,0.114253,0.013157,-0.002167,-0.000020,0.000011,-0.000004,0.000001,1.361078,-0.010053,-0.001155,0.000191,0.000002,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.116557,-0.127608,-0.006251,0.002090,-0.000001,0.000000,-0.000000,0.000000,-0.451119,0.134023,0.006604,-0.002190,0.000001,-0.000000,0.000000,-0.000000,1.350062,-0.011787,-0.000578,0.000193,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.248328,-0.133845,0.000013,0.002087,-0.000000,0.000000,0.000000,0.000000,-0.312681,0.140663,0.000035,-0.002191,-0.000001,-0.000000,-0.000000,0.000000,1.337890,-0.012363,0.000001,0.000193,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.380074,-0.127561,0.006271,0.002087,0.000001,-0.000002,0.000001,-0.000001,-0.174175,0.134156,-0.006545,-0.002198,-0.000003,0.000002,-0.000001,0.000001,1.325720,-0.011783,0.000579,0.000193,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.499278,-0.108764,0.012516,0.002065,-0.000019,0.000018,-0.000017,0.000014,-0.048763,0.114472,-0.013133,-0.002181,0.000019,-0.000018,0.000018,-0.000015,1.314709,-0.010047,0.001156,0.000191,-0.000002,0.000002,-0.000002,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.593465,-0.077531,0.018805,0.002305,0.000296,-0.000134,0.000310,-0.000227,0.050400,0.081654,-0.019781,-0.002437,-0.000312,0.000141,-0.000327,0.000240,1.306009,-0.007162,0.001737,0.000213,0.000027,-0.000012,0.000029,-0.000021,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.649642,-0.032224,0.026028,0.000387,-0.003685,0.000013,-0.004581,0.003024,0.109577,0.033956,-0.027419,-0.000416,0.003885,-0.000015,0.004830,-0.003188,1.300820,-0.002977,0.002404,0.000036,-0.000340,0.000001,-0.000423,0.000279,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/15.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/15.csv
new file mode 100644
index 0000000000000000000000000000000000000000..3647529d32e1e3259e8b51142eba4ba8598e6b38
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/15.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.660680,0.000000,0.000000,0.000000,-0.005595,0.006021,-0.002773,0.000506,0.121210,0.000000,0.000000,0.000000,-0.010053,0.010814,-0.004979,0.000908,1.299800,0.000000,0.000000,0.000000,0.007091,-0.007626,0.003511,-0.000640,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.662521,-0.005371,-0.004331,0.000073,0.000616,-0.000510,0.000214,-0.000038,0.117900,-0.009661,-0.007798,0.000122,0.001106,-0.000916,0.000385,-0.000068,1.302136,0.006819,0.005507,-0.000083,-0.000780,0.000646,-0.000271,0.000048,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.671868,-0.012880,-0.003100,0.000393,-0.000050,0.000034,-0.000013,0.000002,0.101070,-0.023214,-0.005613,0.000697,-0.000089,0.000062,-0.000024,0.000004,1.314021,0.016402,0.003976,-0.000489,0.000063,-0.000043,0.000017,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.687481,-0.017991,-0.002027,0.000352,0.000003,-0.000002,0.000001,-0.000000,0.072893,-0.032510,-0.003711,0.000623,0.000005,-0.000003,0.000001,-0.000000,1.333944,0.023002,0.002643,-0.000438,-0.000004,0.000002,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.707147,-0.020988,-0.000970,0.000350,-0.000003,-0.000001,0.000000,-0.000000,0.037297,-0.038055,-0.001833,0.000625,-0.000002,-0.000000,0.000000,-0.000000,1.359148,0.026965,0.001319,-0.000442,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.728757,-0.021889,0.000063,0.000339,-0.000002,0.000000,0.000000,-0.000000,-0.001967,-0.039853,0.000031,0.000619,-0.000001,0.000000,-0.000000,0.000000,1.386991,0.028279,-0.000005,-0.000441,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.750246,-0.020754,0.001069,0.000333,-0.000001,-0.000000,0.000000,-0.000000,-0.041171,-0.037937,0.001884,0.000617,0.000000,-0.000000,0.000000,-0.000000,1.414825,0.026948,-0.001326,-0.000441,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.769599,-0.017621,0.002061,0.000327,-0.000003,0.000003,-0.000003,0.000002,-0.076606,-0.032319,0.003731,0.000611,-0.000006,0.000005,-0.000005,0.000004,1.440006,0.022975,-0.002645,-0.000436,0.000004,-0.000004,0.000004,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.784833,-0.012519,0.003055,0.000365,0.000047,-0.000021,0.000050,-0.000037,-0.104585,-0.023024,0.005591,0.000682,0.000088,-0.000040,0.000092,-0.000067,1.459902,0.016377,-0.003973,-0.000487,-0.000062,0.000028,-0.000065,0.000048,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.793893,-0.005190,0.004197,0.000056,-0.000592,0.000002,-0.000735,0.000486,-0.121264,-0.009565,0.007728,0.000113,-0.001094,0.000004,-0.001359,0.000897,1.471768,0.006807,-0.005498,-0.000082,0.000778,-0.000003,0.000968,-0.000639,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/16.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/16.csv
new file mode 100644
index 0000000000000000000000000000000000000000..7cfa7c6f74a3fd497a8e9c738c5fc44ba4418607
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/16.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.795670,0.000000,0.000000,0.000000,-0.000157,0.000174,-0.000082,0.000015,-0.124540,0.000000,0.000000,0.000000,0.007004,-0.007535,0.003470,-0.000633,1.474100,0.000000,0.000000,0.000000,0.010480,-0.011270,0.005188,-0.000946,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.795719,-0.000139,-0.000105,0.000011,0.000017,-0.000015,0.000006,-0.000001,-0.122234,0.006730,0.005432,-0.000086,-0.000770,0.000638,-0.000268,0.000048,1.477552,0.010078,0.008140,-0.000122,-0.001153,0.000954,-0.000401,0.000071,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.795945,-0.000295,-0.000052,0.000013,-0.000003,0.000001,-0.000000,0.000000,-0.110510,0.016171,0.003911,-0.000484,0.000063,-0.000043,0.000017,-0.000003,1.495120,0.024244,0.005878,-0.000722,0.000093,-0.000064,0.000025,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.796282,-0.000369,-0.000025,0.000007,-0.000000,0.000001,-0.000000,0.000000,-0.090879,0.022658,0.002596,-0.000430,-0.000003,0.000002,-0.000001,0.000000,1.524569,0.034004,0.003910,-0.000646,-0.000006,0.000003,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.796669,-0.000400,-0.000006,0.000005,-0.000002,-0.000001,0.000000,0.000000,-0.066057,0.026555,0.001300,-0.000432,0.000001,-0.000000,-0.000000,0.000000,1.561834,0.039873,0.001956,-0.000653,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.797072,-0.000404,-0.000002,-0.000001,0.000001,0.000002,-0.000000,-0.000000,-0.038633,0.027861,0.000006,-0.000432,-0.000000,-0.000000,0.000000,0.000000,1.603011,0.041829,-0.000001,-0.000652,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.797477,-0.000401,0.000012,0.000012,0.000003,-0.000001,-0.000000,0.000000,-0.011198,0.026576,-0.001293,-0.000434,-0.000001,0.000000,-0.000000,0.000000,1.644187,0.039870,-0.001958,-0.000653,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.797852,-0.000335,0.000053,0.000010,-0.000003,-0.000000,0.000000,-0.000000,0.013651,0.022687,-0.002597,-0.000433,0.000003,-0.000004,0.000004,-0.000003,1.681447,0.033998,-0.003911,-0.000646,0.000006,-0.000005,0.000005,-0.000004,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.798127,-0.000209,0.000069,0.000002,-0.000001,0.000000,0.000000,-0.000000,0.033309,0.016192,-0.003917,-0.000485,-0.000062,0.000028,-0.000065,0.000048,1.710889,0.024236,-0.005878,-0.000721,-0.000092,0.000042,-0.000097,0.000071,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.798267,-0.000071,0.000065,-0.000008,-0.000007,-0.000000,-0.000007,0.000005,0.045047,0.006738,-0.005439,-0.000085,0.000771,-0.000003,0.000959,-0.000633,1.728449,0.010073,-0.008136,-0.000121,0.001152,-0.000004,0.001432,-0.000945,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/17.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/17.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0340fbcbdf653950b22e06258e9c494bd30a0a1d
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/17.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/18.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/18.csv
new file mode 100644
index 0000000000000000000000000000000000000000..413c7e1ab0199c071ef72c90658e23a7cfbac496
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/18.csv
@@ -0,0 +1,8 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/19.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/19.csv
new file mode 100644
index 0000000000000000000000000000000000000000..d98828c96992fea09300d200404244064a38ac64
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/19.csv
@@ -0,0 +1,20 @@
+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,
+1.000000,-0.512160,0.000000,0.000000,0.000000,0.024548,-0.026002,0.011889,-0.002158,-0.627310,0.000000,0.000000,0.000000,-0.002748,0.003075,-0.001440,0.000265,1.910400,0.000000,0.000000,0.000000,-0.019659,0.020793,-0.009501,0.001724,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.503884,0.024407,0.020280,0.000418,-0.002657,0.002172,-0.000905,0.000159,-0.628158,-0.002400,-0.001766,0.000246,0.000316,-0.000269,0.000116,-0.000021,1.903757,-0.019610,-0.016339,-0.000393,0.002123,-0.001735,0.000722,-0.000127,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.460009,0.062141,0.017087,-0.001009,0.000208,-0.000144,0.000055,-0.000009,-0.631937,-0.004731,-0.000532,0.000398,-0.000026,0.000019,-0.000008,0.000001,1.868399,-0.050204,-0.013964,0.000745,-0.000168,0.000114,-0.000044,0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.381681,0.093662,0.014492,-0.000849,-0.000018,0.000007,-0.000003,0.000001,-0.636815,-0.004647,0.000608,0.000376,0.000001,-0.000001,-0.000000,0.000000,1.804886,-0.076208,-0.012092,0.000604,0.000010,-0.000006,0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.274388,0.120053,0.011882,-0.000884,-0.000005,0.000001,-0.000000,0.000000,-0.640480,-0.002308,0.001724,0.000362,-0.000008,-0.000001,0.000000,0.000000,1.717195,-0.098561,-0.010258,0.000611,-0.000002,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.143341,0.141149,0.009207,-0.000897,-0.000001,0.000001,-0.000001,0.000000,-0.640711,0.002189,0.002754,0.000324,-0.000008,0.000003,0.000000,-0.000000,1.608986,-0.117251,-0.008437,0.000603,-0.000002,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.006118,0.156873,0.006517,-0.000896,0.000000,0.000000,-0.000000,0.000000,-0.635449,0.008651,0.003706,0.000319,0.000002,0.000000,-0.000001,0.000000,1.483899,-0.132322,-0.006635,0.000600,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.168612,0.167220,0.003832,-0.000894,0.000001,0.000001,-0.000000,0.000000,-0.622771,0.017026,0.004668,0.000318,-0.000003,-0.000001,0.000000,-0.000000,1.345542,-0.143789,-0.004831,0.000602,0.000001,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.338772,0.172209,0.001160,-0.000888,0.000002,0.000000,0.000000,0.000000,-0.600762,0.027302,0.005598,0.000298,-0.000009,-0.000001,-0.000002,-0.000000,1.197524,-0.151644,-0.003022,0.000605,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.511255,0.171875,-0.001484,-0.000868,0.000014,0.000006,-0.000001,0.000000,-0.567577,0.039336,0.006392,0.000203,-0.000054,-0.000017,0.000005,-0.000001,1.043465,-0.155869,-0.001201,0.000610,0.000002,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.680797,0.166380,-0.003964,-0.000774,0.000032,0.000002,-0.000001,0.000000,-0.521713,0.052452,0.006565,-0.000112,-0.000093,0.000001,0.000000,-0.000000,0.887006,-0.156434,0.000642,0.000620,0.000003,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.842472,0.156265,-0.006078,-0.000630,0.000040,-0.000000,-0.000001,0.000000,-0.462899,0.064882,0.005687,-0.000468,-0.000085,0.000007,-0.000000,-0.000000,0.731837,-0.153276,0.002523,0.000634,0.000004,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.992068,0.142375,-0.007737,-0.000480,0.000035,-0.000002,-0.000000,0.000000,-0.392876,0.074547,0.003843,-0.000742,-0.000055,0.000007,-0.000001,0.000000,0.581723,-0.146312,0.004448,0.000648,0.000003,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.126259,0.125592,-0.008985,-0.000359,0.000026,-0.000002,-0.000000,0.000000,-0.315277,0.079817,0.001345,-0.000908,-0.000031,0.000005,-0.000001,0.000000,0.440509,-0.135461,0.006408,0.000659,0.000002,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.242531,0.106639,-0.009926,-0.000275,0.000016,-0.000002,-0.000000,0.000000,-0.235050,0.079682,-0.001523,-0.000993,-0.000014,0.000003,-0.000000,0.000000,0.312117,-0.120661,0.008397,0.000667,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.338983,0.086015,-0.010677,-0.000232,0.000006,-0.000001,-0.000000,0.000000,-0.157894,0.073617,-0.004557,-0.001024,-0.000004,0.000002,-0.000001,0.000000,0.200521,-0.101861,0.010404,0.000671,0.000001,-0.000001,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.414095,0.063989,-0.011338,-0.000208,0.000009,-0.000007,0.000006,-0.000005,-0.089861,0.061423,-0.007636,-0.001020,0.000009,-0.000009,0.000009,-0.000007,0.109735,-0.079041,0.012412,0.000660,-0.000011,0.000010,-0.000009,0.000008,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.466542,0.040689,-0.012003,-0.000315,-0.000128,0.000052,-0.000123,0.000095,-0.037093,0.043083,-0.010753,-0.001153,-0.000161,0.000072,-0.000167,0.000123,0.043763,-0.052232,0.014452,0.000806,0.000176,-0.000075,0.000176,-0.000133,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.494809,0.015413,-0.013044,0.000563,0.001620,0.000020,0.001927,-0.001308,-0.006049,0.017694,-0.014376,-0.000106,0.002003,-0.000004,0.002478,-0.001641,0.006933,-0.020456,0.017021,-0.000381,-0.002220,-0.000014,-0.002686,0.001804,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/2.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..1e228b8f1c51c486dcc83634f616334c782134cb
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/2.csv
@@ -0,0 +1,17 @@
+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,
+1.000000,2.280100,0.000000,0.000000,-0.000000,-0.022791,0.024192,-0.011072,0.002011,-0.213800,0.000000,0.000000,0.000000,-0.008352,0.008869,-0.004060,0.000737,0.698800,0.000000,0.000000,0.000000,0.025351,-0.026906,0.012313,-0.002236,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,2.272440,-0.022562,-0.018680,-0.000306,0.002469,-0.002026,0.000845,-0.000149,-0.216605,-0.008259,-0.006832,-0.000103,0.000906,-0.000743,0.000310,-0.000055,0.707322,0.025099,0.020783,0.000341,-0.002748,0.002252,-0.000939,0.000166,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,2.232032,-0.057063,-0.015491,0.001002,-0.000197,0.000135,-0.000051,0.000009,-0.231381,-0.020847,-0.005633,0.000378,-0.000072,0.000049,-0.000019,0.000003,0.752276,0.063479,0.017224,-0.001123,0.000218,-0.000149,0.000057,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,2.160375,-0.085400,-0.012907,0.000839,0.000012,-0.000007,0.000002,-0.000000,-0.257521,-0.031110,-0.004653,0.000319,0.000004,-0.000003,0.000001,-0.000000,0.831973,0.094961,0.014325,-0.000942,-0.000014,0.000008,-0.000003,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,2.062915,-0.108671,-0.010358,0.000853,-0.000001,0.000000,-0.000000,0.000000,-0.292962,-0.039450,-0.003685,0.000323,-0.000000,0.000000,-0.000000,0.000000,0.940308,0.120755,0.011463,-0.000957,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.944738,-0.126830,-0.007802,0.000852,0.000000,0.000000,0.000000,0.000000,-0.335773,-0.045850,-0.002716,0.000323,-0.000000,-0.000000,-0.000000,-0.000000,1.071570,0.140811,0.008593,-0.000956,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.810959,-0.139877,-0.005244,0.000853,0.000000,0.000000,0.000000,-0.000000,-0.384017,-0.050314,-0.001749,0.000322,-0.000000,-0.000000,-0.000000,0.000000,1.220018,0.155129,0.005724,-0.000956,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.666691,-0.147804,-0.002682,0.000855,0.000001,0.000000,0.000000,-0.000000,-0.435758,-0.052847,-0.000786,0.000320,-0.000001,-0.000000,-0.000000,0.000000,1.379915,0.163708,0.002855,-0.000956,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.517061,-0.150598,-0.000109,0.000860,0.000002,0.000000,0.000000,-0.000000,-0.489072,-0.053463,0.000167,0.000315,-0.000002,-0.000000,-0.000000,0.000000,1.545522,0.166550,-0.000013,-0.000956,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.367216,-0.148229,0.002481,0.000867,0.000002,-0.000000,-0.000000,0.000000,-0.542055,-0.052193,0.001099,0.000305,-0.000003,-0.000000,0.000000,-0.000000,1.711102,0.163655,-0.002881,-0.000955,0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.222337,-0.140659,0.005091,0.000873,0.000001,-0.000000,-0.000000,-0.000000,-0.592847,-0.049092,0.001996,0.000293,-0.000003,-0.000000,0.000000,0.000000,1.870922,0.155029,-0.005745,-0.000954,0.000000,0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,1.087644,-0.127852,0.007718,0.000878,0.000001,-0.000000,0.000000,0.000000,-0.639652,-0.044231,0.002858,0.000282,-0.000002,0.000001,-0.000000,-0.000000,2.019251,0.140677,-0.008606,-0.000953,0.000000,-0.000000,-0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.968390,-0.109778,0.010357,0.000881,0.000001,-0.000001,0.000000,-0.000000,-0.680745,-0.037675,0.003696,0.000278,-0.000000,-0.000000,0.000000,-0.000000,2.150368,0.120605,-0.011465,-0.000953,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.869849,-0.086424,0.012991,0.000868,-0.000012,0.000011,-0.000011,0.000009,-0.714446,-0.029451,0.004525,0.000272,-0.000004,0.000004,-0.000003,0.000003,2.258555,0.094818,-0.014315,-0.000938,0.000014,-0.000012,0.000011,-0.000010,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.797281,-0.057831,0.015664,0.001034,0.000199,-0.000086,0.000201,-0.000151,-0.739101,-0.019585,0.005362,0.000326,0.000067,-0.000029,0.000067,-0.000051,2.338124,0.063369,-0.017201,-0.001118,-0.000218,0.000094,-0.000219,0.000165,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.756312,-0.022886,0.018942,-0.000300,-0.002508,-0.000011,-0.003050,0.002041,-0.752944,-0.007711,0.006399,-0.000122,-0.000841,-0.000004,-0.001020,0.000684,2.382995,0.025051,-0.020745,0.000343,0.002742,0.000013,0.003333,-0.002231,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/3.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..0259efd2e37092d8152907b659ba0d1059a4bbd6
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/3.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/4.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..a373e919bee60b2f0b588449dc65e7506c2b6a12
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/4.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,0.719370,0.000000,0.000000,0.000000,-0.020368,0.021903,-0.010083,0.001838,-0.754360,0.000000,0.000000,-0.000000,-0.004233,0.004551,-0.002095,0.000382,2.092600,0.000000,0.000000,-0.000000,0.015831,-0.017025,0.007837,-0.001429,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.712660,-0.019587,-0.015820,0.000236,0.002240,-0.001854,0.000779,-0.000138,-0.755755,-0.004072,-0.003289,0.000048,0.000466,-0.000385,0.000162,-0.000029,2.097815,0.015224,0.012296,-0.000184,-0.001741,0.001441,-0.000606,0.000107,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.678516,-0.047121,-0.011426,0.001402,-0.000180,0.000125,-0.000049,0.000008,-0.762854,-0.009800,-0.002379,0.000290,-0.000037,0.000026,-0.000010,0.000002,2.124354,0.036625,0.008881,-0.001090,0.000140,-0.000097,0.000038,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.621277,-0.066093,-0.007600,0.001256,0.000012,-0.000007,0.000002,-0.000000,-0.774762,-0.013754,-0.001587,0.000260,0.000002,-0.000001,0.000000,-0.000000,2.168844,0.051371,0.005907,-0.000976,-0.000009,0.000005,-0.000002,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.548847,-0.077502,-0.003803,0.001268,-0.000001,0.000000,-0.000000,0.000000,-0.789841,-0.016142,-0.000799,0.000263,-0.000000,0.000000,-0.000000,0.000000,2.225141,0.060239,0.002956,-0.000986,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.468810,-0.081304,-0.000000,0.001268,0.000000,0.000000,-0.000000,0.000000,-0.806519,-0.016951,-0.000010,0.000264,0.000000,0.000000,0.000000,0.000000,2.287350,0.063195,0.000000,-0.000985,-0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.388773,-0.077502,0.003803,0.001268,0.000001,-0.000001,0.000000,-0.000000,-0.823217,-0.016179,0.000782,0.000265,0.000000,-0.000000,0.000000,-0.000000,2.349559,0.060239,-0.002956,-0.000986,-0.000000,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.316343,-0.066093,0.007600,0.001256,-0.000012,0.000011,-0.000010,0.000008,-0.838348,-0.013819,0.001577,0.000264,-0.000002,0.000002,-0.000002,0.000002,2.405856,0.051371,-0.005907,-0.000976,0.000009,-0.000008,0.000008,-0.000007,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.259104,-0.047121,0.011426,0.001402,0.000180,-0.000081,0.000188,-0.000138,-0.850326,-0.009869,0.002385,0.000297,0.000038,-0.000017,0.000040,-0.000029,2.450346,0.036625,-0.008881,-0.001090,-0.000140,0.000063,-0.000146,0.000107,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,0.224960,-0.019587,0.015820,0.000236,-0.002240,0.000008,-0.002785,0.001838,-0.857482,-0.004109,0.003316,0.000053,-0.000471,0.000002,-0.000585,0.000386,2.476885,0.015224,-0.012296,-0.000184,0.001741,-0.000006,0.002165,-0.001429,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/5.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/5.csv
new file mode 100644
index 0000000000000000000000000000000000000000..e937da4215bf248c52e95cec56f10cb174bf23a3
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/5.csv
@@ -0,0 +1,9 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/6.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/6.csv
new file mode 100644
index 0000000000000000000000000000000000000000..40c79d2fa39c68bda8020e0461e7cbc91989ddd7
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/6.csv
@@ -0,0 +1,11 @@
+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,
+1.000000,-0.406870,0.000000,0.000000,0.000000,-0.006429,0.006914,-0.003183,0.000580,-0.966910,0.000000,0.000000,0.000000,0.015764,-0.016954,0.007805,-0.001423,2.296100,0.000000,0.000000,-0.000000,-0.046811,0.050340,-0.023174,0.004225,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.408988,-0.006183,-0.004994,0.000075,0.000707,-0.000585,0.000246,-0.000044,-0.961718,0.015157,0.012240,-0.000185,-0.001734,0.001436,-0.000603,0.000107,2.280679,-0.045017,-0.036358,0.000543,0.005149,-0.004262,0.001791,-0.000318,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.419766,-0.014874,-0.003607,0.000443,-0.000057,0.000039,-0.000015,0.000003,-0.935301,0.036452,0.008834,-0.001086,0.000140,-0.000097,0.000038,-0.000007,2.202208,-0.108295,-0.026259,0.003223,-0.000413,0.000287,-0.000111,0.000019,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.437833,-0.020862,-0.002399,0.000396,0.000004,-0.000002,0.000001,-0.000000,-0.891027,0.051118,0.005874,-0.000971,-0.000009,0.000005,-0.000002,0.000000,2.070658,-0.151897,-0.017468,0.002886,0.000026,-0.000015,0.000005,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.460696,-0.024464,-0.001201,0.000400,-0.000000,0.000000,-0.000000,0.000000,-0.835011,0.059934,0.002938,-0.000981,0.000000,-0.000000,0.000000,-0.000000,1.904195,-0.178118,-0.008741,0.002915,-0.000001,0.000001,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.485960,-0.025664,0.000000,0.000400,-0.000000,0.000000,-0.000000,0.000000,-0.773120,0.062869,-0.000002,-0.000980,0.000000,-0.000000,0.000000,-0.000000,1.720250,-0.186858,0.000000,0.002913,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.511224,-0.024464,0.001201,0.000400,0.000000,-0.000000,0.000000,-0.000000,-0.711233,0.059925,-0.002942,-0.000980,-0.000000,0.000001,-0.000000,0.000000,1.536305,-0.178118,0.008741,0.002915,0.000001,-0.000002,0.000001,-0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.534087,-0.020862,0.002399,0.000396,-0.000004,0.000003,-0.000003,0.000003,-0.655231,0.051101,-0.005877,-0.000971,0.000009,-0.000008,0.000008,-0.000007,1.369842,-0.151897,0.017468,0.002886,-0.000026,0.000025,-0.000024,0.000019,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.552154,-0.014874,0.003607,0.000443,0.000057,-0.000026,0.000059,-0.000044,-0.610976,0.036431,-0.008834,-0.001084,-0.000139,0.000063,-0.000146,0.000107,1.238292,-0.108295,0.026259,0.003223,0.000413,-0.000187,0.000433,-0.000318,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.562932,-0.006183,0.004994,0.000075,-0.000707,0.000003,-0.000879,0.000580,-0.584578,0.015144,-0.012231,-0.000183,0.001732,-0.000006,0.002153,-0.001421,1.159821,-0.045017,0.036358,0.000543,-0.005149,0.000019,-0.006400,0.004225,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/7.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/7.csv
new file mode 100644
index 0000000000000000000000000000000000000000..78a2a02dc67dcfa61a65d9d265dcc3c58517b231
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/7.csv
@@ -0,0 +1,12 @@
+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,
+1.000000,-0.565050,0.000000,0.000000,0.000000,0.015778,-0.016912,0.007774,-0.001416,-0.579390,0.000000,0.000000,0.000000,-0.006867,0.007355,-0.003379,0.000615,1.144400,0.000000,0.000000,0.000000,0.037560,-0.040260,0.018507,-0.003371,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.559826,0.015284,0.012423,-0.000085,-0.001729,0.001428,-0.000599,0.000106,-0.581667,-0.006666,-0.005427,0.000025,0.000752,-0.000620,0.000260,-0.000046,1.156836,0.036385,0.029575,-0.000202,-0.004116,0.003399,-0.001426,0.000253,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.532998,0.037247,0.009314,-0.000990,0.000138,-0.000096,0.000037,-0.000006,-0.593389,-0.016301,-0.004109,0.000420,-0.000060,0.000042,-0.000016,0.000003,1.220703,0.088670,0.022175,-0.002357,0.000329,-0.000228,0.000088,-0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.487354,0.053158,0.006639,-0.000877,-0.000009,0.000005,-0.000002,0.000000,-0.613411,-0.023369,-0.002977,0.000371,0.000004,-0.000002,0.000001,-0.000000,1.329367,0.126552,0.015806,-0.002087,-0.000021,0.000012,-0.000004,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.428439,0.063787,0.003986,-0.000886,0.000000,-0.000000,0.000000,-0.000000,-0.639384,-0.028201,-0.001852,0.000377,0.000000,0.000000,0.000000,-0.000000,1.469625,0.151859,0.009491,-0.002110,0.000001,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.361552,0.069101,0.001328,-0.000886,-0.000000,0.000000,-0.000000,0.000000,-0.669059,-0.030771,-0.000717,0.000380,0.000001,0.000000,0.000000,0.000000,1.628865,0.164512,0.003163,-0.002109,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.292009,0.069100,-0.001329,-0.000886,0.000000,-0.000000,0.000000,-0.000000,-0.700167,-0.031062,0.000429,0.000386,0.000002,0.000000,0.000000,-0.000000,1.794432,0.164512,-0.003163,-0.002109,0.000000,-0.000000,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.225123,0.063785,-0.003986,-0.000886,-0.000000,0.000001,-0.000000,0.000000,-0.730411,-0.029034,0.001607,0.000402,0.000006,0.000000,0.000000,-0.000000,1.953672,0.151860,-0.009490,-0.002110,-0.000001,0.000002,-0.000001,0.000001,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.166211,0.053155,-0.006639,-0.000877,0.000009,-0.000008,0.000008,-0.000006,-0.757430,-0.024587,0.002854,0.000428,0.000005,0.000005,-0.000004,0.000003,2.093932,0.126553,-0.015806,-0.002087,0.000021,-0.000019,0.000019,-0.000015,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.120569,0.037244,-0.009314,-0.000990,-0.000138,0.000062,-0.000144,0.000106,-0.778726,-0.017554,0.004219,0.000522,0.000077,-0.000032,0.000072,-0.000052,2.202596,0.088671,-0.022175,-0.002357,-0.000329,0.000147,-0.000342,0.000253,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+1.000000,-0.093743,0.015283,-0.012422,-0.000085,0.001729,-0.000003,0.002138,-0.001416,-0.791474,-0.007336,0.005905,0.000112,-0.000840,0.000002,-0.001052,0.000694,2.266464,0.036385,-0.029575,-0.000202,0.004117,-0.000007,0.005089,-0.003371,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/8.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/8.csv
new file mode 100644
index 0000000000000000000000000000000000000000..20b919fcaa41528ec4ae162febb76feafe2024fe
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/8.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/sequence_trajectories/7/9.csv b/real_robot/crazyswarm/scripts/sequence_trajectories/7/9.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ba13f329a673a59bc322c03f7d3abc1ae8482edd
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/sequence_trajectories/7/9.csv
@@ -0,0 +1,10 @@
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
+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,
diff --git a/real_robot/crazyswarm/scripts/swap6v.py b/real_robot/crazyswarm/scripts/swap6v.py
new file mode 100644
index 0000000000000000000000000000000000000000..0a1da5db9321e7e1f1d0ccabcc599c0890347bb0
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/swap6v.py
@@ -0,0 +1,64 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+from pycrazyswarm import *
+import pycrazyswarm.cfsim.cffirmware as firm
+import uav_trajectory
+
+TIMESCALE = 1.0
+
+POSITIONS = [
+ [0, 1.0, 1.5],
+ [0.25, 0.5, 2.37],
+ [0.25, -0.5, 2.37],
+ [-0.25, 0.5, 0.63],
+ [-0.25, -0.5, 0.63],
+ [0, -1.0, 1.5]
+]
+
+OFFSET = [0.0, 0.0, -0.3]
+
+if __name__ == "__main__":
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+
+ ids = [15, 16, 17, 18, 19, 20] #[15, 16, 17] #range(1, 6+1)
+ trajIds = [1, 2, 3, 4, 5, 6] #[1, 2, 3, 4, 5, 6]
+
+ cfs = [allcfs.crazyfliesById[i] for i in ids]
+ root = 'swap6v_pps'
+ fnames = ['{0}/pp{1}.csv'.format(root, i) for i in trajIds]
+ # trajs = [piecewise.loadcsv(fname) for fname in fnames]
+
+ T = 0
+ for cf, fname in zip(cfs, fnames):
+ traj = uav_trajectory.Trajectory()
+ traj.loadcsv(fname)
+ cf.uploadTrajectory(0, 0, traj)
+ T = max(T, traj.duration)
+ print("T: ", T * TIMESCALE)
+
+ allcfs.takeoff(targetHeight=1.0, duration=2.0)
+ timeHelper.sleep(2.5)
+
+ for cf, trajId in zip(cfs, trajIds):
+ pos = POSITIONS[trajId - 1]
+ print(pos)
+ cf.goTo(np.array(pos) + np.array(OFFSET), 0, 3.0)
+ timeHelper.sleep(3.5)
+
+ allcfs.startTrajectory(0, timescale=TIMESCALE)
+ timeHelper.sleep(T + 3.0)
+ allcfs.startTrajectory(0, timescale=TIMESCALE, reverse=True)
+ timeHelper.sleep(T + 3.0)
+
+ for cf in cfs:
+ pos = np.array(cf.initialPosition) + np.array([0, 0, 1.0])
+ cf.goTo(pos, 0, 3.0)
+ timeHelper.sleep(3.5)
+
+ allcfs.land(targetHeight=0.06, duration=2.0)
+ timeHelper.sleep(3.0)
+
diff --git a/real_robot/crazyswarm/scripts/swap6v_pps/pp1.csv b/real_robot/crazyswarm/scripts/swap6v_pps/pp1.csv
new file mode 100644
index 0000000000000000000000000000000000000000..566504cbdf1d75b85cae766f83abb4034aa80a9c
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/swap6v_pps/pp1.csv
@@ -0,0 +1,18 @@
+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,
+0.250000,-3.300000,0.000000,0.000000,0.000000,-0.329790,0.516062,-0.307082,0.073682,0.000000,0.000000,0.000000,0.000000,-1.136523,1.428935,-0.753062,0.159072,2.000000,0.000000,0.000000,0.000000,0.173516,-0.206401,0.104584,-0.021660,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.300855,-0.012206,-0.059519,-0.093141,0.067693,0.151610,-0.172804,0.061839,-0.003218,-0.047264,-0.243788,-0.457023,0.030642,0.508959,-0.457256,0.163264,2.000500,0.007389,0.038502,0.074237,0.001718,-0.078163,0.064024,-0.023697,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.308707,-0.053144,-0.089158,0.023761,0.129019,-0.028332,-0.065544,0.031943,-0.036897,-0.245394,-0.518983,-0.228852,0.327448,0.032531,-0.170897,0.087426,2.005859,0.039475,0.085877,0.043871,-0.048922,-0.012505,0.022228,-0.012322,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.326732,-0.086087,-0.030567,0.118957,0.049625,-0.085447,-0.012183,0.020340,-0.132983,-0.527547,-0.570967,0.077476,0.255707,-0.112843,-0.021212,0.027042,2.021582,0.087447,0.099530,-0.007605,-0.050453,0.005414,0.000815,-0.000358,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.348197,-0.077670,0.063613,0.114152,-0.057482,-0.076188,0.021095,0.033683,-0.298460,-0.784804,-0.435290,0.259724,0.109555,-0.110824,0.022816,0.000645,2.049353,0.132742,0.075792,-0.054469,-0.043117,0.006810,0.000376,0.009888,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.362147,-0.029360,0.117693,0.020249,-0.114521,0.001270,0.080050,-0.034284,-0.517483,-0.948933,-0.215380,0.307232,-0.007232,-0.075668,0.021689,-0.001801,2.086264,0.157883,0.020060,-0.091861,-0.028845,0.020758,0.018290,-0.007760,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.362243,0.026561,0.094120,-0.073149,-0.056636,0.077161,0.022394,-0.051777,-0.763474,-1.000822,0.001743,0.259239,-0.082469,-0.044284,0.017186,0.016297,2.125465,0.149386,-0.055496,-0.103077,0.010006,0.037852,0.005857,-0.018116,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.351007,0.057916,0.030327,-0.081640,0.032495,0.042402,-0.066939,0.034700,-1.009880,-0.957234,0.159669,0.156692,-0.112799,0.004503,0.045819,-0.026533,2.157808,0.103679,-0.123166,-0.070060,0.052905,0.022106,-0.025168,0.005217,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.335754,0.060298,-0.015303,-0.038818,0.041719,-0.013088,-0.006621,0.006143,-1.237187,-0.854759,0.237733,0.057399,-0.078725,0.039582,0.000569,-0.007553,2.175158,0.032559,-0.153785,-0.010490,0.059796,-0.009670,-0.016324,0.011123,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.322094,0.047692,-0.031079,-0.006508,0.022511,-0.015294,0.003679,0.000744,-1.435391,-0.729287,0.257323,0.002558,-0.032844,0.031023,-0.011730,0.000428,2.173743,-0.042828,-0.141469,0.039681,0.038487,-0.020133,0.002543,0.001934,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.312141,0.032063,-0.029677,0.007695,0.007249,-0.008912,0.004717,-0.001051,-1.601691,-0.601661,0.251093,-0.014504,-0.004828,0.014072,-0.010563,0.001735,2.154946,-0.104092,-0.100232,0.066644,0.016763,-0.014034,0.005514,-0.002132,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.305839,0.018972,-0.022325,0.010705,-0.000043,-0.003217,0.002783,-0.001082,-1.736647,-0.478920,0.240019,-0.013602,0.003807,0.000454,-0.007514,0.000941,2.123752,-0.140910,-0.045876,0.076067,0.003223,-0.008612,0.001528,-0.002923,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.302327,0.009766,-0.014674,0.009373,-0.002047,-0.000433,0.000877,-0.000609,-1.841575,-0.361256,0.230896,-0.011730,-0.002154,-0.009543,-0.006152,0.001542,2.086850,-0.149548,0.011067,0.073986,-0.007709,-0.010039,-0.003886,-0.001036,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.300664,0.004053,-0.008441,0.007246,-0.002099,0.000115,-0.000185,-0.000108,-1.917660,-0.248362,0.219471,-0.021560,-0.019008,-0.016461,-0.003952,0.006114,2.051270,-0.130845,0.061848,0.058647,-0.024467,-0.016858,-0.006210,0.004866,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.300074,0.001061,-0.003789,0.005147,-0.002187,-0.000271,-0.000382,0.000417,-1.966462,-0.144191,0.193495,-0.051255,-0.039946,-0.013779,0.006326,0.015494,2.023228,-0.090811,0.093760,0.022368,-0.048701,-0.019066,0.001726,0.016392,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.299973,-0.000011,-0.000805,0.002727,-0.002657,-0.000257,0.000338,0.001046,-1.991384,-0.059756,0.138609,-0.095717,-0.042764,0.016672,0.033862,0.025256,2.006527,-0.043115,0.089731,-0.035469,-0.061951,0.005952,0.030503,0.030766,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.299994,-0.000070,0.000246,0.000159,-0.002089,0.001648,0.002228,0.001488,-1.999296,-0.010504,0.055892,-0.114027,0.023634,0.100040,0.081449,0.017670,2.000575,-0.008425,0.043246,-0.079960,-0.009088,0.091985,0.087505,0.030810,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/swap6v_pps/pp2.csv b/real_robot/crazyswarm/scripts/swap6v_pps/pp2.csv
new file mode 100644
index 0000000000000000000000000000000000000000..ea411604a62480896d5160402e2a08238e01fae1
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/swap6v_pps/pp2.csv
@@ -0,0 +1,18 @@
+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,
+0.250000,-3.050000,0.000000,0.000000,0.000000,-0.060696,0.043589,-0.012758,-0.000719,-0.500000,0.000000,0.000000,0.000000,0.048791,-0.090020,0.057520,-0.015756,2.865000,0.000000,0.000000,0.000000,-0.555496,0.656161,-0.332329,0.065456,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.050198,-0.003018,-0.016713,-0.037538,-0.018564,0.023824,-0.013237,0.004036,-0.499884,0.001601,0.007278,0.008350,-0.018425,-0.024226,0.029083,-0.012100,2.863394,-0.023738,-0.123916,-0.240299,-0.011056,0.244337,-0.209460,0.073350,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.052636,-0.019179,-0.048798,-0.044798,0.001013,0.009253,-0.005910,0.003867,-0.498988,0.005331,0.004302,-0.017783,-0.028060,0.003935,0.008044,-0.005516,2.846109,-0.127773,-0.280877,-0.154072,0.138110,0.024387,-0.080521,0.041468,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.061168,-0.051761,-0.080838,-0.039319,0.009154,0.005328,0.001018,0.001681,-0.497768,0.002509,-0.018584,-0.041623,-0.018616,0.009039,-0.001343,-0.001076,2.794749,-0.288392,-0.344696,-0.020213,0.115783,-0.043678,-0.009231,0.014123,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.079734,-0.098868,-0.105968,-0.026287,0.017687,0.008793,0.004225,-0.002581,-0.499017,-0.015585,-0.055472,-0.055157,-0.009165,0.005751,-0.003057,0.001148,2.701200,-0.458177,-0.323513,0.067318,0.060255,-0.039900,0.014301,-0.000416,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.111406,-0.155483,-0.117482,-0.002138,0.031227,0.011276,0.000058,-0.010004,-0.507273,-0.054139,-0.099533,-0.061526,-0.004214,0.002737,-0.000968,0.002164,2.567688,-0.604241,-0.255834,0.107047,0.023559,-0.019359,0.013004,-0.006211,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.157520,-0.212470,-0.105815,0.034787,0.039905,-0.002338,-0.017510,0.008295,-0.528004,-0.115654,-0.146843,-0.064036,-0.000516,0.004131,0.002940,0.003364,2.402387,-0.710927,-0.169104,0.121721,0.008155,-0.008126,0.001898,-0.001082,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.216558,-0.256495,-0.065982,0.068893,0.025103,-0.018088,-0.003535,0.002379,-0.567093,-0.201010,-0.194177,-0.060592,0.009244,0.012811,0.009210,0.001043,2.216012,-0.772296,-0.075936,0.125242,-0.000815,-0.006687,-0.000195,-0.000888,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.283649,-0.275369,-0.007882,0.081912,0.000481,-0.020275,0.000023,0.002260,-0.630377,-0.308576,-0.233591,-0.040320,0.034462,0.027472,0.011863,-0.007311,2.020139,-0.786966,0.016615,0.120065,-0.009843,-0.007998,-0.001989,0.001478,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.351722,-0.264314,0.050612,0.070037,-0.023606,-0.016925,0.003462,0.007697,-0.722586,-0.430184,-0.246071,0.014019,0.075925,0.034537,0.000135,-0.025265,1.826265,-0.756927,0.101637,0.104804,-0.020897,-0.008730,0.000327,0.006420,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.413650,-0.227648,0.092004,0.037987,-0.037308,-0.001098,0.016891,-0.009586,-0.844964,-0.545213,-0.202198,0.108118,0.105406,0.000069,-0.044052,0.018459,1.644934,-0.687921,0.171190,0.079431,-0.027992,0.000586,0.011575,-0.009226,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.464362,-0.176794,0.107125,0.003961,-0.028087,0.012076,0.000477,-0.002912,-0.991813,-0.619677,-0.083774,0.202325,0.074288,-0.042879,-0.013026,0.000979,1.484788,-0.589119,0.220846,0.054160,-0.021454,0.006164,-0.004384,-0.004057,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.501901,-0.124011,0.101418,-0.016828,-0.014137,0.009185,-0.004347,0.000512,-1.148562,-0.619898,0.088385,0.245877,0.009013,-0.061256,-0.013136,-0.000980,1.352078,-0.469794,0.254044,0.034634,-0.020078,-0.005428,-0.011650,0.000883,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.526875,-0.077186,0.084687,-0.026512,-0.006450,0.003437,-0.003350,0.002095,-1.294198,-0.530315,0.265811,0.212365,-0.080408,-0.081033,-0.017295,0.018366,1.250962,-0.337706,0.270978,0.007643,-0.037302,-0.021178,-0.010750,0.009901,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.541316,-0.040165,0.062768,-0.031575,-0.004149,0.001223,0.000351,0.003074,-1.407242,-0.364269,0.381633,0.078417,-0.187870,-0.080095,0.012415,0.062743,1.183423,-0.203575,0.258986,-0.044900,-0.068438,-0.023298,0.005866,0.026047,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.547942,-0.014930,0.037805,-0.034430,-0.000611,0.005791,0.005894,0.003148,-1.474037,-0.171876,0.359494,-0.147054,-0.242037,0.024448,0.122606,0.118851,1.147727,-0.087154,0.196884,-0.122505,-0.077817,0.020829,0.051941,0.043933,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.549845,-0.002368,0.013069,-0.029149,0.013875,0.018524,0.012019,-0.000439,-1.497720,-0.033430,0.171881,-0.319248,-0.031537,0.363886,0.343090,0.118051,1.136061,-0.015759,0.083023,-0.165065,0.020940,0.155725,0.134125,0.035714,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/swap6v_pps/pp3.csv b/real_robot/crazyswarm/scripts/swap6v_pps/pp3.csv
new file mode 100644
index 0000000000000000000000000000000000000000..5436484ec17e93bdaaaacebbec926594ae5247bb
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/swap6v_pps/pp3.csv
@@ -0,0 +1,18 @@
+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,
+0.250000,-3.050000,0.000000,0.000000,0.000000,-0.675389,0.879090,-0.473181,0.103356,-1.500000,0.000000,0.000000,0.000000,0.584666,-0.724336,0.378638,-0.079059,2.865000,0.000000,0.000000,0.000000,0.003689,-0.077488,0.064811,-0.022218,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.051889,-0.027638,-0.141519,-0.259696,0.036389,0.305225,-0.281821,0.101771,-1.498336,0.024478,0.126637,0.239471,-0.009016,-0.260665,0.231369,-0.082728,2.864953,-0.000941,-0.007382,-0.027526,-0.044562,-0.008838,0.025546,-0.012252,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.071324,-0.140332,-0.289380,-0.106697,0.209368,0.012978,-0.103531,0.053209,-1.480798,0.128257,0.273990,0.128532,-0.163181,-0.019786,0.086170,-0.044945,2.863649,-0.012622,-0.044872,-0.071303,-0.038360,0.013973,0.004534,-0.003173,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.125351,-0.292205,-0.293837,0.085703,0.157629,-0.074810,-0.012613,0.015977,-1.430240,0.279195,0.310232,-0.026232,-0.131708,0.052424,0.009051,-0.013816,2.856440,-0.050531,-0.110351,-0.099946,-0.018378,0.016883,-0.000520,0.001201,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.214888,-0.414710,-0.182550,0.194819,0.061029,-0.073682,0.013156,0.001261,-1.341923,0.422214,0.249606,-0.124236,-0.065249,0.048853,-0.013678,0.001903,2.835293,-0.125266,-0.189570,-0.107771,0.002895,0.017635,0.002107,0.000487,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.326761,-0.467002,-0.024266,0.214080,-0.018049,-0.052035,0.013802,0.005342,-1.222921,0.520522,0.138832,-0.162966,-0.015966,0.031079,-0.009429,0.005842,2.790473,-0.239720,-0.266424,-0.093129,0.027181,0.021027,0.003598,-0.006024,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.441800,-0.441049,0.122314,0.168553,-0.067232,-0.023350,0.022442,-0.010782,-1.086694,0.558946,0.015044,-0.161654,0.017239,0.024345,0.001523,-0.002296,2.712564,-0.388273,-0.322705,-0.052505,0.053543,0.017732,-0.006404,0.003643,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.542065,-0.352833,0.220962,0.092266,-0.081277,-0.002628,0.003478,0.007525,-0.948452,0.537716,-0.095886,-0.129038,0.047842,0.022904,-0.001790,-0.013761,2.594731,-0.555809,-0.339535,0.010616,0.071695,0.011836,0.000346,-0.012729,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.615340,-0.230150,0.259630,0.011462,-0.077186,0.013588,0.017035,-0.011296,-0.821824,0.468982,-0.171532,-0.069322,0.067269,0.001192,-0.025820,0.013041,2.435015,-0.718893,-0.303079,0.088077,0.079854,-0.005508,-0.022076,0.013054,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.656757,-0.102665,0.242171,-0.053452,-0.050407,0.025069,-0.001986,-0.003144,-0.716124,0.374316,-0.199357,-0.007594,0.051684,-0.021189,-0.003630,0.006010,2.238027,-0.849142,-0.208962,0.159375,0.059412,-0.022375,0.000103,0.002531,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.668295,0.005721,0.186916,-0.089242,-0.022653,0.018306,-0.006948,0.002344,-0.634942,0.276020,-0.189070,0.030535,0.025082,-0.019125,0.006321,-0.000046,2.015382,-0.920459,-0.070590,0.205180,0.032923,-0.019393,0.003962,-0.005353,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.656649,0.081351,0.113991,-0.102304,-0.005002,0.011037,-0.002521,0.003572,-0.577196,0.188441,-0.159382,0.045634,0.007077,-0.009815,0.005950,-0.001818,1.784172,-0.915590,0.092733,0.226489,0.009469,-0.020612,-0.006014,-0.007518,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.630795,0.119059,0.037037,-0.100708,0.008384,0.011816,0.004083,0.001523,-0.539315,0.117589,-0.123725,0.048187,-0.000608,-0.003269,0.002673,-0.001713,1.564624,-0.826616,0.262423,0.220168,-0.026045,-0.039094,-0.020338,-0.001039,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.600244,0.119476,-0.033233,-0.083454,0.027815,0.019517,0.007338,-0.005191,-0.516903,0.064672,-0.088202,0.046137,-0.003125,-0.001461,-0.000369,-0.000966,1.377667,-0.656635,0.410461,0.163191,-0.094548,-0.069535,-0.024262,0.021773,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.573627,0.089365,-0.082020,-0.041858,0.056251,0.022876,-0.001052,-0.018519,-0.505540,0.028994,-0.055042,0.041851,-0.005826,-0.003193,-0.002155,0.000449,1.241270,-0.428178,0.485558,0.020578,-0.192306,-0.074510,0.011574,0.067215,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.556825,0.044431,-0.089187,0.025830,0.073732,-0.004098,-0.033483,-0.035661,-0.501104,0.008882,-0.026454,0.033417,-0.011592,-0.005661,-0.001544,0.003229,1.164077,-0.194832,0.419292,-0.205490,-0.237834,0.034575,0.129881,0.122237,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.550614,0.008952,-0.045499,0.081662,0.017717,-0.101121,-0.099377,-0.037469,-0.500066,0.001081,-0.006648,0.018245,-0.018351,-0.003477,0.004014,0.007492,1.137508,-0.036935,0.191506,-0.364415,-0.006004,0.388950,0.357116,0.115030,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/swap6v_pps/pp4.csv b/real_robot/crazyswarm/scripts/swap6v_pps/pp4.csv
new file mode 100644
index 0000000000000000000000000000000000000000..b892ddd77a8490040ad6c7cff5855c8e5a1946bb
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/swap6v_pps/pp4.csv
@@ -0,0 +1,18 @@
+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,
+0.250000,-3.550000,0.000000,0.000000,0.000000,-0.120958,0.175170,-0.100145,0.024070,-0.500000,0.000000,0.000000,0.000000,-0.206522,0.232409,-0.113584,0.020764,1.135000,0.000000,0.000000,0.000000,0.473198,-0.555078,0.279546,-0.054397,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.550324,-0.004684,-0.023363,-0.039481,0.017282,0.056444,-0.056064,0.021367,-0.500606,-0.008998,-0.047362,-0.093923,-0.011141,0.089662,-0.074207,0.025279,1.136371,0.020278,0.105982,0.206195,0.011676,-0.207832,0.177277,-0.061629,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.553462,-0.021878,-0.040521,-0.001521,0.046962,-0.000303,-0.018720,0.010416,-0.507256,-0.049627,-0.111802,-0.068759,0.045192,0.010867,-0.029699,0.014889,1.151169,0.109535,0.241657,0.134949,-0.115621,-0.021104,0.068925,-0.034959,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.561309,-0.039586,-0.024982,0.040826,0.034730,-0.015227,-0.000938,0.002185,-0.527544,-0.115531,-0.146161,-0.024020,0.039076,-0.014715,-0.004074,0.005675,1.195307,0.248372,0.299535,0.022897,-0.096502,0.037824,0.008855,-0.012185,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.572008,-0.042551,0.016272,0.066045,0.016012,-0.014007,0.002471,-0.001660,-0.565799,-0.190975,-0.151944,0.005361,0.019966,-0.013680,0.005451,0.000847,1.276140,0.397171,0.286698,-0.048864,-0.047584,0.035834,-0.011404,-0.000700,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.580548,-0.021293,0.069732,0.073847,-0.000088,-0.012478,-0.000805,-0.001630,-0.622890,-0.264928,-0.142236,0.018597,0.008440,-0.004524,0.006801,-0.001260,1.392434,0.529016,0.237123,-0.077711,-0.013866,0.018027,-0.012096,0.002777,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.580372,0.027163,0.123054,0.065486,-0.017331,-0.015562,-0.004124,0.002544,-0.697691,-0.332082,-0.125458,0.026162,0.008472,0.003894,0.004716,-0.003340,1.538255,0.632426,0.175805,-0.083711,-0.001154,0.003551,-0.007133,0.003157,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.564951,0.099562,0.163049,0.037488,-0.039259,-0.017823,-0.000221,0.011877,-0.788106,-0.389278,-0.101844,0.038085,0.015934,0.006346,-0.000931,-0.007116,1.706039,0.704593,0.112790,-0.084443,-0.001677,-0.002979,-0.001702,0.003582,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.529454,0.185333,0.173889,-0.011356,-0.055250,-0.001771,0.020487,-0.010558,-0.891128,-0.431957,-0.066514,0.056721,0.019103,-0.004656,-0.013505,0.012034,1.887907,0.744989,0.048338,-0.088024,-0.005038,-0.000764,0.004555,-0.004039,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.472643,0.266762,0.145360,-0.062754,-0.044032,0.015755,0.002480,-0.003643,-1.002321,-0.453535,-0.018082,0.070339,0.007203,-0.009238,0.007273,-0.003002,2.075781,0.752343,-0.019505,-0.092668,-0.003931,0.000830,-0.002489,0.002181,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.398005,0.325240,0.084315,-0.096662,-0.024005,0.015055,-0.003452,0.002113,-1.115715,-0.449080,0.036295,0.073630,0.000832,-0.002283,0.001955,-0.003225,2.261184,0.724975,-0.090452,-0.096561,-0.004035,0.000017,0.001325,0.003119,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.313015,0.348051,0.005011,-0.112047,-0.007266,0.012759,0.000624,0.003833,-1.224564,-0.417113,0.091521,0.073205,-0.001952,-0.003550,-0.003793,-0.002707,2.435251,0.661405,-0.164241,-0.099744,-0.001065,0.006108,0.006963,0.003270,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.227455,0.329353,-0.079640,-0.110619,0.011364,0.018549,0.007885,0.001021,-1.321991,-0.357845,0.144860,0.067479,-0.011426,-0.012615,-0.008908,0.000136,2.588782,0.560682,-0.238019,-0.094369,0.014885,0.020610,0.013302,-0.000458,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.151759,0.269912,-0.154962,-0.085059,0.042501,0.031075,0.010613,-0.009229,-1.401403,-0.273775,0.188695,0.045404,-0.035471,-0.025260,-0.009435,0.008700,2.712683,0.425388,-0.299224,-0.062508,0.052869,0.039162,0.013687,-0.013216,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.095096,0.179792,-0.197530,-0.021081,0.086247,0.033606,-0.004515,-0.029619,-1.457509,-0.173665,0.205124,-0.007614,-0.071134,-0.026944,0.004970,0.025498,2.799599,0.268183,-0.319629,0.017308,0.107426,0.040763,-0.008199,-0.038574,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.062457,0.083044,-0.176620,0.080709,0.107824,-0.013632,-0.056600,-0.054592,-1.488526,-0.077430,0.169343,-0.090549,-0.086210,0.015247,0.049916,0.045408,2.847394,0.119010,-0.261265,0.142375,0.129598,-0.024071,-0.076224,-0.068488,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.051083,0.015915,-0.082220,0.154862,0.007867,-0.169871,-0.157961,-0.052642,-1.499021,-0.014457,0.075340,-0.145423,0.004476,0.149285,0.134482,0.041093,2.863502,0.022139,-0.115517,0.223745,-0.009405,-0.227602,-0.203854,-0.061325,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/swap6v_pps/pp5.csv b/real_robot/crazyswarm/scripts/swap6v_pps/pp5.csv
new file mode 100644
index 0000000000000000000000000000000000000000..302a42a0c02e3d94d4bc3f2d38cbe1d81f6a8e97
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/swap6v_pps/pp5.csv
@@ -0,0 +1,18 @@
+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,
+0.250000,-3.550000,0.000000,0.000000,0.000000,0.269437,-0.337244,0.177199,-0.037339,-1.500000,0.000000,0.000000,0.000000,0.202310,-0.229737,0.112890,-0.020900,1.135000,0.000000,0.000000,0.000000,0.111282,-0.084255,0.025820,0.001354,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.549236,0.011227,0.057962,0.108929,-0.006414,-0.120662,0.107726,-0.038520,-1.499408,0.008783,0.046156,0.091144,0.009542,-0.088184,0.073322,-0.025013,1.135359,0.005463,0.030106,0.066876,0.030910,-0.044285,0.026736,-0.006671,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.541223,0.058440,0.123922,0.055500,-0.077314,-0.008491,0.040138,-0.020611,-1.492936,0.048211,0.108097,0.065065,-0.045628,-0.010361,0.029294,-0.014515,1.139735,0.034268,0.086365,0.077551,-0.003029,-0.012898,0.014700,-0.006137,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.518303,0.126010,0.137157,-0.017396,-0.061570,0.025574,0.004818,-0.006068,-1.473292,0.111552,0.139584,0.020131,-0.039054,0.015104,0.004335,-0.005310,1.154890,0.091626,0.142113,0.070216,-0.008727,0.001222,0.003996,-0.004127,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.478715,0.187995,0.105175,-0.062306,-0.028404,0.025265,-0.005050,0.000763,-1.436503,0.182989,0.142543,-0.008854,-0.019013,0.014926,-0.004513,-0.000722,1.187743,0.175342,0.191843,0.062937,-0.005710,0.001886,-0.003169,-0.002723,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.426203,0.227590,0.051461,-0.076394,-0.001140,0.018711,-0.003159,0.001084,-1.382046,0.251676,0.130826,-0.020047,-0.004982,0.007288,-0.005560,0.000545,1.244531,0.282721,0.236958,0.057043,-0.007813,-0.006317,-0.008123,-0.000748,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.367270,0.239274,-0.003501,-0.066678,0.019880,0.015100,-0.000807,-0.003622,-1.311277,0.313129,0.114746,-0.022137,-0.000786,-0.000321,-0.004616,0.000800,1.330874,0.411235,0.275332,0.042642,-0.023734,-0.019122,-0.010009,0.005033,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.308620,0.226548,-0.043817,-0.038108,0.036017,0.008603,-0.006870,-0.012087,-1.226173,0.366271,0.097545,-0.024457,-0.005077,-0.006116,-0.003400,0.002055,1.451444,0.554989,0.294942,0.004516,-0.054268,-0.026718,-0.002025,0.017960,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.255171,0.199852,-0.058198,-0.000514,0.033721,-0.018027,-0.028563,0.028291,-1.128918,0.410004,0.076186,-0.034138,-0.014787,-0.008299,-0.000060,0.004901,1.608458,0.699412,0.274054,-0.064627,-0.079742,-0.005036,0.029237,-0.014465,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.208744,0.172293,-0.049848,0.016882,-0.000119,-0.023768,0.020224,-0.006835,-1.022254,0.440618,0.043837,-0.053461,-0.022537,-0.001632,0.008464,-0.003777,1.799119,0.819386,0.196309,-0.140358,-0.066538,0.020821,0.004542,-0.004906,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.168542,0.150169,-0.039900,0.007294,-0.014607,-0.002191,0.008204,-0.003477,-0.910283,0.451115,-0.004546,-0.074889,-0.018707,0.006384,0.002043,0.000638,2.013803,0.887490,0.069508,-0.193135,-0.038938,0.021777,-0.003404,0.004399,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.133437,0.130673,-0.039841,-0.006594,-0.011556,0.005722,0.002289,-0.000765,-0.799025,0.433769,-0.066598,-0.088880,-0.008463,0.010409,0.003467,0.002599,2.236871,0.884010,-0.086652,-0.218925,-0.012502,0.022627,0.004963,0.007301,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.103400,0.108918,-0.048107,-0.013963,-0.002676,0.008190,0.001194,-0.000145,-0.696155,0.383504,-0.134549,-0.089399,0.009219,0.018875,0.008578,0.000286,2.449012,0.799338,-0.251558,-0.214736,0.024427,0.039273,0.018912,0.001228,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.079398,0.082246,-0.058236,-0.011167,0.008601,0.009661,0.001232,-0.002205,-0.610029,0.300463,-0.194683,-0.065664,0.041011,0.031497,0.010032,-0.009601,2.629907,0.635703,-0.396180,-0.159686,0.091920,0.067862,0.023115,-0.020943,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.062608,0.051763,-0.061850,0.003556,0.020627,0.008311,-0.002374,-0.007062,-0.547914,0.194030,-0.223240,-0.003145,0.084537,0.032695,-0.005775,-0.029567,2.762006,0.414841,-0.469946,-0.020992,0.186964,0.072284,-0.011336,-0.065122,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.053389,0.022931,-0.050433,0.027670,0.024928,-0.004885,-0.014840,-0.012813,-0.513049,0.087658,-0.189733,0.095979,0.103823,-0.016303,-0.057849,-0.053571,2.836811,0.188852,-0.406284,0.198704,0.231078,-0.033598,-0.125962,-0.118583,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.050288,0.004256,-0.022228,0.043156,-0.002098,-0.043825,-0.038758,-0.011377,-0.501121,0.016528,-0.085851,0.164210,-0.000087,-0.172959,-0.157517,-0.049742,2.862568,0.035812,-0.185664,0.353208,0.006142,-0.377329,-0.346405,-0.111667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/swap6v_pps/pp6.csv b/real_robot/crazyswarm/scripts/swap6v_pps/pp6.csv
new file mode 100644
index 0000000000000000000000000000000000000000..886a09163633a5a486b9e1b2684a596fed20e819
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/swap6v_pps/pp6.csv
@@ -0,0 +1,18 @@
+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,
+0.250000,-3.300000,0.000000,0.000000,0.000000,0.371339,-0.482492,0.259857,-0.057039,-2.000000,0.000000,0.000000,0.000000,0.805862,-0.977830,0.504158,-0.102597,2.000000,0.000000,0.000000,0.000000,0.048944,-0.063808,0.034422,-0.007578,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.298961,0.015210,0.077919,0.143189,-0.019353,-0.167717,0.154278,-0.056310,-1.997690,0.034047,0.176849,0.338240,0.000114,-0.357138,0.312422,-0.110472,2.000137,0.002002,0.010246,0.018785,-0.002689,-0.022138,0.020400,-0.007450,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.288256,0.077340,0.159733,0.059526,-0.115157,-0.008506,0.055590,-0.029580,-1.973119,0.180565,0.390809,0.197671,-0.213827,-0.030350,0.118413,-0.061034,2.001543,0.010153,0.020909,0.007616,-0.015311,-0.001090,0.007344,-0.003896,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.258454,0.161279,0.162515,-0.047620,-0.089851,0.037383,0.004920,-0.008324,-1.901304,0.399665,0.459822,-0.006465,-0.174130,0.069735,0.013648,-0.019904,2.005448,0.021094,0.021060,-0.006613,-0.011919,0.004989,0.000673,-0.001077,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.209035,0.228737,0.099064,-0.113707,-0.043062,0.034487,-0.008626,0.002044,-1.773359,0.618889,0.400962,-0.135466,-0.085051,0.065369,-0.019244,0.000607,2.011893,0.029739,0.012428,-0.015351,-0.005640,0.004670,-0.001073,0.000281,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.147572,0.254884,0.002561,-0.137631,-0.006922,0.024340,-0.004326,0.003758,-1.595967,0.789819,0.276567,-0.185592,-0.021048,0.037630,-0.017067,0.005867,2.019847,0.032807,-0.000528,-0.018369,-0.000655,0.003439,-0.000480,0.000444,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.085846,0.230383,-0.099632,-0.130179,0.021502,0.022460,0.002923,-0.001388,-1.384176,0.892633,0.134480,-0.187653,0.013198,0.019540,-0.006217,0.002829,2.027729,0.029123,-0.014032,-0.016964,0.003437,0.003251,0.000395,-0.000377,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.036405,0.157955,-0.185550,-0.093916,0.051558,0.024252,0.001238,-0.013657,-1.155475,0.925863,0.001436,-0.163799,0.033341,0.013431,-0.000856,-0.005079,2.033885,0.019206,-0.024943,-0.011423,0.007666,0.003235,-0.000165,-0.002197,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.009756,0.051251,-0.233071,-0.028681,0.075565,0.007090,-0.022430,0.007129,-0.926336,0.898202,-0.106966,-0.123026,0.046549,0.004799,-0.009587,-0.000668,2.036981,0.005130,-0.030185,-0.002088,0.010353,-0.000044,-0.004008,0.002234,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.011661,-0.065920,-0.226305,0.045281,0.067298,-0.018188,-0.010492,0.008675,-0.710209,0.824597,-0.181605,-0.076564,0.043195,-0.011087,-0.011082,0.008950,2.036384,-0.009728,-0.028065,0.007291,0.007762,-0.003240,-0.000195,0.000635,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.041334,-0.166778,-0.170386,0.099119,0.039471,-0.023133,0.004004,-0.000769,-0.516451,0.721872,-0.225028,-0.042538,0.023841,-0.016323,0.004092,0.003217,2.032339,-0.021971,-0.020190,0.013054,0.003877,-0.002757,0.000835,-0.000299,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.091997,-0.231349,-0.084641,0.125278,0.013888,-0.018346,0.002116,-0.004103,-0.350633,0.602583,-0.250236,-0.027180,0.009033,-0.006105,0.009542,0.000929,2.025801,-0.029426,-0.009334,0.015428,0.001050,-0.001912,0.000256,-0.000553,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.153130,-0.249665,0.011699,0.127800,-0.009305,-0.020414,-0.005673,-0.001814,-0.216020,0.472872,-0.267610,-0.018854,0.010855,0.009255,0.011446,-0.001817,2.018105,-0.031171,0.002336,0.015288,-0.001402,-0.002232,-0.000778,-0.000208,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.212876,-0.220869,0.100500,0.103715,-0.041133,-0.030683,-0.009775,0.008099,-0.114768,0.336455,-0.275600,0.001114,0.032161,0.023553,0.008982,-0.009587,2.010689,-0.027273,0.012878,0.012219,-0.005036,-0.003596,-0.001251,0.001006,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.260384,-0.154386,0.157661,0.041458,-0.084223,-0.033471,0.003380,0.028026,-0.047712,0.201370,-0.258694,0.049492,0.064780,0.023489,-0.007079,-0.024846,2.004843,-0.018933,0.019539,0.004682,-0.010154,-0.004003,0.000387,0.003409,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.288838,-0.073633,0.152713,-0.058796,-0.107565,0.009972,0.052574,0.052988,-0.012492,0.085727,-0.194537,0.123343,0.073917,-0.020824,-0.051057,-0.041814,2.001361,-0.008991,0.018709,-0.007387,-0.012931,0.001243,0.006372,0.006409,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
+0.250000,-3.299015,-0.014430,0.074005,-0.136454,-0.016833,0.158221,0.150742,0.053343,-0.001038,0.015428,-0.081413,0.162574,-0.022845,-0.151597,-0.129387,-0.033257,2.000120,-0.001757,0.009019,-0.016674,-0.001899,0.019190,0.018247,0.006419,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,
diff --git a/real_robot/crazyswarm/scripts/takeoff.csv b/real_robot/crazyswarm/scripts/takeoff.csv
new file mode 100644
index 0000000000000000000000000000000000000000..53fb2210e117cd11c7c6ead10e043d2c370f0d17
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/takeoff.csv
@@ -0,0 +1,3 @@
+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
+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
+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
diff --git a/real_robot/crazyswarm/scripts/test_collisionAvoidance.py b/real_robot/crazyswarm/scripts/test_collisionAvoidance.py
new file mode 100644
index 0000000000000000000000000000000000000000..f62024b84295f9dbaae8a693fbbfe3381bd5e355
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/test_collisionAvoidance.py
@@ -0,0 +1,316 @@
+import sys
+
+import numpy as np
+import pytest
+
+from pycrazyswarm import *
+from pycrazyswarm.util import *
+
+
+RADII = np.array([0.125, 0.125, 0.375])
+Z = 1.0
+
+
+def setUp(args):
+ crazyflies_yaml = """
+ crazyflies:
+ - id: 1
+ channel: 100
+ initialPosition: [-1.0, 0.0, 0.0]
+ - id: 2
+ channel: 100
+ initialPosition: [1.0, 0.0, 0.0]
+ """
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args=args)
+ timeHelper = swarm.timeHelper
+ return swarm.allcfs, timeHelper
+
+
+def test_velocityMode_sidestepWorstCase(args=None):
+ if args is None:
+ args = "--sim --vis null --dt 0.05 --maxvel 1.0"
+
+ allcfs, timeHelper = setUp(args)
+ a, b = allcfs.crazyflies
+
+ a.enableCollisionAvoidance([b], RADII)
+ b.enableCollisionAvoidance([a], RADII)
+
+ a.cmdVelocityWorld([1.0, 0.0, 0.0], yawRate=0.0)
+ b.cmdVelocityWorld([-1.0, 0.0, 0.0], yawRate=0.0)
+
+ while timeHelper.time() < 10.0:
+ positions = np.stack([a.position(), b.position()])
+ collisions = check_ellipsoid_collisions(positions, RADII)
+ assert not np.any(collisions)
+
+ timeHelper.sleep(timeHelper.dt)
+ if a.position()[0] > 1.0 and b.position()[0] < -1.0:
+ return
+ assert False
+
+
+def test_goToWithoutCA_CheckCollision():
+ args = "--sim --vis null"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+
+ cf0.goTo([1.0, 0.0, 1.0], 0, 5.0)
+ cf1.goTo([-1.0, 0.0, 1.0], 0, 5.0)
+ while timeHelper.time() < 10.0:
+ positions = np.stack([cf0.position(), cf1.position()])
+ collisions = check_ellipsoid_collisions(positions, RADII)
+ if np.any(collisions):
+ return
+ timeHelper.sleep(timeHelper.dt)
+ assert False
+
+
+def test_goToWithCA_CheckCollision():
+ args = "--sim --vis null"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+ cf0.enableCollisionAvoidance([cf1], RADII)
+ cf1.enableCollisionAvoidance([cf0], RADII)
+
+ cf0.goTo([1.0, 0.0, 1.0], 0, 5.0)
+ cf1.goTo([-1.0, 0.0, 1.0], 0, 5.0)
+ while timeHelper.time() < 10.0:
+ positions = np.stack([cf0.position(), cf1.position()])
+ collisions = check_ellipsoid_collisions(positions, RADII)
+ assert not np.any(collisions)
+ timeHelper.sleep(timeHelper.dt)
+
+
+def test_goToWithCA_CheckDestination():
+ args = "--sim --vis null"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+ cf0.enableCollisionAvoidance([cf1], RADII)
+ cf1.enableCollisionAvoidance([cf0], RADII)
+
+ goal0 = [1.0, 0.0, 1.0]
+ goal1 = [-1.0, 0.0, 1.0]
+ cf0.goTo(goal0, 0, 5.0)
+ cf1.goTo(goal1, 0, 5.0)
+ timeHelper.sleep(5)
+ assert np.all(np.isclose(cf0.position(), goal0))
+ assert np.all(np.isclose(cf1.position(), goal1))
+
+
+def test_goToWithCA_changeEllipsoid():
+ args = "--sim --vis null"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+
+ newRADII = np.array([0.1, 0.1, 0.3])
+ cf0.enableCollisionAvoidance([cf1], newRADII)
+ cf1.enableCollisionAvoidance([cf0], newRADII)
+
+ goal0 = [1.0, 0.0, 1.0]
+ goal1 = [-1.0, 0.0, 1.0]
+ cf0.goTo(goal0, 0, 5.0)
+ cf1.goTo(goal1, 0, 5.0)
+
+ # flag if a collision happened during sleep
+ collisionHappend = False
+ while timeHelper.time() < 10.0:
+ positions = np.stack([cf0.position(), cf1.position()])
+ collisionsOld = check_ellipsoid_collisions(positions, RADII)
+ collisionsNew = check_ellipsoid_collisions(positions, newRADII)
+ if np.any(collisionsOld):
+ collisionHappend = True
+ assert not np.any(collisionsNew)
+
+ timeHelper.sleep(timeHelper.dt)
+ assert collisionHappend
+
+
+def test_goToWithCA_Intersection():
+ args = "--sim --vis null --dt 0.01"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+ cf0.enableCollisionAvoidance([cf1], RADII)
+ cf1.enableCollisionAvoidance([cf0], RADII)
+
+ goal0 = [1.0, 1.0, 1.0]
+ goal1 = [-1.0, 1.0, 1.0]
+ cf0.goTo(goal0, 0, 5.0)
+ cf1.goTo(goal1, 0, 5.0)
+
+ while timeHelper.time() < 10.0:
+ positions = np.stack([cf0.position(), cf1.position()])
+ collisions = check_ellipsoid_collisions(positions, RADII)
+ assert not np.any(collisions)
+
+ timeHelper.sleep(timeHelper.dt)
+ assert np.all(np.isclose(cf0.position(), goal0))
+ assert np.all(np.isclose(cf1.position(), goal1))
+
+
+def test_goToWithoutCA_Intersection():
+ args = "--sim --vis null --dt 0.05"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+
+ goal0 = [1.0, 1.0, 1.0]
+ goal1 = [-1.0, 1.0, 1.0]
+ cf0.goTo(goal0, 0, 5.0)
+ cf1.goTo(goal1, 0, 5.0)
+
+ while timeHelper.time() < 10.0:
+ positions = np.stack([cf0.position(), cf1.position()])
+ collisions = check_ellipsoid_collisions(positions, RADII)
+ if np.any(collisions):
+ return
+
+ timeHelper.sleep(timeHelper.dt)
+ assert False
+
+
+def test_goToWithCA_random():
+ rows, cols = 3, 5
+ N = rows * cols
+ crazyflies_yaml = grid_yaml(rows, cols, spacing=0.5)
+
+ args = "--sim --vis null"
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args=args)
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+
+ cfs = allcfs.crazyflies
+
+ for _, cf in enumerate(cfs):
+ cf.enableCollisionAvoidance(cfs, RADII)
+
+ np.random.seed(0)
+ xy_radius = 0.125
+ for _ in range(5):
+ lastTime = timeHelper.time()
+ goals = poisson_disk_sample(N, dim=2, mindist=5*xy_radius)
+ goals_z = Z * np.ones(N) + 0.2 * np.random.uniform(-1.0, 1.0, size=N)
+ goals = np.hstack([goals, goals_z[:, None]])
+
+ for goal, cf in zip(goals, cfs):
+ cf.goTo(goal, yaw=0.0, duration=5)
+ while timeHelper.time() - lastTime < 20.0:
+ positions = np.stack([cf.position() for cf in cfs])
+ timeHelper.sleep(timeHelper.dt)
+ collisions = check_ellipsoid_collisions(positions, RADII)
+ assert not np.any(collisions)
+ for goal, cf in zip(goals, cfs):
+ assert np.all(np.isclose(cf.position(), goal, atol=1e-4))
+
+
+def test_cmdPosition():
+ args = "--sim --vis null --maxvel 1.0"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+ cf0.enableCollisionAvoidance([cf1], RADII)
+ cf1.enableCollisionAvoidance([cf0], RADII)
+
+ goal0 = np.array([1.0, 0.0, 2.0])
+ goal1 = np.array([-1.0, 0.0, 2.0])
+ cf0.cmdPosition(goal0, yaw=0.0)
+ cf1.cmdPosition(goal1, yaw=0.0)
+ while timeHelper.time() < 10.0:
+ positions = np.stack([cf0.position(), cf1.position()])
+ collisions = check_ellipsoid_collisions(positions, RADII)
+ assert not np.any(collisions)
+
+ timeHelper.sleep(timeHelper.dt)
+ assert np.all(np.isclose(cf0.position(), goal0))
+ assert np.all(np.isclose(cf1.position(), goal1))
+
+
+def test_boundingBox():
+ args = "--sim --vis null --maxvel 1.0"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+ BBOXMAX = [5, 5, 5]
+ BBOXMIN = [-3, -3, -3]
+ cf0.enableCollisionAvoidance(
+ [cf1], RADII, bboxMax=BBOXMAX, bboxMin=BBOXMIN)
+ cf1.enableCollisionAvoidance(
+ [cf0], RADII, bboxMax=BBOXMAX, bboxMin=BBOXMIN)
+
+ goal0 = np.array([BBOXMAX[0] - 1, 0.0, Z])
+ goal1 = np.array([BBOXMIN[0] - 1, 0.0, Z])
+ cf0.cmdPosition(goal0, yaw=0.0)
+ cf1.cmdPosition(goal1, yaw=0.0)
+ while timeHelper.time() < 10.0:
+ assert np.all(cf1.position() >= BBOXMIN)
+ assert np.all(cf1.position() <= BBOXMAX)
+ timeHelper.sleep(timeHelper.dt)
+
+ assert np.all(np.isclose(cf0.position(), goal0))
+ assert not np.all(np.isclose(cf1.position(), goal1))
+
+
+#@pytest.mark.xfail(reason="Bug in firmware")
+def test_maxSpeed_zero():
+ args = "--sim --vis null --maxvel 1.0"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+ cf0.enableCollisionAvoidance([cf1], RADII, maxSpeed=0.0)
+ cf1.enableCollisionAvoidance([cf0], RADII, maxSpeed=0.0)
+
+ goal0 = np.array([1.0, 0.0, Z])
+ goal1 = np.array([-1.0, 0.0, Z])
+ cf0.cmdPosition(goal0, yaw=0.0)
+ cf1.cmdPosition(goal1, yaw=0.0)
+ while timeHelper.time() < 10.0:
+ timeHelper.sleep(timeHelper.dt)
+ assert np.all(cf0.velocity() == np.zeros(3))
+ assert np.all(cf1.velocity() == np.zeros(3))
+
+ assert np.all(np.isclose(cf0.position(), goal1))
+ assert np.all(np.isclose(cf1.position(), goal0))
+
+
+def test_maxSpeed_limit():
+ args = "--sim --vis null --maxvel 1.0"
+ allcfs, timeHelper = setUp(args)
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf0, cf1 = allcfs.crazyflies
+
+ cf0.enableCollisionAvoidance([cf1], RADII, maxSpeed=1.0)
+ cf1.enableCollisionAvoidance([cf0], RADII, maxSpeed=3.0)
+
+ cf0.cmdVelocityWorld([2.0, 0.0, 0.0], yawRate=0.0)
+ cf1.cmdVelocityWorld([-2.0, 0.0, 0.0], yawRate=0.0)
+
+ while timeHelper.time() < 10.0:
+ assert np.all(np.abs(cf0.velocity()) < 1.2 * np.ones(3))
+ assert np.all(np.abs(cf1.velocity()) < 3.2 * np.ones(3))
+ timeHelper.sleep(timeHelper.dt)
+ if cf0.position()[0] > 2.0 and cf1.position()[0] < -2.0:
+ return
+
+ assert False
+
+
+if __name__ == "__main__":
+ # test_velocityMode_sidestepWorstCase(sys.argv)
+ test_maxSpeed_limit()
diff --git a/real_robot/crazyswarm/scripts/test_highLevel.py b/real_robot/crazyswarm/scripts/test_highLevel.py
new file mode 100644
index 0000000000000000000000000000000000000000..2946222227a5bcce57bbafdc3994fdb1114f44b0
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/test_highLevel.py
@@ -0,0 +1,178 @@
+#!/usr/bin/env python
+
+import numpy as np
+from pycrazyswarm import *
+import uav_trajectory
+
+Z = 1.0
+
+def setUp():
+ crazyflies_yaml = """
+ crazyflies:
+ - channel: 100
+ id: 1
+ initialPosition: [1.0, 0.0, 0.0]
+ - channel: 100
+ id: 10
+ initialPosition: [0.0, -1.0, 0.0]
+ """
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args="--sim --vis null")
+ timeHelper = swarm.timeHelper
+ return swarm.allcfs, timeHelper
+
+def _collectRelativePositions(timeHelper, cf, duration):
+ t0 = timeHelper.time()
+ positions = []
+ while timeHelper.time() - t0 < duration:
+ positions.append(cf.position() - cf.initialPosition)
+ timeHelper.sleep(timeHelper.dt + 1e-6)
+ return np.stack(positions)
+
+
+def test_takeOff():
+ allcfs, timeHelper = setUp()
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+
+ for cf in allcfs.crazyflies:
+ pos = cf.initialPosition + np.array([0, 0, Z])
+ assert np.all(np.isclose(cf.position(), pos, atol=0.0001))
+
+def test_goTo_nonRelative():
+ allcfs, timeHelper = setUp()
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+
+ for cf in allcfs.crazyflies:
+ pos = np.array(cf.initialPosition) + np.array([1, 1, Z])
+ cf.goTo(pos, 0, 1.0)
+ timeHelper.sleep(1.0)
+
+ for cf in allcfs.crazyflies:
+ pos = cf.initialPosition + np.array([1, 1, Z])
+ assert np.all(np.isclose(cf.position(), pos))
+
+def test_goTo_relative():
+ allcfs, timeHelper = setUp()
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+
+ allcfs.goTo(np.array([1.0,1.0,1.0]), 0, Z)
+ timeHelper.sleep(2.0)
+
+ for cf in allcfs.crazyflies:
+ pos = cf.initialPosition + np.array([1.0,1.0,2*Z])
+ assert np.all(np.isclose(cf.position(), pos))
+
+def test_landing():
+ allcfs, timeHelper = setUp()
+ allcfs.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+
+ allcfs.land(targetHeight=0.02, duration=1.0+Z)
+ timeHelper.sleep(1.0+Z)
+
+ for cf in allcfs.crazyflies:
+ pos = cf.initialPosition + np.array([0, 0, 0.02])
+ assert np.all(np.isclose(cf.position(), pos, atol=0.0001))
+
+def test_uploadTrajectory_timescale():
+ allcfs, timeHelper = setUp()
+ cf = allcfs.crazyflies[0]
+
+ traj = uav_trajectory.Trajectory()
+ traj.loadcsv("figure8.csv")
+ trajId = 100
+ cf.uploadTrajectory(trajectoryId=trajId, pieceOffset=0, trajectory=traj)
+
+ # We know the traj isn't close to origin at 3/4 of its duration
+ cf.startTrajectory(trajId)
+ timeHelper.sleep(0.75 * traj.duration)
+ assert np.linalg.norm(cf.position() - cf.initialPosition) >= 0.5
+
+ # Make sure we're back at origin
+ timeHelper.sleep(traj.duration)
+
+ # Speeding up time by 2x, we should finish the trajectory in less time
+ cf.startTrajectory(trajId, timescale=0.5)
+ timeHelper.sleep(0.75 * traj.duration)
+ assert np.linalg.norm(cf.position() - cf.initialPosition) <= 0.001
+
+def test_uploadTrajectory_fig8Bounds():
+ allcfs, timeHelper = setUp()
+ cf = allcfs.crazyflies[0]
+
+ traj = uav_trajectory.Trajectory()
+ traj.loadcsv("figure8.csv")
+
+ trajId = 100
+ cf.uploadTrajectory(trajectoryId=trajId, pieceOffset=0, trajectory=traj)
+ cf.startTrajectory(trajId)
+ positions = _collectRelativePositions(timeHelper, cf, traj.duration)
+
+ # We know the approximate range the bounding box should lie in by plotting
+ # the trajectory.
+ xs, ys, _ = positions.T
+ assert 0.9 < np.amax(xs) < 1.1
+ assert -0.9 > np.amin(xs) > -1.1
+ assert 0.4 < np.amax(ys) < 0.6
+ assert -0.4 > np.amin(ys) > -0.6
+
+def test_uploadTrajectory_reverse():
+ allcfs, timeHelper = setUp()
+ cf = allcfs.crazyflies[0]
+
+ traj = uav_trajectory.Trajectory()
+ traj.loadcsv("figure8.csv")
+ trajId = 100
+ cf.uploadTrajectory(trajectoryId=trajId, pieceOffset=0, trajectory=traj)
+
+ cf.startTrajectory(trajId)
+ positions = _collectRelativePositions(timeHelper, cf, traj.duration)
+
+ cf.startTrajectory(trajId, reverse=True)
+ positionsReverse = _collectRelativePositions(timeHelper, cf, traj.duration)
+ positions2 = np.flipud(positionsReverse)
+
+ # The distance threshold must be large because the trajectory is not
+ # symmetrical, not because time/reversing is super sloppy.
+ dists = np.linalg.norm(positions - positions2, axis=1)
+ assert not np.any(dists > 0.2)
+
+def test_uploadTrajectory_broadcast():
+ allcfs, timeHelper = setUp()
+ cf0, cf1 = allcfs.crazyflies
+
+ relativeInitial = cf1.initialPosition - cf0.initialPosition
+
+ traj = uav_trajectory.Trajectory()
+ traj.loadcsv("figure8.csv")
+ trajId = 100
+ for cf in (cf0, cf1):
+ cf.uploadTrajectory(trajectoryId=trajId, pieceOffset=0, trajectory=traj)
+
+ allcfs.startTrajectory(trajId)
+ t0 = timeHelper.time()
+ while timeHelper.time() - t0 < traj.duration:
+ relative = cf1.position() - cf0.position()
+ assert np.all(np.isclose(relativeInitial, relative))
+ timeHelper.sleep(timeHelper.dt + 1e-6)
+
+def test_setGroupMask():
+ allcfs, timeHelper = setUp()
+ cf0, cf1 = allcfs.crazyflies
+ cf0.setGroupMask(1)
+ cf1.setGroupMask(2)
+ allcfs.takeoff(targetHeight=Z, duration=1.0 + Z, groupMask = 1)
+ timeHelper.sleep(1.5+Z)
+
+ pos0 = cf0.initialPosition + np.array([0, 0, Z])
+ assert np.all(np.isclose(cf0.position(), pos0, atol=0.0001))
+ assert np.all(np.isclose(cf1.position(), cf1.initialPosition, atol=0.0001))
+
+ allcfs.takeoff(targetHeight=Z, duration=1.0 + Z, groupMask = 2)
+ timeHelper.sleep(1.5+Z)
+
+ pos1 = cf1.initialPosition + np.array([0, 0, Z])
+ assert np.all(np.isclose(cf0.position(), pos0, atol=0.0001))
+ assert np.all(np.isclose(cf1.position(), pos1, atol=0.0001))
\ No newline at end of file
diff --git a/real_robot/crazyswarm/scripts/test_lowLevel.py b/real_robot/crazyswarm/scripts/test_lowLevel.py
new file mode 100644
index 0000000000000000000000000000000000000000..7ae2b73699ee2f0a3ceb2c979279eebbc3544b15
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/test_lowLevel.py
@@ -0,0 +1,105 @@
+#!/usr/bin/env python
+
+import numpy as np
+from pycrazyswarm import *
+
+Z = 1.0
+
+def setUp(extra_args=""):
+ crazyflies_yaml = """
+ crazyflies:
+ - channel: 100
+ id: 1
+ initialPosition: [1.0, 0.0, 0.0]
+ """
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args="--sim --vis null " + extra_args)
+ timeHelper = swarm.timeHelper
+ return swarm.allcfs, timeHelper
+
+def test_cmdFullState_zeroVel():
+ allcfs, timeHelper = setUp()
+ cf = allcfs.crazyflies[0]
+
+ pos = np.array(cf.initialPosition) + np.array([1, 1, Z])
+ cf.cmdFullState(pos, np.zeros(3), np.zeros(3), 0, np.zeros(3))
+ timeHelper.sleep(1.0)
+
+ assert np.all(np.isclose(cf.position(), pos))
+
+def test_cmdPosition():
+ allcfs, timeHelper = setUp()
+ cf = allcfs.crazyflies[0]
+
+ pos = np.array(cf.initialPosition) + np.array([1, 1, Z])
+ cf.cmdPosition(pos,yaw=0.0)
+ timeHelper.sleep(1.0)
+
+ assert np.all(np.isclose(cf.position(), pos))
+
+def test_cmdVelocityWorld_checkVelocity():
+ allcfs, timeHelper = setUp()
+
+ cf = allcfs.crazyflies[0]
+ vel = np.ones(3)
+ cf.cmdVelocityWorld(vel, yawRate=0)
+ timeHelper.sleep(1.0)
+
+ assert np.all(np.isclose(cf.velocity(), vel))
+
+def test_cmdVelocityWorld_checkIntegrate():
+ allcfs, timeHelper = setUp()
+
+ cf = allcfs.crazyflies[0]
+ vel = np.ones(3)
+ cf.cmdVelocityWorld(vel, yawRate=0)
+ timeHelper.sleep(1.0)
+
+ pos = cf.initialPosition + vel
+ assert np.all(np.isclose(cf.position(), pos))
+
+def test_cmdVelocityWorld_disturbance():
+ crazyflies_yaml = """
+ crazyflies:
+ - channel: 100
+ id: 1
+ initialPosition: [1.0, 0.0, 0.0]
+ """
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args="--sim --vis null --disturbance 1.0")
+ timeHelper = swarm.timeHelper
+
+ cf = swarm.allcfs.crazyflies[0]
+
+ vel = np.ones(3)
+ cf.cmdVelocityWorld(vel, yawRate=0)
+ timeHelper.sleep(1.0)
+
+ pos = cf.initialPosition + vel
+ assert not np.any(np.isclose(cf.position(), pos))
+
+def test_sleepResidual():
+ """Verify TimeHelper's time() is consistent with its integration steps."""
+ np.random.seed(0)
+ TRIALS = 100
+ for _ in range(TRIALS):
+ dtTick = 10 ** np.random.uniform(-2, 0)
+ dtSleep = 10 ** np.random.uniform(-2, 0)
+ allcfs, timeHelper = setUp("--dt {}".format(dtTick))
+
+ cf = allcfs.crazyflies[0]
+ vel = np.ones(3)
+ cf.cmdVelocityWorld(vel, yawRate=0)
+ time = 0.0
+ while timeHelper.time() < 1.0:
+ timeHelper.sleep(dtSleep)
+ time += dtSleep
+
+ assert time >= timeHelper.time()
+
+ # We don't expect them to be exactly the same because timeHelper.time()
+ # will always be an integer multiple of dtTick. However, we should not
+ # be off by more than a tick.
+ assert time - timeHelper.time() < dtTick
+
+ pos = cf.initialPosition + timeHelper.time() * vel
+ assert np.all(np.isclose(cf.position(), pos))
+
diff --git a/real_robot/crazyswarm/scripts/test_ros.py b/real_robot/crazyswarm/scripts/test_ros.py
new file mode 100644
index 0000000000000000000000000000000000000000..f907fde05e079c78cfd8f42686d2b654d10a214f
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/test_ros.py
@@ -0,0 +1,6 @@
+import pytest
+
+
+@pytest.mark.ros
+def test_ros_import():
+ from pycrazyswarm.crazyflie import TimeHelper, CrazyflieServer
diff --git a/real_robot/crazyswarm/scripts/test_simOnly.py b/real_robot/crazyswarm/scripts/test_simOnly.py
new file mode 100644
index 0000000000000000000000000000000000000000..5ec386ef70e62892a4bbe13e7220774aae2f3c84
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/test_simOnly.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+
+"""Tests for simulation-only functionality."""
+
+import numpy as np
+
+from pycrazyswarm import *
+
+
+def setUp():
+ crazyflies_yaml = """
+ crazyflies:
+ - channel: 100
+ id: 1
+ initialPosition: [1.0, 0.0, 0.0]
+ """
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args="--sim --vis null")
+ timeHelper = swarm.timeHelper
+ return swarm.allcfs, timeHelper
+
+
+def test_attitudeRPY():
+ """Checks differential flatness and roll/pitch/yaw calculations."""
+
+ end = 0.99 * np.pi # Not trying to deal with wrapping here.
+ yaws = np.linspace(-end, end, 11)
+
+ for yaw in yaws:
+ allcfs, timeHelper = setUp()
+ cf = allcfs.crazyflies[0]
+ Z = 1.0
+
+ cf.takeoff(targetHeight=Z, duration=1.0+Z)
+ timeHelper.sleep(1.5+Z)
+ cf.goTo(np.zeros(3), yaw=yaw, duration=1.0, relative=True)
+ timeHelper.sleep(1.5)
+
+ c = np.cos(yaw)
+ s = np.sin(yaw)
+ Ryaw = np.array([
+ [c, -s, 0],
+ [s, c, 0],
+ [0, 0, 1],
+ ])
+ forward, left, up = Ryaw.T
+
+ dirAngleSigns = [
+ ( forward, 1, 1),
+ (-forward, 1, -1),
+ ( left, 0, -1),
+ ( -left, 0, 1),
+ ]
+
+ for direction, angleIdx, sign in dirAngleSigns:
+ cf.goTo(direction, yaw=0.0, duration=1.0, relative=True)
+ timeHelper.sleep(0.25)
+ rpy = cf.rpy()
+ assert rpy[angleIdx] * sign > np.radians(20)
+ assert np.abs(rpy[1 ^ angleIdx]) < np.radians(0.001)
+ assert np.abs(rpy[2] - yaw) < np.radians(0.001)
+ timeHelper.sleep(1.0)
diff --git a/real_robot/crazyswarm/scripts/test_videoOutput.py b/real_robot/crazyswarm/scripts/test_videoOutput.py
new file mode 100644
index 0000000000000000000000000000000000000000..2a0c69509e5ce37f5006bf9b68d32e9ddf04a5ee
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/test_videoOutput.py
@@ -0,0 +1,85 @@
+"""Test video output in simulation.
+
+This test is a bit strange: due to the following conditions
+
+- Video capture runs until the script process stops,
+- Video capture uses a child ffmpeg process to do compression,
+- We register an `atexit` function to ensure that the pipe is flushed and
+ ffmpeg process is closed cleanly,
+
+we must run the video-generating script in a separate process.
+
+Using `multiprocessing` would be easier, but `multiprocessing` processes don't
+run `atexit` handlers when they exit. This design is controversial [1, 2].
+Therefore, we must do a full `system()`-style process spawn with `subprocess`
+instead. To avoid adding another script just to support this test, this script
+will behave as the video generator process when run as `__main__()`, and behave
+as the test process when run via pytest.
+
+[1] https://stackoverflow.com/questions/2546276/python-process-wont-call-atexit
+[2] https://stackoverflow.com/questions/34506638/how-to-register-atexit-function-in-pythons-multiprocessing-subprocess
+"""
+
+import os
+import subprocess
+import sys
+
+import numpy as np
+import pytest
+
+from pycrazyswarm import *
+
+crazyflies_yaml = """
+crazyflies:
+- id: 1
+ channel: 110
+ initialPosition: [0.0, 0.0, 0.0]
+"""
+
+Z = 1.0
+FPS = 12
+DT = 1.0 / FPS
+TOTAL_TIME = 4.0
+
+
+try:
+ import vispy
+ import ffmpeg
+ HAS_DEPENDENCIES = True
+# For some reason the vispy import fails in Github CI for MacOS Py2.7 with a
+# ValueError, even though we install vispy. Strange but doesn't matter.
+except (ImportError, ValueError):
+ HAS_DEPENDENCIES = False
+
+
+def videoWriterProcess(path):
+ args = "--sim --vis vispy --dt {} --video {}".format(DT, path)
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args=args)
+ timeHelper = swarm.timeHelper
+ cf = swarm.allcfs.crazyflies[0]
+
+ cf.takeoff(targetHeight=Z, duration=TOTAL_TIME / 2)
+ timeHelper.sleep(TOTAL_TIME / 2)
+
+ cf.goTo(cf.initialPosition + np.array([0.0, 1.0, Z]), yaw=0.0, duration=TOTAL_TIME / 2)
+ timeHelper.sleep(TOTAL_TIME / 2)
+
+
+@pytest.mark.skipif(not HAS_DEPENDENCIES, reason="vispy and ffmpeg required for video.")
+def test_videoOutput(tmp_path):
+ # tmp_path is supplied by pytest - a directory where we can write that will
+ # eventually be deleted.
+ path = str(tmp_path / "crazyswarm_test_video.mp4")
+ subprocess.call([os.environ["CSW_PYTHON"], __file__, path])
+
+ import ffmpeg
+ properties = ffmpeg.probe(path)
+ stream = properties["streams"][0]
+ file_duration = float(stream["duration"])
+ file_fps = int(stream["avg_frame_rate"].split("/")[0])
+ assert file_fps == FPS
+ assert abs(file_duration - TOTAL_TIME) <= DT
+
+
+if __name__ == "__main__":
+ videoWriterProcess(sys.argv[1])
diff --git a/real_robot/crazyswarm/scripts/test_yamlString.py b/real_robot/crazyswarm/scripts/test_yamlString.py
new file mode 100644
index 0000000000000000000000000000000000000000..d01c8842a1996444c336e054f9a40224eaa08cd1
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/test_yamlString.py
@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+
+import numpy as np
+from pycrazyswarm import *
+
+def test_yaml_string_load():
+
+ crazyflies_yaml = """
+ crazyflies:
+ - channel: 100
+ id: 1
+ initialPosition: [1.0, 0.0, 0.0]
+ - channel: 100
+ id: 10
+ initialPosition: [0.0, -1.0, 0.0]
+ """
+
+ swarm = Crazyswarm(crazyflies_yaml=crazyflies_yaml, args="--sim --vis null")
+ timeHelper = swarm.timeHelper
+ cfs = swarm.allcfs.crazyflies
+ byId = swarm.allcfs.crazyfliesById
+
+ assert len(cfs) == 2
+ cf1 = byId[1]
+ assert np.all(cf1.initialPosition == [1.0, 0.0, 0.0])
+
+ cf10 = byId[10]
+ assert np.all(cf10.initialPosition == [0.0, -1.0, 0.0])
diff --git a/real_robot/crazyswarm/scripts/uav_trajectory.py b/real_robot/crazyswarm/scripts/uav_trajectory.py
new file mode 100644
index 0000000000000000000000000000000000000000..91939450acca30456d7a0f3534a85480fbb8dc7b
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/uav_trajectory.py
@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+
+import numpy as np
+
+def normalize(v):
+ norm = np.linalg.norm(v)
+ assert norm > 0
+ return v / norm
+
+
+class Polynomial:
+ def __init__(self, p):
+ self.p = p
+
+ # evaluate a polynomial using horner's rule
+ def eval(self, t):
+ assert t >= 0
+ x = 0.0
+ for i in range(0, len(self.p)):
+ x = x * t + self.p[len(self.p) - 1 - i]
+ return x
+
+ # compute and return derivative
+ def derivative(self):
+ return Polynomial([(i+1) * self.p[i+1] for i in range(0, len(self.p) - 1)])
+
+
+class TrajectoryOutput:
+ def __init__(self):
+ self.pos = None # position [m]
+ self.vel = None # velocity [m/s]
+ self.acc = None # acceleration [m/s^2]
+ self.omega = None # angular velocity [rad/s]
+ self.yaw = None # yaw angle [rad]
+
+
+# 4d single polynomial piece for x-y-z-yaw, includes duration.
+class Polynomial4D:
+ def __init__(self, duration, px, py, pz, pyaw):
+ self.duration = duration
+ self.px = Polynomial(px)
+ self.py = Polynomial(py)
+ self.pz = Polynomial(pz)
+ self.pyaw = Polynomial(pyaw)
+
+ # compute and return derivative
+ def derivative(self):
+ return Polynomial4D(
+ self.duration,
+ self.px.derivative().p,
+ self.py.derivative().p,
+ self.pz.derivative().p,
+ self.pyaw.derivative().p)
+
+ def eval(self, t):
+ result = TrajectoryOutput()
+ # flat variables
+ result.pos = np.array([self.px.eval(t), self.py.eval(t), self.pz.eval(t)])
+ result.yaw = self.pyaw.eval(t)
+
+ # 1st derivative
+ derivative = self.derivative()
+ result.vel = np.array([derivative.px.eval(t), derivative.py.eval(t), derivative.pz.eval(t)])
+ dyaw = derivative.pyaw.eval(t)
+
+ # 2nd derivative
+ derivative2 = derivative.derivative()
+ result.acc = np.array([derivative2.px.eval(t), derivative2.py.eval(t), derivative2.pz.eval(t)])
+
+ # 3rd derivative
+ derivative3 = derivative2.derivative()
+ jerk = np.array([derivative3.px.eval(t), derivative3.py.eval(t), derivative3.pz.eval(t)])
+
+ thrust = result.acc + np.array([0, 0, 9.81]) # add gravity
+
+ z_body = normalize(thrust)
+ x_world = np.array([np.cos(result.yaw), np.sin(result.yaw), 0])
+ y_body = normalize(np.cross(z_body, x_world))
+ x_body = np.cross(y_body, z_body)
+
+ jerk_orth_zbody = jerk - (np.dot(jerk, z_body) * z_body)
+ h_w = jerk_orth_zbody / np.linalg.norm(thrust)
+
+ result.omega = np.array([-np.dot(h_w, y_body), np.dot(h_w, x_body), z_body[2] * dyaw])
+ return result
+
+
+class Trajectory:
+ def __init__(self):
+ self.polynomials = None
+ self.duration = None
+
+ def n_pieces(self):
+ return len(self.polynomials)
+
+ def loadcsv(self, filename):
+ data = np.loadtxt(filename, delimiter=",", skiprows=1, usecols=range(33))
+ self.polynomials = [Polynomial4D(row[0], row[1:9], row[9:17], row[17:25], row[25:33]) for row in data]
+ self.duration = np.sum(data[:,0])
+
+ def eval(self, t):
+ assert t >= 0
+ assert t <= self.duration
+
+ current_t = 0.0
+ for p in self.polynomials:
+ if t <= current_t + p.duration:
+ return p.eval(t - current_t)
+ current_t = current_t + p.duration
diff --git a/real_robot/crazyswarm/scripts/uav_trajectory.pyc b/real_robot/crazyswarm/scripts/uav_trajectory.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..f2b6c8cb7fcfe825445bd76d160fb5a772d6a5a4
Binary files /dev/null and b/real_robot/crazyswarm/scripts/uav_trajectory.pyc differ
diff --git a/real_robot/crazyswarm/scripts/udp_multicast.py b/real_robot/crazyswarm/scripts/udp_multicast.py
new file mode 100644
index 0000000000000000000000000000000000000000..c39c64642c0f5640bd91c055d8c65a073b4527ca
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/udp_multicast.py
@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+
+import socket
+import struct
+
+# see https://stackoverflow.com/questions/603852/multicast-in-python
+class UdpMulticastSender:
+ def __init__(self, MCAST_GRP = '224.1.1.1', MCAST_PORT = 5007):
+ self.MCAST_GRP = MCAST_GRP
+ self.MCAST_PORT = MCAST_PORT
+ self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+ self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 2)
+
+ def send(self, msg):
+ self.sock.sendto(msg, (self.MCAST_GRP, self.MCAST_PORT))
+
+
+class UdpMulticastReceiver:
+ def __init__(self, MCAST_GRP = '224.1.1.1', MCAST_PORT = 5007):
+ self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
+ self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ self.sock.bind(('', MCAST_PORT)) # use MCAST_GRP instead of '' to listen only
+ # to MCAST_GRP, not all groups on MCAST_PORT
+ mreq = struct.pack("4sl", socket.inet_aton(MCAST_GRP), socket.INADDR_ANY)
+ self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
+
+ def recv(self, bufsize=4096):
+ return self.sock.recv(bufsize)
diff --git a/real_robot/crazyswarm/scripts/waypoints.csv b/real_robot/crazyswarm/scripts/waypoints.csv
new file mode 100644
index 0000000000000000000000000000000000000000..a71053563bd7203d5d4241ff20dc38a8291fed01
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/waypoints.csv
@@ -0,0 +1,9 @@
+id,x[m],y[m],z[m],t[s]
+1,0,0,1,0
+1,0,1,1,2
+1,0,0,2,4
+2,1,0,1,0
+2,1,1,2,4
+3,1,1,1,0
+3,1,1,3,2
+3,1,2,2,4
diff --git a/real_robot/crazyswarm/scripts/waypoints.py b/real_robot/crazyswarm/scripts/waypoints.py
new file mode 100644
index 0000000000000000000000000000000000000000..aee5fb4ad276021590ec75f9ae584f4151730168
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/waypoints.py
@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+
+import numpy as np
+from pycrazyswarm import *
+
+class Waypoint:
+ def __init__(self, agent, x, y, z, arrival, duration):
+ self.agent = agent
+ self.x = x
+ self.y = y
+ self.z = z
+ self.arrival = arrival
+ self.duration = duration
+
+ def __lt__(self, other):
+ return self.arrival < other.arrival
+
+ def __repr__(self):
+ return "Ag {} at {} s. [{}, {}, {}]".format(self.agent, self.arrival, self.x, self.y, self.z)
+
+
+if __name__ == "__main__":
+
+ # load csv file
+ data = np.loadtxt("waypoints.csv", skiprows=1, delimiter=',')
+
+ # sort by agents
+ data[data[:,0].argsort()]
+
+ # convert to internal data structure
+ waypoints = []
+ lastAgent = None
+ for row in data:
+ if lastAgent is None or lastAgent != row[0]:
+ lastTime = 0.0
+ waypoints.append(Waypoint(
+ int(row[0]),
+ row[1],
+ row[2],
+ row[3],
+ row[4],
+ row[4] - lastTime))
+ lastTime = row[4]
+ lastAgent = int(row[0])
+
+ # sort waypoints by arrival time
+ waypoints.sort()
+
+ # print waypoints
+ print(waypoints)
+
+ # execute waypoints
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ allcfs = swarm.allcfs
+
+ allcfs.takeoff(targetHeight=1.0, duration=2.0)
+ timeHelper.sleep(2.0)
+ lastTime = 0.0
+ for waypoint in waypoints:
+ if waypoint.arrival == 0:
+ pos = [waypoint.x, waypoint.y, waypoint.z]
+ # print(waypoint.agent, pos, 2.0)
+ cf = allcfs.crazyfliesById[waypoint.agent]
+ cf.goTo(pos, 0, 2.0)
+ elif waypoint.duration > 0:
+ timeHelper.sleep(waypoint.arrival - lastTime)
+ lastTime = waypoint.arrival
+ pos = [waypoint.x, waypoint.y, waypoint.z]
+ # print(waypoint.agent, pos, waypoint.duration)
+ cf = allcfs.crazyfliesById[waypoint.agent]
+ cf.goTo(pos, 0, waypoint.duration)
+
+ # land
+ allcfs.land(targetHeight=0.02, duration=2.0)
+ timeHelper.sleep(2.0)
diff --git a/real_robot/crazyswarm/scripts/waypoints_simple.py b/real_robot/crazyswarm/scripts/waypoints_simple.py
new file mode 100644
index 0000000000000000000000000000000000000000..f011e94a754b4874123cb77d19aee10c1b4859f8
--- /dev/null
+++ b/real_robot/crazyswarm/scripts/waypoints_simple.py
@@ -0,0 +1,36 @@
+"""Single CF: takeoff, follow absolute-coords waypoints, land."""
+
+import numpy as np
+
+from pycrazyswarm import Crazyswarm
+
+
+Z = 1.0
+TAKEOFF_DURATION = 2.5
+GOTO_DURATION = 3.0
+WAYPOINTS = np.array([
+ (1.0, 0.0, Z),
+ (1.0, 1.0, Z),
+ (0.0, 1.0, Z),
+ (0.0, 0.0, Z),
+])
+
+
+def main():
+ swarm = Crazyswarm()
+ timeHelper = swarm.timeHelper
+ cf = swarm.allcfs.crazyflies[0]
+
+ cf.takeoff(targetHeight=Z, duration=TAKEOFF_DURATION)
+ timeHelper.sleep(TAKEOFF_DURATION + 1.0)
+
+ for p in WAYPOINTS:
+ cf.goTo(cf.initialPosition + p, yaw=0.0, duration=GOTO_DURATION)
+ timeHelper.sleep(GOTO_DURATION + 1.0)
+
+ cf.land(targetHeight=0.05, duration=TAKEOFF_DURATION)
+ timeHelper.sleep(TAKEOFF_DURATION + 1.0)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/real_robot/crazyswarm/src/crazyswarm_server.cpp b/real_robot/crazyswarm/src/crazyswarm_server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..441db7d7e984a50cd5535f2a0b9ca2a68edbffb4
--- /dev/null
+++ b/real_robot/crazyswarm/src/crazyswarm_server.cpp
@@ -0,0 +1,1970 @@
+#include "ros/ros.h"
+#include <tf/transform_broadcaster.h>
+#include <tf_conversions/tf_eigen.h>
+#include <ros/callback_queue.h>
+
+#include "crazyswarm/LogBlock.h"
+#include "crazyswarm/GenericLogData.h"
+#include "crazyswarm/UpdateParams.h"
+#include "crazyswarm/UploadTrajectory.h"
+#include "crazyswarm/NotifySetpointsStop.h"
+#undef major
+#undef minor
+#include "crazyswarm/Hover.h"
+#include "crazyswarm/Takeoff.h"
+#include "crazyswarm/Land.h"
+#include "crazyswarm/GoTo.h"
+#include "crazyswarm/StartTrajectory.h"
+#include "crazyswarm/SetGroupMask.h"
+#include "crazyswarm/FullState.h"
+#include "crazyswarm/Position.h"
+#include "crazyswarm/VelocityWorld.h"
+#include "crazyswarm/ZDistance.h"
+#include "std_srvs/Empty.h"
+#include <std_msgs/Empty.h>
+#include "geometry_msgs/Twist.h"
+#include "sensor_msgs/Imu.h"
+#include "sensor_msgs/Temperature.h"
+#include "sensor_msgs/MagneticField.h"
+#include "std_msgs/Float32.h"
+
+#include <sensor_msgs/Joy.h>
+#include <sensor_msgs/PointCloud.h>
+
+//#include <regex>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+
+#include <crazyflie_cpp/Crazyflie.h>
+
+// debug test
+#include <signal.h>
+#include <csignal> // or C++ style alternative
+
+// Motion Capture
+#include <libmotioncapture/motioncapture.h>
+
+// Object tracker
+#include <libobjecttracker/object_tracker.h>
+#include <libobjecttracker/cloudlog.hpp>
+
+#include <fstream>
+#include <future>
+#include <mutex>
+#include <wordexp.h> // tilde expansion
+
+/*
+Threading
+ * There are 2N+1 threads, where N is the number of groups (== number of unique channels)
+ * The main thread uses the libmotioncapture to receive external state updates; Once a new frame comes in, the
+ workers (CrazyflieGroup) are notified using a condition variable. Each CrazyflieGroup
+ does the objectTracking for its own group and broadcasts the resulting mocap data.
+ * One helper thread is used in the server to take care of incoming global service requests.
+ Those are forwarded to the groups (using a function call, i.e. the broadcasts run in this thread).
+ * Each group has two threads:
+ * Mocap worker. Waits for new mocap data (using a condition variable) and does the object tracking
+ and broadcasts the result.
+ * Service worker: Listens to CF-based service calls (such as upload trajectory) and executes
+ them. Those can be potentially long, without interfering with the mocap update.
+*/
+
+constexpr double pi() { return std::atan(1)*4; }
+
+double degToRad(double deg) {
+ return deg / 180.0 * pi();
+}
+
+double radToDeg(double rad) {
+ return rad * 180.0 / pi();
+}
+
+void logWarn(const std::string& msg)
+{
+ ROS_WARN("%s", msg.c_str());
+}
+
+class ROSLogger : public Logger
+{
+public:
+ ROSLogger()
+ : Logger()
+ {
+ }
+
+ virtual ~ROSLogger() {}
+
+ virtual void info(const std::string& msg)
+ {
+ ROS_INFO("%s", msg.c_str());
+ }
+
+ virtual void warning(const std::string& msg)
+ {
+ ROS_WARN("%s", msg.c_str());
+ }
+
+ virtual void error(const std::string& msg)
+ {
+ ROS_ERROR("%s", msg.c_str());
+ }
+};
+
+static ROSLogger rosLogger;
+
+class CrazyflieROS
+{
+public:
+ CrazyflieROS(
+ const std::string& link_uri,
+ const std::string& tf_prefix,
+ const std::string& frame,
+ const std::string& worldFrame,
+ int id,
+ const std::string& type,
+ const std::vector<crazyswarm::LogBlock>& log_blocks,
+ ros::CallbackQueue& queue)
+ : m_tf_prefix(tf_prefix)
+ , m_cf(
+ link_uri,
+ rosLogger,
+ std::bind(&CrazyflieROS::onConsole, this, std::placeholders::_1))
+ , m_frame(frame)
+ , m_worldFrame(worldFrame)
+ , m_id(id)
+ , m_type(type)
+ , m_serviceUpdateParams()
+ , m_serviceUploadTrajectory()
+ , m_serviceStartTrajectory()
+ , m_serviceTakeoff()
+ , m_serviceLand()
+ , m_serviceGoTo()
+ , m_serviceSetGroupMask()
+ , m_serviceNotifySetpointsStop()
+ , m_logBlocks(log_blocks)
+ , m_initializedPosition(false)
+ {
+ ros::NodeHandle nl("~");
+ nl.param("enable_logging", m_enableLogging, false);
+ nl.param("enable_logging_pose", m_enableLoggingPose, false);
+ nl.param("enable_parameters", m_enableParameters, true);
+ nl.param("force_no_cache", m_forceNoCache, false);
+
+ ros::NodeHandle n;
+ n.setCallbackQueue(&queue);
+ m_serviceUploadTrajectory = n.advertiseService(tf_prefix + "/upload_trajectory", &CrazyflieROS::uploadTrajectory, this);
+ m_serviceStartTrajectory = n.advertiseService(tf_prefix + "/start_trajectory", &CrazyflieROS::startTrajectory, this);
+ m_serviceTakeoff = n.advertiseService(tf_prefix + "/takeoff", &CrazyflieROS::takeoff, this);
+ m_serviceLand = n.advertiseService(tf_prefix + "/land", &CrazyflieROS::land, this);
+ m_serviceGoTo = n.advertiseService(tf_prefix + "/go_to", &CrazyflieROS::goTo, this);
+ m_serviceSetGroupMask = n.advertiseService(tf_prefix + "/set_group_mask", &CrazyflieROS::setGroupMask, this);
+ m_serviceNotifySetpointsStop = n.advertiseService(tf_prefix + "/notify_setpoints_stop", &CrazyflieROS::notifySetpointsStop, this);
+
+ m_subscribeCmdVel = n.subscribe(tf_prefix + "/cmd_vel", 1, &CrazyflieROS::cmdVelChanged, this);
+ m_subscribeCmdPosition = n.subscribe(tf_prefix + "/cmd_position", 1, &CrazyflieROS::cmdPositionSetpoint, this);
+ m_subscribeCmdFullState = n.subscribe(tf_prefix + "/cmd_full_state", 1, &CrazyflieROS::cmdFullStateSetpoint, this);
+ m_subscribeCmdZDistance = n.subscribe(tf_prefix + "/cmd_zdistance", 1, &CrazyflieROS::cmdZDistanceSetpoint, this);
+ m_subscribeCmdVelocityWorld = n.subscribe(tf_prefix + "/cmd_velocity_world", 1, &CrazyflieROS::cmdVelocityWorldSetpoint, this);
+ m_subscribeCmdStop = n.subscribe(m_tf_prefix + "/cmd_stop", 1, &CrazyflieROS::cmdStop, this);
+
+ // New Velocity command type (Hover)
+ m_subscribeCmdHover=n.subscribe(m_tf_prefix+"/cmd_hover",1,&CrazyflieROS::cmdHoverSetpoint, this);
+
+ if (m_enableLogging) {
+ m_logFile.open("logcf" + std::to_string(id) + ".csv");
+ m_logFile << "time,";
+ for (auto& logBlock : m_logBlocks) {
+ m_pubLogDataGeneric.push_back(n.advertise<crazyswarm::GenericLogData>(tf_prefix + "/" + logBlock.topic_name, 10));
+ for (const auto& variableName : logBlock.variables) {
+ m_logFile << variableName << ",";
+ }
+ }
+ m_logFile << std::endl;
+
+ if (m_enableLoggingPose) {
+ m_pubPose = n.advertise<geometry_msgs::PoseStamped>(m_tf_prefix + "/pose", 10);
+ }
+ }
+
+ // m_subscribeJoy = n.subscribe("/joy", 1, &CrazyflieROS::joyChanged, this);
+ }
+
+ ~CrazyflieROS()
+ {
+ m_logBlocks.clear();
+ m_logBlocksGeneric.clear();
+ // m_cf.trySysOff();
+ m_logFile.close();
+ }
+
+ const std::string& frame() const {
+ return m_frame;
+ }
+
+ const int id() const {
+ return m_id;
+ }
+
+ const std::string& type() const {
+ return m_type;
+ }
+
+ void sendPing() {
+ m_cf.sendPing();
+ }
+
+ // void joyChanged(
+ // const sensor_msgs::Joy::ConstPtr& msg)
+ // {
+ // static bool lastState = false;
+ // // static float x = 0.0;
+ // // static float y = 0.0;
+ // // static float z = 1.0;
+ // // static float yaw = 0;
+ // // bool changed = false;
+
+ // // float dx = msg->axes[4];
+ // // if (fabs(dx) > 0.1) {
+ // // x += dx * 0.01;
+ // // changed = true;
+ // // }
+ // // float dy = msg->axes[3];
+ // // if (fabs(dy) > 0.1) {
+ // // y += dy * 0.01;
+ // // changed = true;
+ // // }
+ // // float dz = msg->axes[1];
+ // // if (fabs(dz) > 0.1) {
+ // // z += dz * 0.01;
+ // // changed = true;
+ // // }
+ // // float dyaw = msg->axes[0];
+ // // if (fabs(dyaw) > 0.1) {
+ // // yaw += dyaw * 1.0;
+ // // changed = true;
+ // // }
+
+ // // if (changed) {
+ // // ROS_INFO("[%f, %f, %f, %f]", x, y, z, yaw);
+ // // m_cf.trajectoryHover(x, y, z, yaw);
+ // // }
+
+ // if (msg->buttons[4] && !lastState) {
+ // ROS_INFO("hover!");
+ // m_cf.trajectoryHover(0, 0, 1.0, 0, 2.0);
+ // }
+ // lastState = msg->buttons[4];
+ // }
+
+public:
+
+ template<class T, class U>
+ void updateParam(uint16_t id, const std::string& ros_param) {
+ U value;
+ ros::param::get(ros_param, value);
+ m_cf.addSetParam<T>(id, (T)value);
+ }
+
+ bool updateParams(
+ crazyswarm::UpdateParams::Request& req,
+ crazyswarm::UpdateParams::Response& res)
+ {
+ ROS_INFO("[%s] Update parameters", m_frame.c_str());
+ m_cf.startSetParamRequest();
+ for (auto&& p : req.params) {
+ std::string ros_param = "/" + m_tf_prefix + "/" + p;
+ size_t pos = p.find("/");
+ std::string group(p.begin(), p.begin() + pos);
+ std::string name(p.begin() + pos + 1, p.end());
+
+ auto entry = m_cf.getParamTocEntry(group, name);
+ if (entry)
+ {
+ switch (entry->type) {
+ case Crazyflie::ParamTypeUint8:
+ updateParam<uint8_t, int>(entry->id, ros_param);
+ break;
+ case Crazyflie::ParamTypeInt8:
+ updateParam<int8_t, int>(entry->id, ros_param);
+ break;
+ case Crazyflie::ParamTypeUint16:
+ updateParam<uint16_t, int>(entry->id, ros_param);
+ break;
+ case Crazyflie::ParamTypeInt16:
+ updateParam<int16_t, int>(entry->id, ros_param);
+ break;
+ case Crazyflie::ParamTypeUint32:
+ updateParam<uint32_t, int>(entry->id, ros_param);
+ break;
+ case Crazyflie::ParamTypeInt32:
+ updateParam<int32_t, int>(entry->id, ros_param);
+ break;
+ case Crazyflie::ParamTypeFloat:
+ updateParam<float, float>(entry->id, ros_param);
+ break;
+ }
+ }
+ else {
+ ROS_ERROR("Could not find param %s/%s", group.c_str(), name.c_str());
+ }
+ }
+ m_cf.setRequestedParams();
+ return true;
+ }
+
+
+ bool uploadTrajectory(
+ crazyswarm::UploadTrajectory::Request& req,
+ crazyswarm::UploadTrajectory::Response& res)
+ {
+ ROS_INFO("[%s] Upload trajectory", m_frame.c_str());
+
+ std::vector<Crazyflie::poly4d> pieces(req.pieces.size());
+ for (size_t i = 0; i < pieces.size(); ++i) {
+ if ( req.pieces[i].poly_x.size() != 8
+ || req.pieces[i].poly_y.size() != 8
+ || req.pieces[i].poly_z.size() != 8
+ || req.pieces[i].poly_yaw.size() != 8) {
+ ROS_FATAL("Wrong number of pieces!");
+ return false;
+ }
+ pieces[i].duration = req.pieces[i].duration.toSec();
+ for (size_t j = 0; j < 8; ++j) {
+ pieces[i].p[0][j] = req.pieces[i].poly_x[j];
+ pieces[i].p[1][j] = req.pieces[i].poly_y[j];
+ pieces[i].p[2][j] = req.pieces[i].poly_z[j];
+ pieces[i].p[3][j] = req.pieces[i].poly_yaw[j];
+ }
+ }
+ m_cf.uploadTrajectory(req.trajectoryId, req.pieceOffset, pieces);
+
+ ROS_INFO("[%s] Uploaded trajectory", m_frame.c_str());
+
+
+ return true;
+ }
+
+ bool startTrajectory(
+ crazyswarm::StartTrajectory::Request& req,
+ crazyswarm::StartTrajectory::Response& res)
+ {
+ ROS_INFO("[%s] Start trajectory", m_frame.c_str());
+
+ m_cf.startTrajectory(req.trajectoryId, req.timescale, req.reversed, req.relative, req.groupMask);
+
+ return true;
+ }
+
+ bool notifySetpointsStop(
+ crazyswarm::NotifySetpointsStop::Request& req,
+ crazyswarm::NotifySetpointsStop::Response& res)
+ {
+ ROS_INFO_NAMED(m_tf_prefix, "NotifySetpointsStop requested");
+ m_cf.notifySetpointsStop(req.remainValidMillisecs);
+ return true;
+ }
+
+ bool takeoff(
+ crazyswarm::Takeoff::Request& req,
+ crazyswarm::Takeoff::Response& res)
+ {
+ ROS_INFO("[%s] Takeoff", m_frame.c_str());
+
+ m_cf.takeoff(req.height, req.duration.toSec(), req.groupMask);
+
+ return true;
+ }
+
+ bool land(
+ crazyswarm::Land::Request& req,
+ crazyswarm::Land::Response& res)
+ {
+ ROS_INFO("[%s] Land", m_frame.c_str());
+
+ m_cf.land(req.height, req.duration.toSec(), req.groupMask);
+
+ return true;
+ }
+
+ bool goTo(
+ crazyswarm::GoTo::Request& req,
+ crazyswarm::GoTo::Response& res)
+ {
+ ROS_INFO("[%s] GoTo", m_frame.c_str());
+
+ m_cf.goTo(req.goal.x, req.goal.y, req.goal.z, req.yaw, req.duration.toSec(), req.relative, req.groupMask);
+
+ return true;
+ }
+
+ bool setGroupMask(
+ crazyswarm::SetGroupMask::Request& req,
+ crazyswarm::SetGroupMask::Response& res)
+ {
+ ROS_INFO("[%s] Set Group Mask", m_frame.c_str());
+
+ m_cf.setGroupMask(req.groupMask);
+
+ return true;
+ }
+
+ void cmdVelChanged(
+ const geometry_msgs::Twist::ConstPtr& msg)
+ {
+ // if (!m_isEmergency) {
+ float roll = msg->linear.y;
+ float pitch = -msg->linear.x;
+ float yawrate = msg->angular.z;
+ uint16_t thrust = (uint16_t)msg->linear.z;
+
+ m_cf.sendSetpoint(roll, pitch, yawrate, thrust);
+ // ROS_INFO("cmdVel %f %f %f %d (%f)", roll, pitch, yawrate, thrust, msg->linear.z);
+ // m_sentSetpoint = true;
+ // }
+ }
+
+ void cmdPositionSetpoint(
+ const crazyswarm::Position::ConstPtr& msg)
+ {
+ // if(!m_isEmergency) {
+ float x = msg->x;
+ float y = msg->y;
+ float z = msg->z;
+ float yaw = msg->yaw;
+
+ m_cf.sendPositionSetpoint(x, y, z, yaw);
+ // m_sentSetpoint = true;
+ // }
+ }
+
+ void cmdFullStateSetpoint(
+ const crazyswarm::FullState::ConstPtr& msg)
+ {
+ //ROS_INFO("got a full state setpoint");
+ // if (!m_isEmergency) {
+ float x = msg->pose.position.x;
+ float y = msg->pose.position.y;
+ float z = msg->pose.position.z;
+ float vx = msg->twist.linear.x;
+ float vy = msg->twist.linear.y;
+ float vz = msg->twist.linear.z;
+ float ax = msg->acc.x;
+ float ay = msg->acc.y;
+ float az = msg->acc.z;
+
+ float qx = msg->pose.orientation.x;
+ float qy = msg->pose.orientation.y;
+ float qz = msg->pose.orientation.z;
+ float qw = msg->pose.orientation.w;
+ float rollRate = msg->twist.angular.x;
+ float pitchRate = msg->twist.angular.y;
+ float yawRate = msg->twist.angular.z;
+
+ m_cf.sendFullStateSetpoint(
+ x, y, z,
+ vx, vy, vz,
+ ax, ay, az,
+ qx, qy, qz, qw,
+ rollRate, pitchRate, yawRate);
+ // m_sentSetpoint = true;
+ //ROS_INFO("set a full state setpoint");
+ // }
+ }
+
+ void cmdHoverSetpoint(const crazyswarm::Hover::ConstPtr& msg){
+ //ROS_INFO("got a hover setpoint");
+ float vx = msg->vx;
+ float vy = msg->vy;
+ float yawRate = msg->yawrate;
+ float zDistance = msg->zDistance;
+
+ m_cf.sendHoverSetpoint(vx, vy, yawRate, zDistance);
+ //ROS_INFO("set a hover setpoint");
+
+ }
+
+ /* changes todo*/
+ void cmdZDistanceSetpoint(const crazyswarm::ZDistance::ConstPtr& msg){
+ //ROS_INFO("got a hover setpoint");
+ float roll = msg->roll;
+ float pitch = msg->pitch;
+ float yawRate = msg->yawrate;
+ float zDistance = msg->zDistance;
+
+ // m_cf.sendzDistanceSetpoint(roll, pitch, yawRate, zDistance);
+ m_cf.testfunction();
+ //ROS_INFO("set a hover setpoint");
+
+ }
+ void cmdVelocityWorldSetpoint(
+ const crazyswarm::VelocityWorld::ConstPtr& msg)
+ {
+ // ROS_INFO("got a velocity world setpoint");
+ // if (!m_isEmergency) {
+ float x = msg->vel.x;
+ float y = msg->vel.y;
+ float z = msg->vel.z;
+ float yawRate = msg->yawRate;
+
+ m_cf.sendVelocityWorldSetpoint(
+ x, y, z, yawRate);
+ // m_sentSetpoint = true;
+ // ROS_INFO("set a velocity world setpoint");
+ // }
+ }
+
+ void cmdStop(
+ const std_msgs::Empty::ConstPtr& msg)
+ {
+ //ROS_INFO("got a stop setpoint");
+ // if (!m_isEmergency) {
+ m_cf.sendStop();
+ // m_sentSetpoint = true;
+ //ROS_INFO("set a stop setpoint");
+ // }
+ }
+
+ void run(
+ ros::CallbackQueue& queue)
+ {
+ // m_cf.reboot();
+ // m_cf.syson();
+ // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+
+ auto start = std::chrono::system_clock::now();
+
+ std::function<void(float)> cb_lq = std::bind(&CrazyflieROS::onLinkQuality, this, std::placeholders::_1);
+ m_cf.setLinkQualityCallback(cb_lq);
+
+ m_cf.logReset();
+
+ int numParams = 0;
+ if (m_enableParameters)
+ {
+ ROS_INFO("[%s] Requesting parameters...", m_frame.c_str());
+ m_cf.requestParamToc(m_forceNoCache);
+ for (auto iter = m_cf.paramsBegin(); iter != m_cf.paramsEnd(); ++iter) {
+ auto entry = *iter;
+ std::string paramName = "/" + m_tf_prefix + "/" + entry.group + "/" + entry.name;
+ switch (entry.type) {
+ case Crazyflie::ParamTypeUint8:
+ ros::param::set(paramName, m_cf.getParam<uint8_t>(entry.id));
+ break;
+ case Crazyflie::ParamTypeInt8:
+ ros::param::set(paramName, m_cf.getParam<int8_t>(entry.id));
+ break;
+ case Crazyflie::ParamTypeUint16:
+ ros::param::set(paramName, m_cf.getParam<uint16_t>(entry.id));
+ break;
+ case Crazyflie::ParamTypeInt16:
+ ros::param::set(paramName, m_cf.getParam<int16_t>(entry.id));
+ break;
+ case Crazyflie::ParamTypeUint32:
+ ros::param::set(paramName, (int)m_cf.getParam<uint32_t>(entry.id));
+ break;
+ case Crazyflie::ParamTypeInt32:
+ ros::param::set(paramName, m_cf.getParam<int32_t>(entry.id));
+ break;
+ case Crazyflie::ParamTypeFloat:
+ ros::param::set(paramName, m_cf.getParam<float>(entry.id));
+ break;
+ }
+ ++numParams;
+ }
+ ros::NodeHandle n;
+ n.setCallbackQueue(&queue);
+ m_serviceUpdateParams = n.advertiseService(m_tf_prefix + "/update_params", &CrazyflieROS::updateParams, this);
+ }
+ auto end1 = std::chrono::system_clock::now();
+ std::chrono::duration<double> elapsedSeconds1 = end1-start;
+ ROS_INFO("[%s] reqParamTOC: %f s (%d params)", m_frame.c_str(), elapsedSeconds1.count(), numParams);
+
+ // Logging
+ if (m_enableLogging) {
+ ROS_INFO("[%s] Requesting logging variables...", m_frame.c_str());
+ m_cf.requestLogToc(m_forceNoCache);
+ auto end2 = std::chrono::system_clock::now();
+ std::chrono::duration<double> elapsedSeconds2 = end2-end1;
+ ROS_INFO("[%s] reqLogTOC: %f s", m_frame.c_str(), elapsedSeconds2.count());
+
+ m_logBlocksGeneric.resize(m_logBlocks.size());
+ // custom log blocks
+ size_t i = 0;
+ for (auto& logBlock : m_logBlocks)
+ {
+ std::function<void(uint32_t, std::vector<double>*, void* userData)> cb =
+ std::bind(
+ &CrazyflieROS::onLogCustom,
+ this,
+ std::placeholders::_1,
+ std::placeholders::_2,
+ std::placeholders::_3);
+
+ m_logBlocksGeneric[i].reset(new LogBlockGeneric(
+ &m_cf,
+ logBlock.variables,
+ (void*)&m_pubLogDataGeneric[i],
+ cb));
+ m_logBlocksGeneric[i]->start(logBlock.frequency / 10);
+ ++i;
+ }
+ auto end3 = std::chrono::system_clock::now();
+ std::chrono::duration<double> elapsedSeconds3 = end3-end2;
+ ROS_INFO("[%s] logBlocks: %f s", m_frame.c_str(), elapsedSeconds1.count());
+
+ if (m_enableLoggingPose) {
+ std::function<void(uint32_t, logPose*)> cb = std::bind(&CrazyflieROS::onPoseData, this, std::placeholders::_1, std::placeholders::_2);
+
+ m_logBlockPose.reset(new LogBlock<logPose>(
+ &m_cf,{
+ {"stateEstimate", "x"},
+ {"stateEstimate", "y"},
+ {"stateEstimate", "z"},
+ {"stateEstimateZ", "quat"}
+ }, cb));
+ m_logBlockPose->start(10); // 100ms
+ }
+ }
+
+ ROS_INFO("Requesting memories...");
+ m_cf.requestMemoryToc();
+
+ auto end = std::chrono::system_clock::now();
+ std::chrono::duration<double> elapsedSeconds = end-start;
+ ROS_INFO("[%s] Ready. Elapsed: %f s", m_frame.c_str(), elapsedSeconds.count());
+ }
+
+ const Crazyflie::ParamTocEntry* getParamTocEntry(
+ const std::string& group,
+ const std::string& name) const
+ {
+ return m_cf.getParamTocEntry(group, name);
+ }
+
+ void initializePositionIfNeeded(float x, float y, float z)
+ {
+ if (m_initializedPosition) {
+ return;
+ }
+
+ m_cf.startSetParamRequest();
+ auto entry = m_cf.getParamTocEntry("kalman", "initialX");
+ m_cf.addSetParam(entry->id, x);
+ entry = m_cf.getParamTocEntry("kalman", "initialY");
+ m_cf.addSetParam(entry->id, y);
+ entry = m_cf.getParamTocEntry("kalman", "initialZ");
+ m_cf.addSetParam(entry->id, z);
+ m_cf.setRequestedParams();
+
+ entry = m_cf.getParamTocEntry("kalman", "resetEstimation");
+ m_cf.setParam<uint8_t>(entry->id, 1);
+
+ // kalmanUSC might not be part of the firmware
+ entry = m_cf.getParamTocEntry("kalmanUSC", "resetEstimation");
+ if (entry) {
+ m_cf.startSetParamRequest();
+ entry = m_cf.getParamTocEntry("kalmanUSC", "initialX");
+ m_cf.addSetParam(entry->id, x);
+ entry = m_cf.getParamTocEntry("kalmanUSC", "initialY");
+ m_cf.addSetParam(entry->id, y);
+ entry = m_cf.getParamTocEntry("kalmanUSC", "initialZ");
+ m_cf.addSetParam(entry->id, z);
+ m_cf.setRequestedParams();
+
+ entry = m_cf.getParamTocEntry("kalmanUSC", "resetEstimation");
+ m_cf.setParam<uint8_t>(entry->id, 1);
+ }
+
+ m_initializedPosition = true;
+ }
+
+private:
+ struct logPose {
+ float x;
+ float y;
+ float z;
+ int32_t quatCompressed;
+ } __attribute__((packed));
+
+private:
+
+ void onLinkQuality(float linkQuality) {
+ if (linkQuality < 0.7) {
+ ROS_WARN("[%s] Link Quality low (%f)", m_frame.c_str(), linkQuality);
+ }
+ }
+
+ void onConsole(const char* msg) {
+ m_messageBuffer += msg;
+ size_t pos = m_messageBuffer.find('\n');
+ if (pos != std::string::npos) {
+ m_messageBuffer[pos] = 0;
+ ROS_INFO("[%s] %s", m_frame.c_str(), m_messageBuffer.c_str());
+ m_messageBuffer.erase(0, pos+1);
+ }
+ }
+
+ void onPoseData(uint32_t time_in_ms, logPose* data) {
+ if (m_enableLoggingPose) {
+ geometry_msgs::PoseStamped msg;
+ msg.header.stamp = ros::Time::now();
+ msg.header.frame_id = "world";
+
+ msg.pose.position.x = data->x;
+ msg.pose.position.y = data->y;
+ msg.pose.position.z = data->z;
+
+ float q[4];
+ quatdecompress(data->quatCompressed, q);
+ msg.pose.orientation.x = q[0];
+ msg.pose.orientation.y = q[1];
+ msg.pose.orientation.z = q[2];
+ msg.pose.orientation.w = q[3];
+
+ m_pubPose.publish(msg);
+
+
+ tf::Transform tftransform;
+ tftransform.setOrigin(tf::Vector3(data->x, data->y, data->z));
+ tftransform.setRotation(tf::Quaternion(q[0], q[1], q[2], q[3]));
+ m_br.sendTransform(tf::StampedTransform(tftransform, ros::Time::now(), "world", frame()));
+ }
+ }
+
+ void onLogCustom(uint32_t time_in_ms, std::vector<double>* values, void* userData) {
+
+ ros::Publisher* pub = reinterpret_cast<ros::Publisher*>(userData);
+
+ crazyswarm::GenericLogData msg;
+ msg.header.stamp = ros::Time(time_in_ms/1000.0);
+ msg.values = *values;
+
+ m_logFile << time_in_ms / 1000.0 << ",";
+ for (const auto& value : *values) {
+ m_logFile << value << ",";
+ }
+ m_logFile << std::endl;
+
+ pub->publish(msg);
+ }
+
+private:
+ std::string m_tf_prefix;
+ Crazyflie m_cf;
+ std::string m_frame;
+ std::string m_worldFrame;
+ bool m_enableParameters;
+ bool m_enableLogging;
+ bool m_enableLoggingPose;
+ int m_id;
+ std::string m_type;
+
+ ros::ServiceServer m_serviceUpdateParams;
+ ros::ServiceServer m_serviceUploadTrajectory;
+ ros::ServiceServer m_serviceStartTrajectory;
+ ros::ServiceServer m_serviceTakeoff;
+ ros::ServiceServer m_serviceLand;
+ ros::ServiceServer m_serviceGoTo;
+ ros::ServiceServer m_serviceSetGroupMask;
+ ros::ServiceServer m_serviceNotifySetpointsStop;
+
+ ros::Subscriber m_subscribeCmdVel;
+ ros::Subscriber m_subscribeCmdPosition;
+ ros::Subscriber m_subscribeCmdFullState;
+ ros::Subscriber m_subscribeCmdZDistance;
+ ros::Subscriber m_subscribeCmdVelocityWorld;
+ ros::Subscriber m_subscribeCmdStop;
+
+ ros::Subscriber m_subscribeCmdHover; // Hover vel subscriber
+
+ tf::TransformBroadcaster m_br;
+
+ std::vector<crazyswarm::LogBlock> m_logBlocks;
+ std::vector<ros::Publisher> m_pubLogDataGeneric;
+ std::vector<std::unique_ptr<LogBlockGeneric> > m_logBlocksGeneric;
+
+ ros::Subscriber m_subscribeJoy;
+
+ ros::Publisher m_pubPose;
+ std::unique_ptr<LogBlock<logPose>> m_logBlockPose;
+
+ std::ofstream m_logFile;
+ bool m_forceNoCache;
+ bool m_initializedPosition;
+ std::string m_messageBuffer;
+};
+
+
+// handles a group of Crazyflies, which share a radio
+class CrazyflieGroup
+{
+public:
+ struct latency
+ {
+ double objectTracking;
+ double broadcasting;
+ };
+
+ CrazyflieGroup(
+ const std::vector<libobjecttracker::DynamicsConfiguration>& dynamicsConfigurations,
+ const std::vector<libobjecttracker::MarkerConfiguration>& markerConfigurations,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr pMarkers,
+ std::map<std::string, libmotioncapture::RigidBody>* pMocapRigidBodies,
+ int radio,
+ int channel,
+ bool useMotionCaptureObjectTracking,
+ const std::vector<crazyswarm::LogBlock>& logBlocks,
+ std::string interactiveObject,
+ bool writeCSVs,
+ bool sendPositionOnly
+ )
+ : m_cfs()
+ , m_tracker(nullptr)
+ , m_radio(radio)
+ , m_pMarkers(pMarkers)
+ , m_pMocapRigidBodies(pMocapRigidBodies)
+ , m_slowQueue()
+ , m_cfbc("radio://" + std::to_string(radio) + "/" + std::to_string(channel) + "/2M/FFE7E7E7E7")
+ , m_isEmergency(false)
+ , m_useMotionCaptureObjectTracking(useMotionCaptureObjectTracking)
+ , m_br()
+ , m_interactiveObject(interactiveObject)
+ , m_sendPositionOnly(sendPositionOnly)
+ , m_outputCSVs()
+ , m_phase(0)
+ , m_phaseStart()
+ {
+ std::vector<libobjecttracker::Object> objects;
+ readObjects(objects, channel, logBlocks);
+ m_tracker = new libobjecttracker::ObjectTracker(
+ dynamicsConfigurations,
+ markerConfigurations,
+ objects);
+ m_tracker->setLogWarningCallback(logWarn);
+ if (writeCSVs) {
+ m_outputCSVs.resize(m_cfs.size());
+ for (auto& output : m_outputCSVs) {
+ output.reset(new std::ofstream);
+ }
+ }
+ }
+
+ ~CrazyflieGroup()
+ {
+ for(auto cf : m_cfs) {
+ delete cf;
+ }
+ delete m_tracker;
+ }
+
+ const latency& lastLatency() const {
+ return m_latency;
+ }
+
+ int radio() const {
+ return m_radio;
+ }
+
+ void runInteractiveObject(std::vector<CrazyflieBroadcaster::externalPose> &states)
+ {
+ publishRigidBody(m_interactiveObject, 0xFF, states);
+ }
+
+ void runFast()
+ {
+ auto stamp = std::chrono::high_resolution_clock::now();
+
+ std::vector<CrazyflieBroadcaster::externalPose> states;
+
+ if (!m_interactiveObject.empty()) {
+ runInteractiveObject(states);
+ }
+
+ if (m_useMotionCaptureObjectTracking) {
+ for (auto cf : m_cfs) {
+ bool found = publishRigidBody(cf->frame(), cf->id(), states);
+ if (found) {
+ cf->initializePositionIfNeeded(states.back().x, states.back().y, states.back().z);
+ }
+ }
+ } else {
+ // run object tracker
+ {
+ auto start = std::chrono::high_resolution_clock::now();
+ m_tracker->update(m_pMarkers);
+ auto end = std::chrono::high_resolution_clock::now();
+ std::chrono::duration<double> elapsedSeconds = end-start;
+ m_latency.objectTracking = elapsedSeconds.count();
+ // totalLatency += elapsedSeconds.count();
+ // ROS_INFO("Tracking: %f s", elapsedSeconds.count());
+ }
+
+ for (size_t i = 0; i < m_cfs.size(); ++i) {
+ if (m_tracker->objects()[i].lastTransformationValid()) {
+
+ const Eigen::Affine3f& transform = m_tracker->objects()[i].transformation();
+ Eigen::Quaternionf q(transform.rotation());
+ const auto& translation = transform.translation();
+
+ states.resize(states.size() + 1);
+ states.back().id = m_cfs[i]->id();
+ states.back().x = translation.x();
+ states.back().y = translation.y();
+ states.back().z = translation.z();
+ states.back().qx = q.x();
+ states.back().qy = q.y();
+ states.back().qz = q.z();
+ states.back().qw = q.w();
+
+ m_cfs[i]->initializePositionIfNeeded(states.back().x, states.back().y, states.back().z);
+
+ tf::Transform tftransform;
+ tftransform.setOrigin(tf::Vector3(translation.x(), translation.y(), translation.z()));
+ tftransform.setRotation(tf::Quaternion(q.x(), q.y(), q.z(), q.w()));
+ // Eigen::Affine3d transformd = transform.cast<double>();
+ // tf::transformEigenToTF(transformd, tftransform);
+ // tftransform.setOrigin(tf::Vector3(translation.x(), translation.y(), translation.z()));
+ // tf::Quaternion tfq(q.x(), q.y(), q.z(), q.w());
+ m_br.sendTransform(tf::StampedTransform(tftransform, ros::Time::now(), "world", m_cfs[i]->frame()));
+
+ if (m_outputCSVs.size() > 0) {
+ std::chrono::duration<double> tDuration = stamp - m_phaseStart;
+ double t = tDuration.count();
+ auto rpy = q.toRotationMatrix().eulerAngles(0, 1, 2);
+ *m_outputCSVs[i] << t << "," << states.back().x << "," << states.back().y << "," << states.back().z
+ << "," << rpy(0) << "," << rpy(1) << "," << rpy(2) << "\n";
+ }
+ } else {
+ std::chrono::duration<double> elapsedSeconds = stamp - m_tracker->objects()[i].lastValidTime();
+ ROS_WARN("No updated pose for CF %s for %f s.",
+ m_cfs[i]->frame().c_str(),
+ elapsedSeconds.count());
+ }
+ }
+ }
+
+ {
+ auto start = std::chrono::high_resolution_clock::now();
+ if (!m_sendPositionOnly) {
+ m_cfbc.sendExternalPoses(states);
+ } else {
+ std::vector<CrazyflieBroadcaster::externalPosition> positions(states.size());
+ for (size_t i = 0; i < positions.size(); ++i) {
+ positions[i].id = states[i].id;
+ positions[i].x = states[i].x;
+ positions[i].y = states[i].y;
+ positions[i].z = states[i].z;
+ }
+ m_cfbc.sendExternalPositions(positions);
+ }
+ auto end = std::chrono::high_resolution_clock::now();
+ std::chrono::duration<double> elapsedSeconds = end-start;
+ m_latency.broadcasting = elapsedSeconds.count();
+ // totalLatency += elapsedSeconds.count();
+ // ROS_INFO("Broadcasting: %f s", elapsedSeconds.count());
+ }
+
+ // auto time = std::chrono::duration_cast<std::chrono::microseconds>(
+ // std::chrono::high_resolution_clock::now().time_since_epoch()).count();
+ // for (const auto& state : states) {
+ // std::cout << time << "," << state.x << "," << state.y << "," << state.z << std::endl;
+ // }
+
+ }
+
+ void runSlow()
+ {
+ ros::NodeHandle nl("~");
+ bool enableLogging;
+ nl.getParam("enable_logging", enableLogging);
+
+ while(ros::ok() && !m_isEmergency) {
+ if (enableLogging) {
+ for (const auto& cf : m_cfs) {
+ cf->sendPing();
+ }
+ }
+ m_slowQueue.callAvailable(ros::WallDuration(0));
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ }
+ }
+
+ void emergency()
+ {
+ m_cfbc.emergencyStop();
+ m_isEmergency = true;
+ }
+
+ void takeoff(float height, float duration, uint8_t groupMask)
+ {
+ // for (size_t i = 0; i < 10; ++i) {
+ m_cfbc.takeoff(height, duration, groupMask);
+ // std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ // }
+ }
+
+ void land(float height, float duration, uint8_t groupMask)
+ {
+ // for (size_t i = 0; i < 10; ++i) {
+ m_cfbc.land(height, duration, groupMask);
+ // std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ // }
+ }
+
+ void goTo(float x, float y, float z, float yaw, float duration, uint8_t groupMask)
+ {
+ // for (size_t i = 0; i < 10; ++i) {
+ m_cfbc.goTo(x, y, z, yaw, duration, groupMask);
+ // std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ // }
+ }
+
+ void startTrajectory(
+ uint8_t trajectoryId,
+ float timescale,
+ bool reversed,
+ uint8_t groupMask)
+ {
+ // for (size_t i = 0; i < 10; ++i) {
+ m_cfbc.startTrajectory(trajectoryId, timescale, reversed, groupMask);
+ // std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ // }
+ }
+
+
+
+ template<class T, class U>
+ void updateParam(const char* group, const char* name, const std::string& ros_param) {
+ U value;
+ ros::param::get(ros_param, value);
+ m_cfbc.setParam<T>(group, name, (T)value);
+ }
+
+ void updateParams(
+ const std::vector<std::string>& params)
+ {
+ for (const auto& p : params) {
+ std::string ros_param = "/allcfs/" + p;
+ size_t pos = p.find("/");
+ std::string g(p.begin(), p.begin() + pos);
+ std::string n(p.begin() + pos + 1, p.end());
+
+ // This assumes that we can find the variable in the TOC of the first
+ // CF to find the type (the actual update is done by name)
+ auto entry = m_cfs.front()->getParamTocEntry(g, n);
+ if (entry)
+ {
+ switch (entry->type) {
+ case Crazyflie::ParamTypeUint8:
+ updateParam<uint8_t, int>(g.c_str(), n.c_str(), ros_param);
+ break;
+ case Crazyflie::ParamTypeInt8:
+ updateParam<int8_t, int>(g.c_str(), n.c_str(), ros_param);
+ break;
+ case Crazyflie::ParamTypeUint16:
+ updateParam<uint16_t, int>(g.c_str(), n.c_str(), ros_param);
+ break;
+ case Crazyflie::ParamTypeInt16:
+ updateParam<int16_t, int>(g.c_str(), n.c_str(), ros_param);
+ break;
+ case Crazyflie::ParamTypeUint32:
+ updateParam<uint32_t, int>(g.c_str(), n.c_str(), ros_param);
+ break;
+ case Crazyflie::ParamTypeInt32:
+ updateParam<int32_t, int>(g.c_str(), n.c_str(), ros_param);
+ break;
+ case Crazyflie::ParamTypeFloat:
+ updateParam<float, float>(g.c_str(), n.c_str(), ros_param);
+ break;
+ }
+ }
+ else {
+ ROS_ERROR("Could not find param %s/%s", g.c_str(), n.c_str());
+ }
+ }
+ }
+
+private:
+
+ bool publishRigidBody(const std::string& name, uint8_t id, std::vector<CrazyflieBroadcaster::externalPose> &states)
+ {
+ assert(m_pMocapRigidBodies);
+ const auto& iter = m_pMocapRigidBodies->find(name);
+ if (iter != m_pMocapRigidBodies->end()) {
+ const auto& rigidBody = iter->second;
+
+ states.resize(states.size() + 1);
+ states.back().id = id;
+ states.back().x = rigidBody.position().x();
+ states.back().y = rigidBody.position().y();
+ states.back().z = rigidBody.position().z();
+ states.back().qx = rigidBody.rotation().x();
+ states.back().qy = rigidBody.rotation().y();
+ states.back().qz = rigidBody.rotation().z();
+ states.back().qw = rigidBody.rotation().w();
+
+ tf::Transform transform;
+ transform.setOrigin(tf::Vector3(
+ states.back().x,
+ states.back().y,
+ states.back().z));
+ tf::Quaternion q(
+ states.back().qx,
+ states.back().qy,
+ states.back().qz,
+ states.back().qw);
+ transform.setRotation(q);
+ m_br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", name));
+ return true;
+ } else {
+ ROS_WARN("No updated pose for motion capture object %s", name.c_str());
+ }
+ return false;
+ }
+
+
+ void readObjects(
+ std::vector<libobjecttracker::Object>& objects,
+ int channel,
+ const std::vector<crazyswarm::LogBlock>& logBlocks)
+ {
+ // read CF config
+ struct CFConfig
+ {
+ std::string uri;
+ std::string tf_prefix;
+ std::string frame;
+ int idNumber;
+ std::string type;
+ };
+ ros::NodeHandle nGlobal;
+
+ XmlRpc::XmlRpcValue crazyflies;
+ nGlobal.getParam("crazyflies", crazyflies);
+ ROS_ASSERT(crazyflies.getType() == XmlRpc::XmlRpcValue::TypeArray);
+
+ objects.clear();
+ m_cfs.clear();
+ std::vector<CFConfig> cfConfigs;
+ for (int32_t i = 0; i < crazyflies.size(); ++i) {
+ ROS_ASSERT(crazyflies[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
+ XmlRpc::XmlRpcValue crazyflie = crazyflies[i];
+ int id = crazyflie["id"];
+ int ch = crazyflie["channel"];
+ std::string type = crazyflie["type"];
+ if (ch == channel) {
+ XmlRpc::XmlRpcValue pos = crazyflie["initialPosition"];
+ ROS_ASSERT(pos.getType() == XmlRpc::XmlRpcValue::TypeArray);
+
+ std::vector<double> posVec(3);
+ for (int32_t j = 0; j < pos.size(); ++j) {
+ switch (pos[j].getType()) {
+ case XmlRpc::XmlRpcValue::TypeDouble:
+ posVec[j] = static_cast<double>(pos[j]);
+ break;
+ case XmlRpc::XmlRpcValue::TypeInt:
+ posVec[j] = static_cast<int>(pos[j]);
+ break;
+ default:
+ std::stringstream message;
+ message << "crazyflies.yaml error:"
+ " entry " << j << " of initialPosition for cf" << id <<
+ " should be type int or double.";
+ throw std::runtime_error(message.str().c_str());
+ }
+ }
+ Eigen::Affine3f m;
+ m = Eigen::Translation3f(posVec[0], posVec[1], posVec[2]);
+ int markerConfigurationIdx;
+ nGlobal.getParam("crazyflieTypes/" + type + "/markerConfiguration", markerConfigurationIdx);
+ int dynamicsConfigurationIdx;
+ nGlobal.getParam("crazyflieTypes/" + type + "/dynamicsConfiguration", dynamicsConfigurationIdx);
+ std::string name = "cf" + std::to_string(id);
+ objects.push_back(libobjecttracker::Object(markerConfigurationIdx, dynamicsConfigurationIdx, m, name));
+
+ std::stringstream sstr;
+ sstr << std::setfill ('0') << std::setw(2) << std::hex << id;
+ std::string idHex = sstr.str();
+
+ std::string uri = "radio://" + std::to_string(m_radio) + "/" + std::to_string(channel) + "/2M/E7E7E7E7" + idHex;
+ std::string tf_prefix = "cf" + std::to_string(id);
+ std::string frame = "cf" + std::to_string(id);
+ cfConfigs.push_back({uri, tf_prefix, frame, id, type});
+ }
+ }
+ ROS_INFO("Parsed crazyflies.yaml successfully.");
+
+ // add Crazyflies
+ for (const auto& config : cfConfigs) {
+ addCrazyflie(config.uri, config.tf_prefix, config.frame, "/world", config.idNumber, config.type, logBlocks);
+
+ auto start = std::chrono::high_resolution_clock::now();
+ updateParams(m_cfs.back());
+ auto end = std::chrono::high_resolution_clock::now();
+ std::chrono::duration<double> elapsed = end - start;
+ ROS_INFO("Update params: %f s", elapsed.count());
+ }
+ }
+
+ void addCrazyflie(
+ const std::string& uri,
+ const std::string& tf_prefix,
+ const std::string& frame,
+ const std::string& worldFrame,
+ int id,
+ const std::string& type,
+ const std::vector<crazyswarm::LogBlock>& logBlocks)
+ {
+ ROS_INFO("Adding CF: %s (%s, %s)...", tf_prefix.c_str(), uri.c_str(), frame.c_str());
+ auto start = std::chrono::high_resolution_clock::now();
+ CrazyflieROS* cf = new CrazyflieROS(
+ uri,
+ tf_prefix,
+ frame,
+ worldFrame,
+ id,
+ type,
+ logBlocks,
+ m_slowQueue);
+ auto end = std::chrono::high_resolution_clock::now();
+ std::chrono::duration<double> elapsed = end - start;
+ ROS_INFO("CF ctor: %f s", elapsed.count());
+ cf->run(m_slowQueue);
+ auto end2 = std::chrono::high_resolution_clock::now();
+ std::chrono::duration<double> elapsed2 = end2 - end;
+ ROS_INFO("CF run: %f s", elapsed2.count());
+ m_cfs.push_back(cf);
+ }
+
+ void updateParams(
+ CrazyflieROS* cf)
+ {
+ ros::NodeHandle n("~");
+ ros::NodeHandle nGlobal;
+ // update parameters
+ // std::cout << "attempt: " << "firmwareParams/" + cf->type() << std::endl;
+ // char dummy;
+ // std::cin >> dummy;
+
+ // update global, type-specific, and CF-specific parameters
+ std::vector<XmlRpc::XmlRpcValue> firmwareParamsVec(2);
+ n.getParam("firmwareParams", firmwareParamsVec[0]);
+ nGlobal.getParam("crazyflieTypes/" + cf->type() + "/firmwareParams", firmwareParamsVec[1]);
+
+ XmlRpc::XmlRpcValue crazyflies;
+ nGlobal.getParam("crazyflies", crazyflies);
+ ROS_ASSERT(crazyflies.getType() == XmlRpc::XmlRpcValue::TypeArray);
+ for (int32_t i = 0; i < crazyflies.size(); ++i) {
+ ROS_ASSERT(crazyflies[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
+ XmlRpc::XmlRpcValue crazyflie = crazyflies[i];
+ int id = crazyflie["id"];
+ if (id == cf->id()) {
+ if (crazyflie.hasMember("firmwareParams")) {
+ firmwareParamsVec.push_back(crazyflie["firmwareParams"]);
+ }
+ break;
+ }
+ }
+
+
+ crazyswarm::UpdateParams::Request request;
+ crazyswarm::UpdateParams::Response response;
+
+ for (auto& firmwareParams : firmwareParamsVec) {
+ // ROS_ASSERT(firmwareParams.getType() == XmlRpc::XmlRpcValue::TypeArray);
+ auto iter = firmwareParams.begin();
+ for (; iter != firmwareParams.end(); ++iter) {
+ std::string group = iter->first;
+ XmlRpc::XmlRpcValue v = iter->second;
+ auto iter2 = v.begin();
+ for (; iter2 != v.end(); ++iter2) {
+ std::string param = iter2->first;
+ XmlRpc::XmlRpcValue value = iter2->second;
+ if (value.getType() == XmlRpc::XmlRpcValue::TypeBoolean) {
+ bool b = value;
+ nGlobal.setParam(cf->frame() + "/" + group + "/" + param, b);
+ std::cout << "update " << group + "/" + param << " to " << b << std::endl;
+ } else if (value.getType() == XmlRpc::XmlRpcValue::TypeInt) {
+ int b = value;
+ nGlobal.setParam(cf->frame() + "/" + group + "/" + param, b);
+ std::cout << "update " << group + "/" + param << " to " << b << std::endl;
+ } else if (value.getType() == XmlRpc::XmlRpcValue::TypeDouble) {
+ double b = value;
+ nGlobal.setParam(cf->frame() + "/" + group + "/" + param, b);
+ std::cout << "update " << group + "/" + param << " to " << b << std::endl;
+ } else if (value.getType() == XmlRpc::XmlRpcValue::TypeString) {
+ // "1e-5" is not recognize as double; convert manually here
+ std::string value_str = value;
+ double value = std::stod(value_str);
+ nGlobal.setParam(cf->frame() + "/" + group + "/" + param, value);
+ std::cout << "update " << group + "/" + param << " to " << value << std::endl;
+ } else {
+ ROS_ERROR("No known type for %s.%s! (type: %d)", group.c_str(), param.c_str(), value.getType());
+ }
+ request.params.push_back(group + "/" + param);
+
+ }
+ }
+ }
+ cf->updateParams(request, response);
+ }
+
+private:
+ std::vector<CrazyflieROS*> m_cfs;
+ std::string m_interactiveObject;
+ libobjecttracker::ObjectTracker* m_tracker; // non-owning pointer
+ int m_radio;
+ pcl::PointCloud<pcl::PointXYZ>::Ptr m_pMarkers;
+ std::map<std::string, libmotioncapture::RigidBody>* m_pMocapRigidBodies; // non-owning pointer
+ ros::CallbackQueue m_slowQueue;
+ CrazyflieBroadcaster m_cfbc;
+ bool m_isEmergency;
+ bool m_useMotionCaptureObjectTracking;
+ tf::TransformBroadcaster m_br;
+ latency m_latency;
+ bool m_sendPositionOnly;
+ std::vector<std::unique_ptr<std::ofstream>> m_outputCSVs;
+ int m_phase;
+ std::chrono::high_resolution_clock::time_point m_phaseStart;
+};
+
+// handles all Crazyflies
+class CrazyflieServer
+{
+public:
+ CrazyflieServer()
+ : m_isEmergency(false)
+ , m_serviceEmergency()
+ , m_serviceStartTrajectory()
+ , m_serviceTakeoff()
+ , m_serviceLand()
+ , m_serviceGoTo()
+ , m_lastInteractiveObjectPosition(-10, -10, 1)
+ , m_broadcastingNumRepeats(15)
+ , m_broadcastingDelayBetweenRepeatsMs(1)
+ {
+ ros::NodeHandle nh;
+ nh.setCallbackQueue(&m_queue);
+
+ m_serviceEmergency = nh.advertiseService("emergency", &CrazyflieServer::emergency, this);
+ m_serviceStartTrajectory = nh.advertiseService("start_trajectory", &CrazyflieServer::startTrajectory, this);
+ m_serviceTakeoff = nh.advertiseService("takeoff", &CrazyflieServer::takeoff, this);
+ m_serviceLand = nh.advertiseService("land", &CrazyflieServer::land, this);
+ m_serviceGoTo = nh.advertiseService("go_to", &CrazyflieServer::goTo, this);
+
+ m_serviceUpdateParams = nh.advertiseService("update_params", &CrazyflieServer::updateParams, this);
+
+ m_pubPointCloud = nh.advertise<sensor_msgs::PointCloud>("pointCloud", 1);
+
+ m_subscribeVirtualInteractiveObject = nh.subscribe("virtual_interactive_object", 1, &CrazyflieServer::virtualInteractiveObjectCallback, this);
+ }
+
+ ~CrazyflieServer()
+ {
+ for (CrazyflieGroup* group : m_groups) {
+ delete group;
+ }
+ }
+
+ void virtualInteractiveObjectCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
+ {
+ m_lastInteractiveObjectPosition = Eigen::Vector3f(
+ msg->pose.position.x,
+ msg->pose.position.y,
+ msg->pose.position.z);
+ }
+
+ void run()
+ {
+ std::thread tSlow(&CrazyflieServer::runSlow, this);
+ runFast();
+ tSlow.join();
+ }
+
+ void runFast()
+ {
+
+ // std::vector<CrazyflieBroadcaster::externalPose> states(1);
+ // states.back().id = 07;
+ // states.back().q0 = 0;
+ // states.back().q1 = 0;
+ // states.back().q2 = 0;
+ // states.back().q3 = 1;
+
+
+ // while(ros::ok()) {
+
+ // m_cfbc.sendPositionExternalBringup(states);
+ // // m_cfs[0]->sendPositionExternalBringup(states[0]);
+ // m_fastQueue.callAvailable(ros::WallDuration(0));
+ // std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ // }
+ // return;
+
+ std::vector<libobjecttracker::DynamicsConfiguration> dynamicsConfigurations;
+ std::vector<libobjecttracker::MarkerConfiguration> markerConfigurations;
+ std::set<int> channels;
+
+ readMarkerConfigurations(markerConfigurations);
+ readDynamicsConfigurations(dynamicsConfigurations);
+ readChannels(channels);
+
+ bool useMotionCaptureObjectTracking;
+ std::string logFilePath;
+ std::string interactiveObject;
+ bool printLatency;
+ bool writeCSVs;
+ bool sendPositionOnly;
+ std::string motionCaptureType;
+
+ ros::NodeHandle nl("~");
+ std::string objectTrackingType;
+ nl.getParam("object_tracking_type", objectTrackingType);
+ useMotionCaptureObjectTracking = (objectTrackingType == "motionCapture");
+ nl.param<std::string>("save_point_clouds", logFilePath, "");
+ nl.param<std::string>("interactive_object", interactiveObject, "");
+ nl.getParam("print_latency", printLatency);
+ nl.getParam("write_csvs", writeCSVs);
+ nl.param<std::string>("motion_capture_type", motionCaptureType, "vicon");
+
+ nl.param<int>("broadcasting_num_repeats", m_broadcastingNumRepeats, 15);
+ nl.param<int>("broadcasting_delay_between_repeats_ms", m_broadcastingDelayBetweenRepeatsMs, 1);
+ nl.param<bool>("send_position_only", sendPositionOnly, false);
+
+ // tilde-expansion
+ wordexp_t wordexp_result;
+ if (wordexp(logFilePath.c_str(), &wordexp_result, 0) == 0) {
+ // success - only read first result, could be more if globs were used
+ logFilePath = wordexp_result.we_wordv[0];
+ }
+ wordfree(&wordexp_result);
+
+ libobjecttracker::PointCloudLogger pointCloudLogger(logFilePath);
+ const bool logClouds = !logFilePath.empty();
+
+ // custom log blocks
+ std::vector<std::string> genericLogTopics;
+ nl.param("genericLogTopics", genericLogTopics, std::vector<std::string>());
+ std::vector<int> genericLogTopicFrequencies;
+ nl.param("genericLogTopicFrequencies", genericLogTopicFrequencies, std::vector<int>());
+ std::vector<crazyswarm::LogBlock> logBlocks;
+ if (genericLogTopics.size() == genericLogTopicFrequencies.size())
+ {
+ size_t i = 0;
+ for (auto& topic : genericLogTopics)
+ {
+ crazyswarm::LogBlock logBlock;
+ logBlock.topic_name = topic;
+ logBlock.frequency = genericLogTopicFrequencies[i];
+ nl.getParam("genericLogTopic_" + topic + "_Variables", logBlock.variables);
+ logBlocks.push_back(logBlock);
+ ++i;
+ }
+ }
+ else
+ {
+ ROS_ERROR("Cardinality of genericLogTopics and genericLogTopicFrequencies does not match!");
+ }
+
+ // Make a new client
+ std::map<std::string, std::string> cfg;
+ std::string hostname;
+ nl.getParam("motion_capture_host_name", hostname);
+ cfg["hostname"] = hostname;
+ if (nl.hasParam("motion_capture_interface_ip")) {
+ std::string interface_ip;
+ nl.param<std::string>("motion_capture_interface_ip", interface_ip);
+ cfg["interface_ip"] = interface_ip;
+ }
+
+ std::unique_ptr<libmotioncapture::MotionCapture> mocap;
+
+ if (motionCaptureType != "none") {
+ ROS_INFO(
+ "libmotioncapture connecting to %s at hostname '%s' - "
+ "might block indefinitely if unreachable!",
+ motionCaptureType.c_str(),
+ hostname.c_str()
+ );
+ mocap.reset(libmotioncapture::MotionCapture::connect(motionCaptureType, cfg));
+ if (!mocap) {
+ throw std::runtime_error("Unknown motion capture type!");
+ }
+ }
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr markers(new pcl::PointCloud<pcl::PointXYZ>);
+ std::map<std::string, libmotioncapture::RigidBody> mocapRigidBodies;
+
+ // Create all groups in parallel and launch threads
+ {
+ std::vector<std::future<CrazyflieGroup*> > handles;
+ int r = 0;
+ std::cout << "ch: " << channels.size() << std::endl;
+ for (int channel : channels) {
+ auto handle = std::async(std::launch::async,
+ [&](int channel, int radio)
+ {
+ // std::cout << "radio: " << radio << std::endl;
+ return new CrazyflieGroup(
+ dynamicsConfigurations,
+ markerConfigurations,
+ // &client,
+ markers,
+ &mocapRigidBodies,
+ radio,
+ channel,
+ useMotionCaptureObjectTracking,
+ logBlocks,
+ interactiveObject,
+ writeCSVs,
+ sendPositionOnly);
+ },
+ channel,
+ r
+ );
+ handles.push_back(std::move(handle));
+ ++r;
+ }
+
+ for (auto& handle : handles) {
+ m_groups.push_back(handle.get());
+ }
+ }
+
+ // start the groups threads
+ std::vector<std::thread> threads;
+ for (auto& group : m_groups) {
+ threads.push_back(std::thread(&CrazyflieGroup::runSlow, group));
+ }
+
+ ROS_INFO("Started %lu threads", threads.size());
+
+ // Connect to a server
+ // ROS_INFO("Connecting to %s ...", hostName.c_str());
+ // while (ros::ok() && !client.IsConnected().Connected) {
+ // // Direct connection
+ // bool ok = (client.Connect(hostName).Result == Result::Success);
+ // if(!ok) {
+ // ROS_WARN("Connect failed...");
+ // }
+ // ros::spinOnce();
+ // }
+ if (mocap) {
+
+ // setup messages
+ sensor_msgs::PointCloud msgPointCloud;
+ msgPointCloud.header.seq = 0;
+ msgPointCloud.header.frame_id = "world";
+
+ auto startTime = std::chrono::high_resolution_clock::now();
+
+ struct latencyEntry {
+ std::string name;
+ double secs;
+ };
+ std::vector<latencyEntry> latencies;
+
+ std::vector<double> latencyTotal(6 + 3 * 2, 0.0);
+ uint32_t latencyCount = 0;
+
+ while (ros::ok() && !m_isEmergency) {
+ // Get a frame
+ mocap->waitForNextFrame();
+
+ latencies.clear();
+
+ auto startIteration = std::chrono::high_resolution_clock::now();
+ double totalLatency = 0;
+
+ // Get the latency
+ const auto& mocapLatency = mocap->latency();
+ float totalMocapLatency = 0;
+ for (const auto& item : mocapLatency) {
+ totalMocapLatency += item.value();
+ }
+ if (totalMocapLatency > 0.035) {
+ std::stringstream sstr;
+ sstr << "MoCap Latency high: " << totalMocapLatency << " s." << std::endl;
+ for (const auto& item : mocapLatency) {
+ sstr << " Latency: " << item.name() << ": " << item.value() << " s." << std::endl;
+ }
+ ROS_WARN("%s", sstr.str().c_str());
+ }
+
+ if (printLatency) {
+ size_t i = 0;
+ for (const auto& item : mocapLatency) {
+ latencies.push_back({item.name(), item.value()});
+ latencyTotal[i] += item.value();
+ totalLatency += item.value();
+ latencyTotal.back() += item.value();
+ }
+ ++i;
+ }
+
+ // size_t latencyCount = client.GetLatencySampleCount().Count;
+ // for(size_t i = 0; i < latencyCount; ++i) {
+ // std::string sampleName = client.GetLatencySampleName(i).Name;
+ // double sampleValue = client.GetLatencySampleValue(sampleName).Value;
+
+ // ROS_INFO("Latency: %s: %f", sampleName.c_str(), sampleValue);
+ // }
+
+ // Get the unlabeled markers and create point cloud
+ if (!useMotionCaptureObjectTracking) {
+ // ToDO: If we switch our datastructure to pointcloud2 (here, for the ROS publisher, and libobjecttracker)
+ // we can avoid a copy here.
+ const auto& pointcloud = mocap->pointCloud();
+ markers->clear();
+ for (size_t i = 0; i < pointcloud.rows(); ++i) {
+ const auto &point = pointcloud.row(i);
+ markers->push_back(pcl::PointXYZ(point(0), point(1), point(2)));
+ }
+
+ msgPointCloud.header.seq += 1;
+ msgPointCloud.header.stamp = ros::Time::now();
+ msgPointCloud.points.resize(markers->size());
+ for (size_t i = 0; i < markers->size(); ++i) {
+ const pcl::PointXYZ& point = markers->at(i);
+ msgPointCloud.points[i].x = point.x;
+ msgPointCloud.points[i].y = point.y;
+ msgPointCloud.points[i].z = point.z;
+ }
+ m_pubPointCloud.publish(msgPointCloud);
+
+ if (logClouds) {
+ pointCloudLogger.log(markers);
+ }
+ }
+
+ if (useMotionCaptureObjectTracking || !interactiveObject.empty()) {
+ // get mocap rigid bodies
+ mocapRigidBodies = mocap->rigidBodies();
+ if (interactiveObject == "virtual") {
+ Eigen::Quaternionf quat(0, 0, 0, 1);
+ mocapRigidBodies.emplace(interactiveObject,
+ libmotioncapture::RigidBody(
+ interactiveObject,
+ m_lastInteractiveObjectPosition,
+ quat));
+ }
+ }
+
+ auto startRunGroups = std::chrono::high_resolution_clock::now();
+ std::vector<std::future<void> > handles;
+ for (auto group : m_groups) {
+ auto handle = std::async(std::launch::async, &CrazyflieGroup::runFast, group);
+ handles.push_back(std::move(handle));
+ }
+
+ for (auto& handle : handles) {
+ handle.wait();
+ }
+ auto endRunGroups = std::chrono::high_resolution_clock::now();
+ if (printLatency) {
+ std::chrono::duration<double> elapsedRunGroups = endRunGroups - startRunGroups;
+ latencies.push_back({"Run All Groups", elapsedRunGroups.count()});
+ latencyTotal[4] += elapsedRunGroups.count();
+ totalLatency += elapsedRunGroups.count();
+ latencyTotal.back() += elapsedRunGroups.count();
+ int groupId = 0;
+ for (auto group : m_groups) {
+ auto latency = group->lastLatency();
+ int radio = group->radio();
+ latencies.push_back({"Group " + std::to_string(radio) + " objectTracking", latency.objectTracking});
+ latencies.push_back({"Group " + std::to_string(radio) + " broadcasting", latency.broadcasting});
+ latencyTotal[5 + 2*groupId] += latency.objectTracking;
+ latencyTotal[6 + 2*groupId] += latency.broadcasting;
+ ++groupId;
+ }
+ }
+
+ auto endIteration = std::chrono::high_resolution_clock::now();
+ std::chrono::duration<double> elapsed = endIteration - startIteration;
+ double elapsedSeconds = elapsed.count();
+ if (elapsedSeconds > 0.009) {
+ ROS_WARN("Latency too high! Is %f s.", elapsedSeconds);
+ }
+
+ if (printLatency) {
+ ++latencyCount;
+ std::cout << "Latencies" << std::endl;
+ for (auto& latency : latencies) {
+ std::cout << latency.name << ": " << latency.secs * 1000 << " ms" << std::endl;
+ }
+ std::cout << "Total " << totalLatency * 1000 << " ms" << std::endl;
+ // // if (latencyCount % 100 == 0) {
+ std::cout << "Avg " << latencyCount << std::endl;
+ for (size_t i = 0; i < latencyTotal.size(); ++i) {
+ std::cout << latencyTotal[i] / latencyCount * 1000.0 << ",";
+ }
+ std::cout << std::endl;
+ // // }
+ }
+
+ // ROS_INFO("Latency: %f s", elapsedSeconds.count());
+
+ // m_fastQueue.callAvailable(ros::WallDuration(0));
+ }
+
+ if (logClouds) {
+ pointCloudLogger.flush();
+ }
+ }
+
+ // wait for other threads
+ for (auto& thread : threads) {
+ thread.join();
+ }
+ }
+
+ void runSlow()
+ {
+ while(ros::ok() && !m_isEmergency) {
+ m_queue.callAvailable(ros::WallDuration(0));
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
+ }
+ }
+
+private:
+
+ bool emergency(
+ std_srvs::Empty::Request& req,
+ std_srvs::Empty::Response& res)
+ {
+ ROS_FATAL("Emergency requested!");
+
+ for (size_t i = 0; i < m_broadcastingNumRepeats; ++i) {
+ for (auto& group : m_groups) {
+ group->emergency();
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(m_broadcastingDelayBetweenRepeatsMs));
+ }
+ m_isEmergency = true;
+
+ return true;
+ }
+
+ bool takeoff(
+ crazyswarm::Takeoff::Request& req,
+ crazyswarm::Takeoff::Response& res)
+ {
+ ROS_INFO("Takeoff!");
+
+ for (size_t i = 0; i < m_broadcastingNumRepeats; ++i) {
+ for (auto& group : m_groups) {
+ group->takeoff(req.height, req.duration.toSec(), req.groupMask);
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(m_broadcastingDelayBetweenRepeatsMs));
+ }
+
+ return true;
+ }
+
+ bool land(
+ crazyswarm::Land::Request& req,
+ crazyswarm::Land::Response& res)
+ {
+ ROS_INFO("Land!");
+
+ for (size_t i = 0; i < m_broadcastingNumRepeats; ++i) {
+ for (auto& group : m_groups) {
+ group->land(req.height, req.duration.toSec(), req.groupMask);
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(m_broadcastingDelayBetweenRepeatsMs));
+ }
+
+ return true;
+ }
+
+ bool goTo(
+ crazyswarm::GoTo::Request& req,
+ crazyswarm::GoTo::Response& res)
+ {
+ ROS_INFO("GoTo!");
+
+ for (size_t i = 0; i < m_broadcastingNumRepeats; ++i) {
+ for (auto& group : m_groups) {
+ group->goTo(req.goal.x, req.goal.y, req.goal.z, req.yaw, req.duration.toSec(), req.groupMask);
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(m_broadcastingDelayBetweenRepeatsMs));
+ }
+
+ return true;
+ }
+
+ bool startTrajectory(
+ crazyswarm::StartTrajectory::Request& req,
+ crazyswarm::StartTrajectory::Response& res)
+ {
+ ROS_INFO("Start trajectory!");
+
+ for (size_t i = 0; i < m_broadcastingNumRepeats; ++i) {
+ for (auto& group : m_groups) {
+ group->startTrajectory(req.trajectoryId, req.timescale, req.reversed, req.groupMask);
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(m_broadcastingDelayBetweenRepeatsMs));
+ }
+
+ return true;
+ }
+
+ bool updateParams(
+ crazyswarm::UpdateParams::Request& req,
+ crazyswarm::UpdateParams::Response& res)
+ {
+ ROS_INFO("UpdateParams!");
+
+ for (size_t i = 0; i < m_broadcastingNumRepeats; ++i) {
+ for (auto& group : m_groups) {
+ group->updateParams(req.params);
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(m_broadcastingDelayBetweenRepeatsMs));
+ }
+
+ return true;
+ }
+
+//
+ void readMarkerConfigurations(
+ std::vector<libobjecttracker::MarkerConfiguration>& markerConfigurations)
+ {
+ markerConfigurations.clear();
+ ros::NodeHandle nGlobal;
+ int numConfigurations;
+ nGlobal.getParam("numMarkerConfigurations", numConfigurations);
+ for (int i = 0; i < numConfigurations; ++i) {
+ markerConfigurations.push_back(pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>));
+ std::stringstream sstr;
+ sstr << "markerConfigurations/" << i << "/numPoints";
+ int numPoints;
+ nGlobal.getParam(sstr.str(), numPoints);
+
+ std::vector<double> offset;
+ std::stringstream sstr2;
+ sstr2 << "markerConfigurations/" << i << "/offset";
+ nGlobal.getParam(sstr2.str(), offset);
+ for (int j = 0; j < numPoints; ++j) {
+ std::stringstream sstr3;
+ sstr3 << "markerConfigurations/" << i << "/points/" << j;
+ std::vector<double> points;
+ nGlobal.getParam(sstr3.str(), points);
+ markerConfigurations.back()->push_back(pcl::PointXYZ(points[0] + offset[0], points[1] + offset[1], points[2] + offset[2]));
+ }
+ }
+ }
+
+ void readDynamicsConfigurations(
+ std::vector<libobjecttracker::DynamicsConfiguration>& dynamicsConfigurations)
+ {
+ ros::NodeHandle nGlobal;
+ int numConfigurations;
+ nGlobal.getParam("numDynamicsConfigurations", numConfigurations);
+ dynamicsConfigurations.resize(numConfigurations);
+ for (int i = 0; i < numConfigurations; ++i) {
+ std::stringstream sstr;
+ sstr << "dynamicsConfigurations/" << i;
+ nGlobal.getParam(sstr.str() + "/maxXVelocity", dynamicsConfigurations[i].maxXVelocity);
+ nGlobal.getParam(sstr.str() + "/maxYVelocity", dynamicsConfigurations[i].maxYVelocity);
+ nGlobal.getParam(sstr.str() + "/maxZVelocity", dynamicsConfigurations[i].maxZVelocity);
+ nGlobal.getParam(sstr.str() + "/maxPitchRate", dynamicsConfigurations[i].maxPitchRate);
+ nGlobal.getParam(sstr.str() + "/maxRollRate", dynamicsConfigurations[i].maxRollRate);
+ nGlobal.getParam(sstr.str() + "/maxYawRate", dynamicsConfigurations[i].maxYawRate);
+ nGlobal.getParam(sstr.str() + "/maxRoll", dynamicsConfigurations[i].maxRoll);
+ nGlobal.getParam(sstr.str() + "/maxPitch", dynamicsConfigurations[i].maxPitch);
+ nGlobal.getParam(sstr.str() + "/maxFitnessScore", dynamicsConfigurations[i].maxFitnessScore);
+ }
+ }
+
+ void readChannels(
+ std::set<int>& channels)
+ {
+ // read CF config
+ ros::NodeHandle nGlobal;
+
+ XmlRpc::XmlRpcValue crazyflies;
+ nGlobal.getParam("crazyflies", crazyflies);
+ ROS_ASSERT(crazyflies.getType() == XmlRpc::XmlRpcValue::TypeArray);
+
+ channels.clear();
+ for (int32_t i = 0; i < crazyflies.size(); ++i) {
+ ROS_ASSERT(crazyflies[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
+ XmlRpc::XmlRpcValue crazyflie = crazyflies[i];
+ int channel = crazyflie["channel"];
+ channels.insert(channel);
+ }
+ }
+
+private:
+ std::string m_worldFrame;
+ bool m_isEmergency;
+ ros::ServiceServer m_serviceEmergency;
+ ros::ServiceServer m_serviceStartTrajectory;
+ ros::ServiceServer m_serviceTakeoff;
+ ros::ServiceServer m_serviceLand;
+ ros::ServiceServer m_serviceGoTo;
+ ros::ServiceServer m_serviceUpdateParams;
+
+ ros::Publisher m_pubPointCloud;
+ // tf::TransformBroadcaster m_br;
+
+ std::vector<CrazyflieGroup*> m_groups;
+
+ ros::Subscriber m_subscribeVirtualInteractiveObject;
+ Eigen::Vector3f m_lastInteractiveObjectPosition;
+
+ int m_broadcastingNumRepeats;
+ int m_broadcastingDelayBetweenRepeatsMs;
+
+private:
+ // We have two callback queues
+ // 1. Fast queue handles pose and emergency callbacks. Those are high-priority and can be served quickly
+ // 2. Slow queue handles all other requests.
+ // Each queue is handled in its own thread. We don't want a thread per CF to make sure that the fast queue
+ // gets called frequently enough.
+
+ ros::CallbackQueue m_queue;
+ // ros::CallbackQueue m_slowQueue;
+};
+
+int main(int argc, char **argv)
+{
+ // raise(SIGSTOP);
+
+ ros::init(argc, argv, "crazyflie_server");
+
+ // ros::NodeHandle n("~");
+ // std::string worldFrame;
+ // n.param<std::string>("world_frame", worldFrame, "/world");
+ // std::string broadcastUri;
+ // n.getParam("broadcast_uri", broadcastUri);
+
+ CrazyflieServer server;//(broadcastUri, worldFrame);
+
+ // read CF config
+ ros::NodeHandle nGlobal;
+
+ XmlRpc::XmlRpcValue crazyflies;
+ nGlobal.getParam("crazyflies", crazyflies);
+ ROS_ASSERT(crazyflies.getType() == XmlRpc::XmlRpcValue::TypeArray);
+
+ std::set<int> cfIds;
+ for (int32_t i = 0; i < crazyflies.size(); ++i)
+ {
+ ROS_ASSERT(crazyflies[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
+ XmlRpc::XmlRpcValue crazyflie = crazyflies[i];
+ int id = crazyflie["id"];
+ int channel = crazyflie["channel"];
+ if (cfIds.find(id) != cfIds.end()) {
+ ROS_FATAL("CF with the same id twice in configuration!");
+ return 1;
+ }
+ cfIds.insert(id);
+ }
+
+ // ROS_INFO("All CFs are ready!");
+
+ server.run();
+
+ return 0;
+}
diff --git a/real_robot/crazyswarm/src/crazyswarm_teleop.cpp b/real_robot/crazyswarm/src/crazyswarm_teleop.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5c62bb64347f752b6bc2104ffa415a0457ca8f7
--- /dev/null
+++ b/real_robot/crazyswarm/src/crazyswarm_teleop.cpp
@@ -0,0 +1,123 @@
+#include <fstream>
+#include <sstream>
+
+#include <ros/ros.h>
+#include <tf/transform_listener.h>
+
+#include <sensor_msgs/Joy.h>
+#include <std_srvs/Empty.h>
+
+#include <crazyswarm/Takeoff.h>
+#include <crazyswarm/Land.h>
+
+namespace Xbox360Buttons {
+
+ enum {
+ Green = 0,
+ Red = 1,
+ Blue = 2,
+ Yellow = 3,
+ LB = 4,
+ RB = 5,
+ Back = 6,
+ Start = 7,
+ COUNT = 8,
+ };
+
+}
+
+class Manager
+{
+public:
+
+ Manager()
+ : m_subscribeJoy()
+ , m_serviceEmergency()
+ , m_serviceTakeoff()
+ , m_serviceLand()
+ {
+ ros::NodeHandle nh;
+ m_subscribeJoy = nh.subscribe("/joy", 1, &Manager::joyChanged, this);
+
+ ROS_INFO("Wait for services...");
+
+ ros::service::waitForService("/emergency");
+ m_serviceEmergency = nh.serviceClient<std_srvs::Empty>("/emergency");
+ ros::service::waitForService("/takeoff");
+ m_serviceTakeoff = nh.serviceClient<crazyswarm::Takeoff>("/takeoff");
+ ros::service::waitForService("/land");
+ m_serviceLand = nh.serviceClient<crazyswarm::Land>("/land");
+
+ ROS_INFO("Manager ready.");
+ }
+
+ ~Manager()
+ {
+ }
+
+private:
+ void joyChanged(
+ const sensor_msgs::Joy::ConstPtr& msg)
+ {
+ static std::vector<int> lastButtonState(Xbox360Buttons::COUNT);
+
+ if (msg->buttons.size() >= Xbox360Buttons::COUNT
+ && lastButtonState.size() >= Xbox360Buttons::COUNT)
+ {
+ if (msg->buttons[Xbox360Buttons::Red] == 1 && lastButtonState[Xbox360Buttons::Red] == 0) {
+ emergency();
+ }
+ if (msg->buttons[Xbox360Buttons::Start] == 1 && lastButtonState[Xbox360Buttons::Start] == 0) {
+ takeoff();
+ }
+ if (msg->buttons[Xbox360Buttons::Back] == 1 && lastButtonState[Xbox360Buttons::Back] == 0) {
+ land();
+ }
+ }
+
+ lastButtonState = msg->buttons;
+ }
+
+ void emergency()
+ {
+ ROS_INFO("emergency requested...");
+ std_srvs::Empty srv;
+ m_serviceEmergency.call(srv);
+ ROS_INFO("Done.");
+ }
+
+ void takeoff()
+ {
+ crazyswarm::Takeoff srv;
+ srv.request.groupMask = 0;
+ srv.request.height = 0.5;
+ srv.request.duration = ros::Duration(2.0);
+ m_serviceTakeoff.call(srv);
+ }
+
+ void land()
+ {
+ crazyswarm::Land srv;
+ srv.request.groupMask = 0;
+ srv.request.height = 0.05;
+ srv.request.duration = ros::Duration(3.5);
+ m_serviceLand.call(srv);
+ }
+
+private:
+ ros::Subscriber m_subscribeJoy;
+
+ ros::ServiceClient m_serviceEmergency;
+ ros::ServiceClient m_serviceTakeoff;
+ ros::ServiceClient m_serviceLand;
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "manager");
+
+ Manager manager;
+ ros::spin();
+
+ return 0;
+}
diff --git a/real_robot/crazyswarm/src/mocap_helper.cpp b/real_robot/crazyswarm/src/mocap_helper.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..702f6e77b37eda2942e0d887a998aa4d6b68b8e5
--- /dev/null
+++ b/real_robot/crazyswarm/src/mocap_helper.cpp
@@ -0,0 +1,68 @@
+#include <iostream>
+#include <vector>
+
+#include <ros/ros.h>
+
+// Motion Capture
+#include <libmotioncapture/motioncapture.h>
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "mocap_helper");
+
+ ros::NodeHandle nl("~");
+
+ std::string motionCaptureType;
+ nl.param<std::string>("motion_capture_type", motionCaptureType, "vicon");
+
+ std::map<std::string, std::string> cfg;
+ std::string hostname;
+ nl.getParam("motion_capture_host_name", hostname);
+ cfg["hostname"] = hostname;
+ if (nl.hasParam("motion_capture_interface_ip")) {
+ std::string interface_ip;
+ nl.param<std::string>("motion_capture_interface_ip", interface_ip);
+ cfg["interface_ip"] = interface_ip;
+ }
+
+ std::unique_ptr<libmotioncapture::MotionCapture> mocap(libmotioncapture::MotionCapture::connect(motionCaptureType, cfg));
+ if (!mocap) {
+ throw std::runtime_error("Unknown motion capture type!");
+ }
+
+ for (size_t frameId = 0; ros::ok(); ++frameId) {
+ std::cout << "frame " << frameId << ":" << std::endl;
+ // Get a frame
+ mocap->waitForNextFrame();
+ const auto& markers = mocap->pointCloud();
+
+ std::cout << " points:" << std::endl;
+
+ for (size_t i = 0; i < markers.rows(); ++i) {
+ const auto& point = markers.row(i);
+ std::cout << " \"" << i << "\": [" << point(0) << "," << point(1) << "," << point(2) << "]" << std::endl;
+ }
+
+ if (mocap->supportsRigidBodyTracking()) {
+ const auto& rigidBodies = mocap->rigidBodies();
+
+ std::cout << " rigidBodies:" << std::endl;
+
+ for (auto const& kv: rigidBodies) {
+ const auto& body = kv.second;
+ std::cout << " \"" << body.name() << "\":" << std::endl;
+
+ Eigen::Vector3f position = body.position();
+ Eigen::Quaternionf rotation = body.rotation();
+ std::cout << " position: [" << position(0) << ", " << position(1) << ", " << position(2) << "]" << std::endl;
+ std::cout << " rotation: [" << rotation.w() << ", " << rotation.vec()(0) << ", "
+ << rotation.vec()(1) << ", " << rotation.vec()(2) << "]" << std::endl;
+ }
+ }
+
+
+ ros::spinOnce();
+ }
+
+ return 0;
+}
diff --git a/real_robot/crazyswarm/srv/GoTo.srv b/real_robot/crazyswarm/srv/GoTo.srv
new file mode 100644
index 0000000000000000000000000000000000000000..bc9621bef1e71e6b70ab1399e4e8a0d05b2dcd93
--- /dev/null
+++ b/real_robot/crazyswarm/srv/GoTo.srv
@@ -0,0 +1,6 @@
+uint8 groupMask
+bool relative
+geometry_msgs/Point goal
+float32 yaw # deg
+duration duration
+---
diff --git a/real_robot/crazyswarm/srv/Land.srv b/real_robot/crazyswarm/srv/Land.srv
new file mode 100644
index 0000000000000000000000000000000000000000..5e1e83967ab7d865acdff3a0a8eefb2100d8ce71
--- /dev/null
+++ b/real_robot/crazyswarm/srv/Land.srv
@@ -0,0 +1,4 @@
+uint8 groupMask
+float32 height
+duration duration
+---
diff --git a/real_robot/crazyswarm/srv/NotifySetpointsStop.srv b/real_robot/crazyswarm/srv/NotifySetpointsStop.srv
new file mode 100644
index 0000000000000000000000000000000000000000..bf91b2cab04424f4bf1ac962a3be005fa5f9c3d8
--- /dev/null
+++ b/real_robot/crazyswarm/srv/NotifySetpointsStop.srv
@@ -0,0 +1,3 @@
+uint8 groupMask
+uint32 remainValidMillisecs
+---
diff --git a/real_robot/crazyswarm/srv/SetGroupMask.srv b/real_robot/crazyswarm/srv/SetGroupMask.srv
new file mode 100644
index 0000000000000000000000000000000000000000..e196632255acdb1f4a05df07053a74a7872a3281
--- /dev/null
+++ b/real_robot/crazyswarm/srv/SetGroupMask.srv
@@ -0,0 +1,2 @@
+uint8 groupMask
+---
diff --git a/real_robot/crazyswarm/srv/StartTrajectory.srv b/real_robot/crazyswarm/srv/StartTrajectory.srv
new file mode 100644
index 0000000000000000000000000000000000000000..e5bc49744790d95543e0d07c7966877b94a68721
--- /dev/null
+++ b/real_robot/crazyswarm/srv/StartTrajectory.srv
@@ -0,0 +1,6 @@
+uint8 groupMask
+uint8 trajectoryId
+float32 timescale
+bool reversed
+bool relative
+---
diff --git a/real_robot/crazyswarm/srv/Stop.srv b/real_robot/crazyswarm/srv/Stop.srv
new file mode 100644
index 0000000000000000000000000000000000000000..e196632255acdb1f4a05df07053a74a7872a3281
--- /dev/null
+++ b/real_robot/crazyswarm/srv/Stop.srv
@@ -0,0 +1,2 @@
+uint8 groupMask
+---
diff --git a/real_robot/crazyswarm/srv/Takeoff.srv b/real_robot/crazyswarm/srv/Takeoff.srv
new file mode 100644
index 0000000000000000000000000000000000000000..5e1e83967ab7d865acdff3a0a8eefb2100d8ce71
--- /dev/null
+++ b/real_robot/crazyswarm/srv/Takeoff.srv
@@ -0,0 +1,4 @@
+uint8 groupMask
+float32 height
+duration duration
+---
diff --git a/real_robot/crazyswarm/srv/UpdateParams.srv b/real_robot/crazyswarm/srv/UpdateParams.srv
new file mode 100644
index 0000000000000000000000000000000000000000..fb2eefe77bf0fd96ec8c0d59e13e0e7fbcb5bce0
--- /dev/null
+++ b/real_robot/crazyswarm/srv/UpdateParams.srv
@@ -0,0 +1,2 @@
+string[] params
+---
diff --git a/real_robot/crazyswarm/srv/UploadTrajectory.srv b/real_robot/crazyswarm/srv/UploadTrajectory.srv
new file mode 100644
index 0000000000000000000000000000000000000000..6025a79a83972b6b376c7b039a947cf614ccf06f
--- /dev/null
+++ b/real_robot/crazyswarm/srv/UploadTrajectory.srv
@@ -0,0 +1,4 @@
+uint8 trajectoryId
+uint32 pieceOffset
+TrajectoryPolynomialPiece[] pieces
+---
diff --git a/real_robot/talker/CMakeLists.txt b/real_robot/talker/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e4eeea8cc9b690c4bb8d4625fee9dd35de1ad515
--- /dev/null
+++ b/real_robot/talker/CMakeLists.txt
@@ -0,0 +1,20 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(talker)
+
+## Find catkin and any catkin packages
+find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs genmsg)
+
+## Declare ROS messages and services
+
+## Generate added messages and services
+
+## Declare a catkin package
+catkin_package(
+ CATKIN_DEPENDS
+ std_msgs
+
+)
+include_directories(include ${catkin_INCLUDE_DIRS})
+
+add_executable(talker src/talker.cpp)
+target_link_libraries(talker ${catkin_LIBRARIES})
diff --git a/real_robot/talker/package.xml b/real_robot/talker/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..14078bd7dc8d0703ce49dae2599b73c8d914a026
--- /dev/null
+++ b/real_robot/talker/package.xml
@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<package>
+ <name>talker</name>
+ <version>0.0.0</version>
+ <description>The talker package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="spot@todo.todo">spot</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/talker</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>std_msgs</build_depend>
+ <run_depend>std_msgs</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/real_robot/talker/src/talker.cpp b/real_robot/talker/src/talker.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9cf75a95088482cee4a6ff81fd211b5dfaa907dc
--- /dev/null
+++ b/real_robot/talker/src/talker.cpp
@@ -0,0 +1,85 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+
+#include <sstream>
+
+/**
+ * This tutorial demonstrates simple sending of messages over the ROS system.
+ */
+int main(int argc, char **argv)
+{
+ /**
+ * The ros::init() function needs to see argc and argv so that it can perform
+ * any ROS arguments and name remapping that were provided at the command line.
+ * For programmatic remappings you can use a different version of init() which takes
+ * remappings directly, but for most command-line programs, passing argc and argv is
+ * the easiest way to do it. The third argument to init() is the name of the node.
+ *
+ * You must call one of the versions of ros::init() before using any other
+ * part of the ROS system.
+ */
+ ros::init(argc, argv, "talker");
+
+ /**
+ * NodeHandle is the main access point to communications with the ROS system.
+ * The first NodeHandle constructed will fully initialize this node, and the last
+ * NodeHandle destructed will close down the node.
+ */
+ ros::NodeHandle n;
+
+ /**
+ * The advertise() function is how you tell ROS that you want to
+ * publish on a given topic name. This invokes a call to the ROS
+ * master node, which keeps a registry of who is publishing and who
+ * is subscribing. After this advertise() call is made, the master
+ * node will notify anyone who is trying to subscribe to this topic name,
+ * and they will in turn negotiate a peer-to-peer connection with this
+ * node. advertise() returns a Publisher object which allows you to
+ * publish messages on that topic through a call to publish(). Once
+ * all copies of the returned Publisher object are destroyed, the topic
+ * will be automatically unadvertised.
+ *
+ * The second parameter to advertise() is the size of the message queue
+ * used for publishing messages. If messages are published more quickly
+ * than we can send them, the number here specifies how many messages to
+ * buffer up before throwing some away.
+ */
+ ros::Publisher chatter_pub = n.advertise<std_msgs::String>("chatter", 1000);
+
+ ros::Rate loop_rate(10);
+
+ /**
+ * A count of how many messages we have sent. This is used to create
+ * a unique string for each message.
+ */
+ int count = 0;
+ while (ros::ok())
+ {
+ /**
+ * This is a message object. You stuff it with data, and then publish it.
+ */
+ std_msgs::String msg;
+
+ std::stringstream ss;
+ ss << "hello world " << count;
+ msg.data = ss.str();
+
+ ROS_INFO("%s", msg.data.c_str());
+
+ /**
+ * The publish() function is how you send messages. The parameter
+ * is the message object. The type of this object must agree with the type
+ * given as a template parameter to the advertise<>() call, as was done
+ * in the constructor above.
+ */
+ chatter_pub.publish(msg);
+
+ ros::spinOnce();
+
+ loop_rate.sleep();
+ ++count;
+ }
+
+
+ return 0;
+}
diff --git a/simulation/untitled.asv b/simulation/untitled.asv
new file mode 100644
index 0000000000000000000000000000000000000000..494a0dcd8a75a0faa9729532918fa15d57d95904
--- /dev/null
+++ b/simulation/untitled.asv
@@ -0,0 +1,15 @@
+A = [3 1 5;
+ 6 2 6];
+B = [6;4;-1];
+C = [2 4; 3 6; -1 2];
+D = [5 1; 3 1]
+E = [3 -2; 1 4]
+F = [2 1 3; 5 7 -2]
+
+A + F
+E-D
+C+B
+C*D
+A*F
+C'
+F'*E
\ No newline at end of file
diff --git a/simulation/untitled.m b/simulation/untitled.m
new file mode 100644
index 0000000000000000000000000000000000000000..10cd5dfe5f445e611a4c157622883fb7af2b90e2
--- /dev/null
+++ b/simulation/untitled.m
@@ -0,0 +1,16 @@
+A = [3 1 5;
+ 6 2 6]
+B = [6;4;-1]
+C = [2 4; 3 6; -1 2]
+D = [5 1; 3 1]
+E = [3 -2; 1 4]
+F = [2 1 3; 5 7 -2]
+
+disp("*******************************************")
+A + F
+E-D
+C+B
+C*D
+%A*F
+C'
+F'*E
\ No newline at end of file