├── .github └── workflows │ ├── benchmark-daily.yml │ ├── build-self-hosted.yml │ ├── build.yml │ └── profile.yml ├── .gitignore ├── .gitmodules ├── .vscode └── launch.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── dbcbs_ros ├── CMakeLists.txt ├── config │ ├── config.rviz │ ├── crazyflies.yaml │ ├── motion_capture.yaml │ ├── server.yaml │ └── teleop.yaml ├── dbcbs_ros │ ├── __init__.py │ ├── cmd_full_state.py │ ├── data │ │ ├── 13_09.yaml │ │ ├── forest_1.yaml │ │ ├── forest_2.yaml │ │ ├── forest_3.yaml │ │ ├── forest_4.yaml │ │ ├── obstacle.yaml │ │ ├── swap2.yaml │ │ ├── swap4_05.yaml │ │ └── swap4_1.yaml │ └── waypoint_plot.py ├── launch │ └── launch.py ├── package.xml └── setup.cfg ├── example ├── alcove_unicycle.yaml ├── alcove_unicycle_sphere.yaml ├── algorithms.yaml ├── at_goal_unicycle.yaml ├── at_goal_unicycle_sphere.yaml ├── bugtrap.yaml ├── classic.yaml ├── forest.yaml ├── gen_p10_n16_0.yaml ├── gen_p10_n16_0_hetero.yaml ├── gen_p10_n16_0_unicycle_sphere.yaml ├── gen_p10_n2_0_hetero.yaml ├── gen_p10_n2_0_unicycle_sphere.yaml ├── gen_p10_n2_1_hetero.yaml ├── gen_p10_n2_1_unicycle_sphere.yaml ├── gen_p10_n2_2_hetero.yaml ├── gen_p10_n2_2_unicycle_sphere.yaml ├── gen_p10_n2_3_hetero.yaml ├── gen_p10_n2_3_unicycle_sphere.yaml ├── gen_p10_n2_4_hetero.yaml ├── gen_p10_n2_4_unicycle_sphere.yaml ├── gen_p10_n2_5_hetero.yaml ├── gen_p10_n2_5_unicycle_sphere.yaml ├── gen_p10_n2_6_hetero.yaml ├── gen_p10_n2_6_unicycle_sphere.yaml ├── gen_p10_n2_7_hetero.yaml ├── gen_p10_n2_7_unicycle_sphere.yaml ├── gen_p10_n2_8_hetero.yaml ├── gen_p10_n2_8_unicycle_sphere.yaml ├── gen_p10_n2_9_hetero.yaml ├── gen_p10_n2_9_unicycle_sphere.yaml ├── gen_p10_n4_0_hetero.yaml ├── gen_p10_n4_0_unicycle_sphere.yaml ├── gen_p10_n4_1_hetero.yaml ├── gen_p10_n4_1_unicycle_sphere.yaml ├── gen_p10_n4_2_hetero.yaml ├── gen_p10_n4_2_unicycle_sphere.yaml ├── gen_p10_n4_3_hetero.yaml ├── gen_p10_n4_3_unicycle_sphere.yaml ├── gen_p10_n4_4_hetero.yaml ├── gen_p10_n4_4_unicycle_sphere.yaml ├── gen_p10_n4_5_hetero.yaml ├── gen_p10_n4_5_unicycle_sphere.yaml ├── gen_p10_n4_6_hetero.yaml ├── gen_p10_n4_6_unicycle_sphere.yaml ├── gen_p10_n4_7_hetero.yaml ├── gen_p10_n4_7_unicycle_sphere.yaml ├── gen_p10_n4_8_hetero.yaml ├── gen_p10_n4_8_unicycle_sphere.yaml ├── gen_p10_n4_9_hetero.yaml ├── gen_p10_n4_9_unicycle_sphere.yaml ├── gen_p10_n8_0_hetero.yaml ├── gen_p10_n8_0_unicycle_sphere.yaml ├── gen_p10_n8_1_hetero.yaml ├── gen_p10_n8_1_unicycle_sphere.yaml ├── gen_p10_n8_2_hetero.yaml ├── gen_p10_n8_2_unicycle_sphere.yaml ├── gen_p10_n8_3_hetero.yaml ├── gen_p10_n8_3_unicycle_sphere.yaml ├── gen_p10_n8_4_hetero.yaml ├── gen_p10_n8_4_unicycle_sphere.yaml ├── gen_p10_n8_5_hetero.yaml ├── gen_p10_n8_5_unicycle_sphere.yaml ├── gen_p10_n8_6_hetero.yaml ├── gen_p10_n8_6_unicycle_sphere.yaml ├── gen_p10_n8_7_hetero.yaml ├── gen_p10_n8_7_unicycle_sphere.yaml ├── gen_p10_n8_8_hetero.yaml ├── gen_p10_n8_8_unicycle_sphere.yaml ├── gen_p10_n8_9_hetero.yaml ├── gen_p10_n8_9_unicycle_sphere.yaml ├── guess_indiv_straight.yaml ├── guess_joint_straight.yaml ├── infeasible_0.yaml ├── makespan_vs_soc_0.yaml ├── makespan_vs_soc_1.yaml ├── parallelpark.yaml ├── straight.yaml ├── swap1_double_integrator.yaml ├── swap1_trailer.yaml ├── swap1_unicycle.yaml ├── swap1_unicycle2.yaml ├── swap1_unicycle_sphere.yaml ├── swap2_demo.yaml ├── swap2_double_integrator.yaml ├── swap2_hetero.yaml ├── swap2_trailer.yaml ├── swap2_unicycle.yaml ├── swap2_unicycle2.yaml ├── swap2_unicycle_sphere.yaml ├── swap3_double_integrator.yaml ├── swap3_trailer.yaml ├── swap3_unicycle.yaml ├── swap3_unicycle2.yaml ├── swap3_unicycle_sphere.yaml ├── swap4_demo.yaml ├── swap4_double_integrator.yaml ├── swap4_trailer.yaml ├── swap4_unicycle.yaml ├── swap4_unicycle2.yaml ├── swap4_unicycle_sphere.yaml ├── wall.yaml ├── window2_unicycle.yaml ├── window3_unicycle.yaml ├── window4_demo.yaml ├── window4_double_integrator.yaml ├── window4_trailer.yaml ├── window4_unicycle.yaml ├── window4_unicycle2.yaml └── window4_unicycle_sphere.yaml ├── k_cbs └── .gitignore ├── more_testing ├── classic_solution.yaml ├── swap2_trailer_db.yaml ├── swap3_unicycle_solution.yaml └── swap3_unicycle_solution_db.yaml ├── scripts ├── benchmark.py ├── benchmark_stats.py ├── benchmark_table.py ├── checker.py ├── example.py ├── gen_motion_primitive.py ├── gen_motion_primitive_komo.py ├── gen_random_example.py ├── main_dbastar.py ├── main_dbcbs.py ├── main_kcbs.py ├── main_komo.py ├── main_ompl.py ├── main_s2m2.py ├── paper_tables.py ├── plot_stats.py ├── robots.py ├── scp.py ├── test_standalone.py ├── translate_g.py ├── utils_motion_primitives.py ├── utils_optimization.py └── visualize.py ├── src ├── db_astar.hpp ├── db_cbs.cpp ├── fclHelper.hpp ├── fclStateValidityChecker.hpp ├── main_dbastar.cpp ├── main_kcbs.cpp ├── main_ompl.cpp ├── main_ompl_geometric.cpp ├── planresult.hpp ├── python_bindings.cpp ├── robotStatePropagator.hpp ├── robots.cpp └── robots.h └── tuning └── unicycle_first_order_0 ├── algorithms.yaml ├── unicycle_first_order_0_mprim.mprim └── unicycle_first_order_0_mprim.yaml /.github/workflows/benchmark-daily.yml: -------------------------------------------------------------------------------- 1 | name: Benchmark 2 | 3 | on: 4 | # every midnight 5 | schedule: 6 | - cron: '0 0 * * *' 7 | # manual trigger 8 | workflow_dispatch: 9 | 10 | env: 11 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 12 | BUILD_TYPE: Release 13 | JOBS: 12 14 | 15 | jobs: 16 | build: 17 | runs-on: self-hosted 18 | steps: 19 | - uses: actions/checkout@v3 20 | with: 21 | submodules: recursive 22 | 23 | - name: Configure CMake 24 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 25 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 26 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_PREFIX_PATH="~/__local;/opt/openrobots/" 27 | 28 | - name: Build 29 | # Build your program with the given configuration 30 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} -j ${{env.JOBS}} 31 | 32 | - name: Get motion primitives 33 | run: | 34 | wget https://tubcloud.tu-berlin.de/s/CijbRaJadf6JwH3/download 35 | unzip download 36 | rm download 37 | 38 | - name: Run Benchmark 39 | run: | 40 | cd ${{github.workspace}}/build 41 | python3 ../scripts/benchmark.py 42 | 43 | - uses: actions/upload-artifact@v3 44 | with: 45 | name: results 46 | path: results/ 47 | -------------------------------------------------------------------------------- /.github/workflows/build-self-hosted.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | # Only trigger if manually requested 4 | on: [workflow_dispatch] 5 | # on: 6 | # push: 7 | # branches: [ "main" ] 8 | # pull_request: 9 | # branches: [ "main" ] 10 | 11 | env: 12 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 13 | BUILD_TYPE: Release 14 | 15 | jobs: 16 | build: 17 | # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. 18 | # You can convert this to a matrix build if you need cross-platform coverage. 19 | # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix 20 | runs-on: self-hosted 21 | 22 | 23 | steps: 24 | - uses: actions/checkout@v3 25 | with: 26 | submodules: recursive 27 | 28 | # - name: Install system dependencies 29 | # run: sudo apt update && sudo apt install libboost-all-dev libfcl-dev cmake libeigen3-dev libyaml-cpp-dev liblz4-dev -y 30 | 31 | # - name: install crocoddyl 32 | # run: | 33 | # sudo tee /etc/apt/sources.list.d/robotpkg.list << EOF 34 | # deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg 35 | # EOF 36 | 37 | # curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add - 38 | # sudo apt update 39 | # sudo apt install robotpkg-py310-crocoddyl -y 40 | 41 | # - name: Install latest ompl 42 | # run: | 43 | # mkdir __local 44 | # git clone https://github.com/ompl/ompl 45 | # cd ompl && mkdir build && cd build && cmake .. -DCMAKE_INSTALL_PREFIX=~/__local && make install -j8 46 | 47 | - name: Configure CMake 48 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 49 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 50 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_PREFIX_PATH="~/__local;/opt/openrobots/" 51 | 52 | - name: Build 53 | # Build your program with the given configuration 54 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 55 | 56 | # - name: Test 57 | # working-directory: ${{github.workspace}}/build 58 | # # Execute tests defined by the CMake configuration. 59 | # # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail 60 | # run: ctest -C ${{env.BUILD_TYPE}} --rerun-failed --output-on-failure 61 | -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | # Triggers the workflow on all push or pull request events 4 | on: [push] 5 | # on: 6 | # push: 7 | # branches: [ "main" ] 8 | # pull_request: 9 | # branches: [ "main" ] 10 | 11 | env: 12 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 13 | BUILD_TYPE: Release 14 | 15 | jobs: 16 | build: 17 | # The CMake configure and build commands are platform agnostic and should work equally well on Windows or Mac. 18 | # You can convert this to a matrix build if you need cross-platform coverage. 19 | # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix 20 | runs-on: ubuntu-latest 21 | 22 | 23 | steps: 24 | - uses: actions/checkout@v3 25 | with: 26 | submodules: recursive 27 | 28 | - name: Install system dependencies 29 | run: sudo apt update && sudo apt install libboost-all-dev libfcl-dev cmake libeigen3-dev libyaml-cpp-dev liblz4-dev libmsgpack-dev -y 30 | 31 | - name: install crocoddyl 32 | run: | 33 | sudo tee /etc/apt/sources.list.d/robotpkg.list << EOF 34 | deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -sc) robotpkg 35 | EOF 36 | 37 | curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key | sudo apt-key add - 38 | sudo apt update 39 | sudo apt install robotpkg-py310-crocoddyl coinor-libipopt-dev -y 40 | 41 | - name: Install latest ompl 42 | run: | 43 | mkdir __local 44 | git clone https://github.com/ompl/ompl 45 | cd ompl 46 | git checkout e2994e5 47 | mkdir build && cd build && cmake .. -DCMAKE_INSTALL_PREFIX=~/__local && make install -j8 48 | 49 | - name: Configure CMake 50 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 51 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 52 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_PREFIX_PATH="~/__local;/opt/openrobots/" 53 | 54 | - name: Build 55 | # Build your program with the given configuration 56 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} 57 | 58 | # - name: Test 59 | # working-directory: ${{github.workspace}}/build 60 | # # Execute tests defined by the CMake configuration. 61 | # # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail 62 | # run: ctest -C ${{env.BUILD_TYPE}} --rerun-failed --output-on-failure 63 | -------------------------------------------------------------------------------- /.github/workflows/profile.yml: -------------------------------------------------------------------------------- 1 | name: Profile 2 | 3 | on: 4 | # every midnight 5 | schedule: 6 | - cron: '0 0 * * *' 7 | # manual trigger 8 | workflow_dispatch: 9 | 10 | env: 11 | # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.) 12 | BUILD_TYPE: Release 13 | JOBS: 12 14 | 15 | jobs: 16 | build: 17 | runs-on: self-hosted 18 | steps: 19 | - uses: actions/checkout@v3 20 | with: 21 | submodules: recursive 22 | 23 | - name: Configure CMake 24 | # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make. 25 | # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type 26 | run: cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}} -DCMAKE_PREFIX_PATH="~/__local;/opt/openrobots/" 27 | 28 | - name: Build 29 | # Build your program with the given configuration 30 | run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}} -j ${{env.JOBS}} 31 | 32 | - name: Get motion primitives 33 | run: | 34 | wget https://tubcloud.tu-berlin.de/s/CijbRaJadf6JwH3/download 35 | unzip download 36 | rm download 37 | 38 | - name: Run Benchmark 39 | run: | 40 | cd ${{github.workspace}}/build 41 | python3 ../scripts/benchmark.py 42 | 43 | - uses: actions/upload-artifact@v3 44 | with: 45 | name: results 46 | path: results/ 47 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | buildRelease/ 2 | buildDebug/ 3 | results/ 4 | motions 5 | __pycache__/ 6 | demo/ 7 | new_format_motions/ 8 | meshes/ 9 | .vscode/ -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "Kinodynamic-Conflict-Based-Search"] 2 | path = k_cbs/Kinodynamic-Conflict-Based-Search 3 | url = git@github.com:IMRCLab/Kinodynamic-Conflict-Based-Search.git 4 | [submodule "k_cbs/K-CBS-Demos"] 5 | path = k_cbs/K-CBS-Demos 6 | url = git@github.com:IMRCLab/K-CBS-Demos.git 7 | [submodule "s2m2"] 8 | path = s2m2 9 | url = git@github.com:IMRCLab/s2m2.git 10 | [submodule "dynoplan"] 11 | path = dynoplan 12 | url = git@github.com:quimortiz/dynoplan.git 13 | -------------------------------------------------------------------------------- /.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "My Debug Launch", 9 | "type": "cppdbg", 10 | "request": "launch", 11 | "program": "${workspaceFolder}/buildDebug/db_cbs", 12 | "args": [ 13 | // "-i", "/home/akmarak-laptop/IMRC/db-CBS/example/parallelpark.yaml", 14 | // "-m", "/home/akmarak-laptop/IMRC/db-CBS/results/dbg/motions.msgpack", 15 | // "-o", "/home/akmarak-laptop/IMRC/db-CBS/example/result/result_dbcbs.yaml" 16 | ], 17 | "cwd": "${workspaceFolder}/buildDebug", 18 | "externalConsole": false, 19 | "MIMode": "gdb", 20 | "setupCommands": [ 21 | { 22 | "description": "Enable pretty-printing for gdb", 23 | "text": "-enable-pretty-printing", 24 | "ignoreFailures": true 25 | } 26 | ] 27 | } 28 | ] 29 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Intelligent Multi-Robot Coordination Lab @ TU Berlin, Germany 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # db-CBS: Discontinuity-Bounded Conflict-Based Search for Multi-Robot Kinodynamic Motion Planning 2 | Db-CBS is a multi-robot kinodynamic motion planner that enables a team of robots with different dynamics, actuation limits, and shapes to reach their goals in challenging environments. 3 | It solves this problem by combining the Multi-Agent Path Finding (MAPF) optimal solver Conflict-Based Search (CBS), the single-robot kinodynamic motion planner discontinuity-bounded A* (db-A*), and non- linear trajectory optimization. 4 | 5 | Paper on [arXiv](https://arxiv.org/abs/2309.16445) and [Video](https://youtu.be/1mglNQOmOLE) are available. 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | ## Get primitives 15 | 16 | The primitives are on the TUB cloud. The easiest is to use a symlink: 17 | 18 | ``` 19 | ln -s /home/${USER}/tubCloud/projects/db-cbs/motions motions 20 | ``` 21 | 22 | Alternatively, download a copy 23 | 24 | ``` 25 | wget https://tubcloud.tu-berlin.de/s/CijbRaJadf6JwH3/download 26 | unzip download 27 | rm download 28 | ``` 29 | 30 | ## Building 31 | 32 | Tested on Ubuntu 22.04. 33 | 34 | ``` 35 | mkdir buildRelease 36 | cd buildRelease 37 | cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_PREFIX_PATH="/opt/openrobots/" .. 38 | make -j 39 | ``` 40 | 41 | ## Running 42 | 43 | ``` 44 | cd buildRelease 45 | python3 ../scripts/benchmark.py 46 | ``` 47 | 48 | ## ROS 49 | ROS2 workspace needs to have [crazyswarm2](https://github.com/IMRCLab/crazyswarm2). 50 | 51 | **Set Up** 52 | 53 | Add a symlink of dbcbs_ros to your ROS2 workspace 54 | 55 | ``` 56 | ln /dbcbs_ros ros2_ws/src/ -s 57 | ``` 58 | 59 | **Build** 60 | 61 | ``` 62 | colcon build --symlink-install 63 | ``` 64 | 65 | **Usage** 66 | 67 | Note that all configuration files are the ones used in cvmrs_ros/config. This allows to commit those files without changing the default value of the (public) crazyswarm2 repository. 68 | 69 | ``` 70 | ros2 launch dbcbs_ros launch.py 71 | ``` 72 | 73 | and in a separate terminal 74 | 75 | ``` 76 | ros2 run dbcbs_ros multi_trajectory -------------------------------------------------------------------------------- /dbcbs_ros/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(dbcbs_ros) 3 | 4 | # find dependencies 5 | find_package(ament_cmake REQUIRED) 6 | find_package(ament_cmake_python REQUIRED) 7 | 8 | # Install Python modules 9 | ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) 10 | 11 | # Install launch and config files. 12 | install(DIRECTORY 13 | config 14 | launch 15 | DESTINATION share/${PROJECT_NAME}/ 16 | ) 17 | 18 | ament_package() 19 | 20 | -------------------------------------------------------------------------------- /dbcbs_ros/config/crazyflies.yaml: -------------------------------------------------------------------------------- 1 | # named list of all robots 2 | robots: 3 | cf14: 4 | enabled: true 5 | uri: radio://0/80/2M/E7E7E7E718 6 | initial_position: [0.5,-1.0, 0] 7 | type: cf21 # see robot_types 8 | cf15: 9 | enabled: true 10 | uri: radio://0/80/2M/E7E7E7E70F 11 | initial_position: [-0.5,2.0, 0] 12 | type: cf21 # see robot_types 13 | 14 | cf17: 15 | enabled: true 16 | uri: radio://0/80/2M/E7E7E7E717 17 | initial_position: [0,-1, 0] 18 | type: cf21 # see robot_types 19 | cf16: 20 | enabled: true 21 | uri: radio://0/80/2M/E7E7E7E716 22 | initial_position: [0,2, 0] 23 | type: cf21 # see robot_types 24 | # Definition of the various robot types 25 | robot_types: 26 | cf21: 27 | motion_capture: 28 | enabled: true 29 | # only if enabled; see motion_capture.yaml 30 | marker: default_single_marker 31 | dynamics: default 32 | big_quad: false 33 | battery: 34 | voltage_warning: 3.8 # V 35 | voltage_critical: 3.7 # V 36 | # firmware_params: 37 | # kalman: 38 | # pNAcc_xy: 1.0 # default 0.5 39 | #firmware_logging: 40 | # enabled: true 41 | # default_topics: 42 | # pose: 43 | # frequency: 1 # Hz 44 | # custom_topics: 45 | # topic_name3: 46 | # frequency: 1 47 | # vars: ["acc.x", "acc.y", "acc.z"] 48 | 49 | cf21_mocap_deck: 50 | motion_capture: 51 | enabled: true 52 | # only if enabled; see motion_capture.yaml 53 | marker: mocap_deck 54 | dynamics: default 55 | big_quad: false 56 | battery: 57 | voltage_warning: 3.8 # V 58 | voltage_critical: 3.7 # V 59 | 60 | # firmware_params: 61 | # kalman: 62 | # pNAcc_xy: 1.0 # default 0.5 63 | 64 | # global settings for all robots 65 | all: 66 | # firmware logging for all drones (use robot_types/type_name to set per type, or 67 | # robots/drone_name to set per drone) 68 | firmware_logging: 69 | enabled: true 70 | default_topics: 71 | # remove to disable default topic 72 | status: 73 | frequency: 1 # Hz 74 | #custom_topics: 75 | # topic_name1: 76 | # frequency: 10 # Hz 77 | # vars: ["stateEstimateZ.x", "stateEstimateZ.y", "stateEstimateZ.z", "pm.vbat"] 78 | # topic_name2: 79 | # frequency: 1 # Hz 80 | # vars: ["stabilizer.roll", "stabilizer.pitch", "stabilizer.yaw"] 81 | # firmware parameters for all drones (use robot_types/type_name to set per type, or 82 | # robots/drone_name to set per drone) 83 | firmware_params: 84 | commander: 85 | enHighLevel: 1 86 | stabilizer: 87 | estimator: 2 # 1: complementary, 2: kalman 88 | controller: 2 # 1: PID, 2: mellinger 89 | # ring: 90 | # effect: 16 # 6: double spinner, 7: solid color, 16: packetRate 91 | # solidBlue: 255 # if set to solid color 92 | # solidGreen: 0 # if set to solid color 93 | # solidRed: 0 # if set to solid color 94 | # headlightEnable: 0 95 | locSrv: 96 | extPosStdDev: 1e-3 97 | extQuatStdDev: 0.5e-1 98 | # kalman: 99 | # resetEstimation: 1 100 | broadcasts: 101 | num_repeats: 15 # number of times broadcast commands are repeated 102 | delay_between_repeats_ms: 1 # delay in milliseconds between individual repeats 103 | -------------------------------------------------------------------------------- /dbcbs_ros/config/motion_capture.yaml: -------------------------------------------------------------------------------- 1 | /motion_capture_tracking: 2 | ros__parameters: 3 | type: "optitrack_closed_source" 4 | hostname: "141.23.110.143" 5 | 6 | mode: "libobjecttracker" # one of motionCapture,libRigidBodyTracker 7 | 8 | topics: 9 | poses: 10 | qos: 11 | mode: "sensor" 12 | deadline: 100.0 # Hz 13 | 14 | marker_configurations: 15 | default: # for standard Crazyflie 16 | offset: [0.0, 0.0, 0.0] 17 | points: 18 | p0: [ 0.0, 0.0, 0.022] # top 19 | p1: [-0.042, 0.042, 0.0 ] # back left (M3) 20 | p2: [-0.042, -0.042, 0.0 ] # back right (M2) 21 | p3: [ 0.042, -0.042, 0.0 ] # front right (M1) 22 | default_single_marker: 23 | offset: [0.0, -0.01, -0.04] 24 | points: 25 | p0: [0.0177184,0.0139654,0.0557585] 26 | mocap_deck: 27 | offset: [0.0, 0.0, -0.01] 28 | points: 29 | p0: [0.03, 0.0, 0.0] # front 30 | p1: [0.00, -0.03, 0.0] # right 31 | p2: [-0.015, 0.0, 0.0] # back 32 | p3: [0.00, 0.03, 0.0] # left 33 | medium_frame: 34 | offset: [0.0, 0.0, -0.03] 35 | points: 36 | p0: [-0.00896228,-0.000716753,0.0716129] 37 | p1: [-0.0156318,0.0997402,0.0508162] 38 | p2: [0.0461693,-0.0881012,0.0380672] 39 | p3: [-0.0789959,-0.0269793,0.0461144] 40 | big_frame: 41 | offset: [0.0, 0.0, -0.06] 42 | points: 43 | p0: [0.0558163,-0.00196302,0.0945539] 44 | p1: [-0.0113941,0.00945842,0.0984811] 45 | p2: [-0.0306277,0.0514879,0.0520456] 46 | p3: [0.0535816,-0.0400775,0.0432799] 47 | 48 | dynamics_configurations: 49 | default: 50 | max_velocity: [2, 2, 3] # m/s 51 | max_angular_velocity: [20, 20, 10] # rad/s 52 | max_roll: 1.4 #rad 53 | max_pitch: 1.4 #rad 54 | max_fitness_score: 0.001 55 | 56 | # Rigid bodies will be automatically generated by the launch file 57 | # rigid_bodies: -------------------------------------------------------------------------------- /dbcbs_ros/config/server.yaml: -------------------------------------------------------------------------------- 1 | /crazyflie_server: 2 | ros__parameters: 3 | warnings: 4 | frequency: 1.0 # report/run checks once per second 5 | motion_capture: 6 | warning_if_rate_outside: [80.0, 120.0] 7 | firmware_params: 8 | query_all_values_on_connect: False 9 | # simulation related 10 | sim: 11 | max_dt: 0.1 #0.1 # artificially limit the step() function (set to 0 to disable) 12 | backend: none # see backend folder for a list 13 | visualizations: # see visualization folder for a list 14 | rviz: 15 | enabled: true 16 | pdf: 17 | enabled: false 18 | output_file: "result.pdf" 19 | record_states: 20 | enabled: false 21 | output_dir: "state_info" 22 | logging_time: 0.2 # how many seconds to leave between logs 23 | file_formats: ["csv", "np"] # csv, np 24 | blender: 25 | enabled: false 26 | fps: 1 # frames per second 27 | cycle_bg: false # if true, pictures will cycle through different environemt background images (useful for synthetic image generation). Otherwise a single environment background image will be used 28 | cf_cameras: # names of crazyflies with cameras on them if enabled in `crazyflies.yaml` 29 | cf231: 30 | calibration: 31 | tvec: [0,0,0] 32 | rvec: [1.2092,-1.2092,1.2092] # 0 deg tilt (cameras looks in front of crazyflie) 33 | cf5: 34 | calibration: 35 | tvec: [0,0,0] 36 | rvec: [ 0.61394313, -0.61394313, 1.48218982] # 45 deg tilt 37 | controller: none # none, pid, mellinger -------------------------------------------------------------------------------- /dbcbs_ros/config/teleop.yaml: -------------------------------------------------------------------------------- 1 | /teleop: 2 | ros__parameters: 3 | frequency: 100 # set to 0, to disable manual flight modes 4 | mode: "cmd_vel_world" # one of cmd_rpy, cmd_vel_world 5 | cmd_rpy: 6 | roll: 7 | axis: 5 8 | max: 30.0 # deg 9 | deadband: 0.0 10 | pitch: 11 | axis: 4 12 | max: 30.0 # deg 13 | deadband: 0.0 14 | yawrate: 15 | axis: 1 16 | max: 200.0 # deg/s 17 | deadband: 5.0 # deg/s 18 | thrust: 19 | axis: 2 20 | max: 60000.0 # PWM 21 | deadband: 0.0 22 | cmd_vel_world: 23 | x_velocity: 24 | axis: 5 25 | max: 0.5 # m/s 26 | deadband: 0.1 # m/s 27 | y_velocity: 28 | axis: 4 29 | max: 0.5 # m/s 30 | deadband: 0.1 # m/s 31 | z_velocity: 32 | axis: 2 33 | max: 0.5 # m/s 34 | deadband: 0.1 # m/s 35 | yaw_velocity: 36 | axis: 1 37 | max: 0.5 # rad/s 38 | deadband: 0.1 # rad/s 39 | # workspace limits 40 | x_limit: [-1.0, 1.0] # m 41 | y_limit: [-1.0, 1.0] # m 42 | z_limit: [0.0, 2.5] # m 43 | initial_position: 44 | x: 0.0 45 | y: 0.0 46 | z: 0.10 47 | auto_yaw_rate: 0.0 # rad/s, use 0.0 for manual yaw control 48 | takeoff: 49 | duration: 2.0 50 | height: 0.5 51 | button: 7 # 7 start 52 | land: 53 | button: 6 # 6 back 54 | emergency: 55 | button: 1 # 1 red -------------------------------------------------------------------------------- /dbcbs_ros/dbcbs_ros/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IMRCLab/db-CBS/220fc05c8f9dfe41ce02fa0d6bffea9eb71886a6/dbcbs_ros/dbcbs_ros/__init__.py -------------------------------------------------------------------------------- /dbcbs_ros/dbcbs_ros/cmd_full_state.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | from pathlib import Path 5 | 6 | from crazyflie_py import * 7 | from crazyflie_py.uav_trajectory import Trajectory 8 | import yaml 9 | 10 | def parse_data(yaml_path,Z): 11 | with open(yaml_path, 'r') as ymlfile: 12 | data = yaml.safe_load(ymlfile)['result'] # a list elements are dictionaries 13 | num_traj = len(data) # number of trajectories 14 | print('number of trajectories',num_traj) 15 | if len(data[0]['states'][0]) == 4: 16 | print("The length of data[0]['states'] is 4.------------") 17 | else: 18 | print("The length of data[0]['states'] is",len(data[0]['states'][0])) 19 | quit() 20 | 21 | num_waypoints = max([len(trajectory['states']) for trajectory in data]) 22 | print("Minimum number of waypoints:", num_waypoints) 23 | 24 | states_list = [] 25 | velocity_list = [] 26 | acceleration_list = [] 27 | for trajectory in data: 28 | states = [row[0:2] + [Z] for row in trajectory['states']] 29 | velocity = [row[2:4] + [0] for row in trajectory['states']] 30 | acceleration = [row[0:2] + [0] for row in trajectory['actions']] 31 | while len(states) < num_waypoints: 32 | states.append(states[-1]) # Append the last line of states 33 | 34 | while len(velocity) < num_waypoints: 35 | velocity.append([0, 0, 0]) # Append [0, 0, 0] to velocity 36 | 37 | while len(acceleration) < num_waypoints: 38 | acceleration.append([0, 0, 0]) # Append [0, 0, Z] to acceleration 39 | states_list.append(np.array(states, dtype=np.float64)) 40 | velocity_list.append(np.array(velocity, dtype=np.float64)) 41 | acceleration_list.append(np.array(acceleration, dtype=np.float64)) 42 | 43 | file_name = yaml_path.stem 44 | print(f'load {file_name} finish') 45 | return num_traj,num_waypoints,states_list,velocity_list,acceleration_list 46 | 47 | def main(): 48 | swarm = Crazyswarm() 49 | timeHelper = swarm.timeHelper 50 | allcfs = swarm.allcfs # CrazyflieServer.crazyflies[0] --> Crazyflie 51 | 52 | rate = 10.0 53 | Z = 0.5 54 | 55 | allcfs.takeoff(targetHeight=Z, duration=Z+1.0) 56 | timeHelper.sleep(Z+2.0) 57 | 58 | # parse data 59 | yaml_path = Path(__file__).parent / "data/forest_4.yaml" 60 | num_traj,num_waypoints,states_list,velocity_list,acceleration_list = parse_data(yaml_path,Z) 61 | 62 | # check the num of UAV <= states_list 63 | if num_traj < len(allcfs.crazyflies): 64 | print(f'not enough trajectory for {len(allcfs.crazyflies)} crazyfile') 65 | quit() 66 | 67 | cfnames=["cf17", "cf16", "cf15", "cf14"] 68 | 69 | # execute trajectory 70 | for state_id in range(num_waypoints): 71 | for drone_id in range(len(allcfs.crazyflies)): 72 | cf = allcfs.crazyfliesByName[cfnames[drone_id]] 73 | pos = states_list[drone_id][state_id] 74 | vel = velocity_list[drone_id][state_id] 75 | acc = acceleration_list[drone_id][state_id] 76 | print('drone_id',drone_id,'pos:',pos,'vel',vel,'acc',acc) 77 | cf.cmdFullState(pos, vel, acc, 0, np.zeros(3)) 78 | timeHelper.sleepForRate(rate) 79 | 80 | for cf in allcfs.crazyflies: 81 | cf.notifySetpointsStop() 82 | 83 | allcfs.land(targetHeight=0.02, duration=3.0) 84 | timeHelper.sleep(3.0) 85 | 86 | if __name__ == "__main__": 87 | main() 88 | -------------------------------------------------------------------------------- /dbcbs_ros/dbcbs_ros/waypoint_plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | from pathlib import Path 5 | import matplotlib.pyplot as plt 6 | import yaml 7 | 8 | def plot_trajectory(yaml_path): 9 | with open(yaml_path, 'r') as ymlfile: 10 | data = yaml.safe_load(ymlfile)['result'] # a list where elements are dictionaries 11 | 12 | # Extract file name without extension 13 | file_name = yaml_path.stem 14 | 15 | # Number of states to consider for each trajectory 16 | number = 20 17 | # number = -1 18 | 19 | # Create a new figure for each YAML file 20 | plt.figure() 21 | 22 | # Plot all trajectories in the current YAML file 23 | for i, traj in enumerate(data): 24 | states = traj['states'][:number] 25 | 26 | # Extract x and y coordinates from the states 27 | x_coords = [point[0] for point in states] 28 | y_coords = [point[1] for point in states] 29 | 30 | # Plot the points for the current trajectory 31 | plt.scatter(x_coords, y_coords, label=f'Trajectory {i+1}') 32 | 33 | plt.xlabel('X') 34 | plt.ylabel('Y') 35 | plt.title(f'position Plot - {file_name}') 36 | plt.grid(True) 37 | plt.legend() 38 | 39 | # Create actions figure 40 | actions_flag = 0 41 | if actions_flag == 1: 42 | plt.figure(figsize=(12, 6)) 43 | # Plot all trajectories in the current YAML file for actions 44 | for i, traj in enumerate(data): 45 | actions = traj['actions'][:number] 46 | 47 | # Plot the actions for the current trajectory 48 | plt.plot(range(len(actions)), actions, label=f'Trajectory {i + 1}') 49 | 50 | plt.xlabel('Time Step') 51 | plt.ylabel('Action Value') 52 | plt.title(f'Action Plot - {file_name}') 53 | plt.grid(True) 54 | plt.legend() 55 | 56 | plt.show() 57 | 58 | def main(): 59 | yaml_path = Path(__file__).parent / "data/13_09.yaml" 60 | plot_trajectory(yaml_path) 61 | 62 | if __name__ == "__main__": 63 | main() 64 | -------------------------------------------------------------------------------- /dbcbs_ros/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | dbcbs_ros 5 | 0.0.0 6 | TODO: Package description 7 | nan 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclpy 14 | crazyflie_py 15 | 16 | ament_copyright 17 | ament_flake8 18 | ament_pep257 19 | python3-pytest 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /dbcbs_ros/setup.cfg: -------------------------------------------------------------------------------- 1 | [options.entry_points] 2 | console_scripts = 3 | cmd_full_state = dbcbs_ros.cmd_full_state:main 4 | -------------------------------------------------------------------------------- /example/alcove_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [4, 1.5] 4 | obstacles: 5 | - type: box 6 | center: [0.75, 0.25] 7 | size: [1.5, 0.5] 8 | - type: box 9 | center: [3.25, 0.25] 10 | size: [1.5, 0.5] 11 | - type: box 12 | center: [2.0, 1.25] 13 | size: [4.0, 0.5] 14 | 15 | robots: 16 | - type: unicycle_first_order_0 17 | start: [0.5,0.75,0] # x,y,theta 18 | goal: [3.5,0.75,0] # x,y,theta 19 | - type: unicycle_first_order_0 20 | start: [1.5,0.75,0] # x,y,theta 21 | goal: [2.5,0.75,0] # x,y,theta -------------------------------------------------------------------------------- /example/alcove_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [6.5, 3.5] 4 | obstacles: 5 | - type: box 6 | center: [1.25, 0.5] 7 | size: [2.5, 1.0] 8 | - type: box 9 | center: [5.25, 0.5] 10 | size: [2.5, 1.0] 11 | - type: box 12 | center: [3.25, 3.0] 13 | size: [6.5, 1.0] 14 | 15 | robots: 16 | - type: unicycle_first_order_0_sphere 17 | start: [0.5,1.75,0] # x,y,theta 18 | goal: [6.0,1.75,0] # x,y,theta 19 | - type: unicycle_first_order_0_sphere 20 | start: [2.0,1.75,0] # x,y,theta 21 | goal: [4.5,1.75,0] # x,y,theta -------------------------------------------------------------------------------- /example/algorithms.yaml: -------------------------------------------------------------------------------- 1 | sst: 2 | default: 3 | goal_epsilon: 0.2 4 | goal_bias: 0.05 5 | selection_radius: 0.2 # delta_BN in paper 6 | pruning_radius: 0.1 # delta_s in paper 7 | propagation_step_size: 0.1 #s 8 | control_duration: [1, 10] # multiples of step size 9 | swap2_unicycle: 10 | goal_epsilon: 0.4 11 | goal_bias: 0.2 12 | selection_radius: 0.4 # delta_BN in paper 13 | pruning_radius: 0.2 # delta_s in paper 14 | s2m2: 15 | default: 16 | goal_epsilon: 0.2 17 | min_seg: 2 18 | max_seg: 10 19 | obs_seg: 10 20 | radius: 0.4 21 | velocity: 0.45 # need to reduce this, since S2M is unaware of control limits 22 | bloating: 0.12 23 | k: [2.0, 2.0, 4.0] 24 | k-cbs: 25 | default: 26 | goal_epsilon: 0.2 27 | goal_bias: 0.05 28 | selection_radius: 0.2 # delta_BN in paper 29 | pruning_radius: 0.1 # delta_s in paper 30 | propagation_step_size: 0.1 #s 31 | control_duration: [1, 10] # multiples of step size 32 | ll_timelimit: 1 33 | swap1_trailer: 34 | goal_epsilon: 0.2 35 | swap2_trailer: 36 | goal_epsilon: 0.2 37 | swap2_hetero: 38 | goal_epsilon: 0.2 39 | db-cbs: 40 | default: 41 | delta_0: 0.5 42 | delta_rate: 0.9 43 | num_primitives_0: 1000 44 | num_primitives_rate: 1.5 45 | alpha: 0.5 46 | filter_duplicates: True 47 | heuristic1: "reverse-search" 48 | heuristic1_delta: 1.0 49 | '*_unicycle_sphere': 50 | delta_0: 0.9 51 | '*_hetero': 52 | delta_0: 0.75 53 | '*_trailer': 54 | delta_0: 0.9 55 | -------------------------------------------------------------------------------- /example/at_goal_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [3, 1.5] 4 | obstacles: 5 | - type: box 6 | center: [0.75, 0.25] 7 | size: [1.5, 0.5] 8 | - type: box 9 | center: [0.75, 1.25] 10 | size: [1.5, 0.5] 11 | 12 | robots: 13 | - type: unicycle_first_order_0 14 | start: [0.5,0.75,0] # x,y,theta 15 | goal: [2.5,0.75,0] # x,y,theta 16 | - type: unicycle_first_order_0 17 | start: [1.5,0.75,0] # x,y,theta 18 | goal: [1.5,0.75,0] # x,y,theta -------------------------------------------------------------------------------- /example/at_goal_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [4.5, 3.5] 4 | obstacles: 5 | - type: box 6 | center: [1.25, 0.55] 7 | size: [2.5, 1.0] 8 | - type: box 9 | center: [1.25, 3.0] 10 | size: [2.5, 1.0] 11 | 12 | robots: 13 | - type: unicycle_first_order_0_sphere 14 | start: [0.5,1.75,0] # x,y,theta 15 | goal: [4.0,1.75,0] # x,y,theta 16 | - type: unicycle_first_order_0_sphere 17 | start: [2.0,1.75,0] # x,y,theta 18 | goal: [2.0,1.75,0] # x,y,theta -------------------------------------------------------------------------------- /example/bugtrap.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [6, 6] 4 | obstacles: 5 | - type: box 6 | center: [4.5, 3] 7 | size: [0.2, 3.2] 8 | - type: box 9 | center: [3, 1.5] 10 | size: [3.2, 0.2] 11 | - type: box 12 | center: [3, 4.5] 13 | size: [3.2, 0.2] 14 | - type: box 15 | center: [1.5, 4.05] 16 | size: [0.2, 1.1] 17 | - type: box 18 | center: [1.5, 1.95] 19 | size: [0.2, 1.1] 20 | robots: 21 | - type: unicycle_first_order 22 | start: [3.8,3,0] 23 | goal: [5.2,3,0] 24 | - type: single_integrator 25 | start: [5.0,2.0] # x,y 26 | goal: [3.3,3.0] # x,y 27 | -------------------------------------------------------------------------------- /example/classic.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [6, 6] 4 | obstacles: 5 | - type: box 6 | center: [3, 3] 7 | size: [1, 1] 8 | 9 | robots: 10 | - type: unicycle_first_order_0 11 | start: [3.5,4,0] # x,y,theta 12 | goal: [4.5,2,0] # x,y,theta 13 | - type: unicycle_first_order_0 14 | start: [4.5,3,0] # x,y,theta 15 | goal: [2,4,0] # x,y,theta -------------------------------------------------------------------------------- /example/forest.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: [0.76, 2.1] 3 | min: [-0.76, -1.1] 4 | obstacles: 5 | - center: [0.75, -0.25] #0 6 | size: [0.1, 0.1] 7 | type: box 8 | - center: [-0.2, -0.25] #1 9 | size: [0.1, 0.1] 10 | type: box 11 | - center: [-0.75, -0.25] #2 12 | size: [0.1, 0.1] 13 | type: box 14 | - center: [0.7, 0.5] #3 15 | size: [0.1, 0.1] 16 | type: box 17 | - center: [0.3, 0.25] #4 18 | size: [0.1, 0.1] 19 | type: box 20 | - center: [-0.6, 0.5] #5 21 | size: [0.1, 0.1] 22 | type: box 23 | - center: [0.5, 1.25] #6 24 | size: [0.1, 0.1] 25 | type: box 26 | - center: [-0.1, 1.0] #7 27 | size: [0.1, 0.1] 28 | type: box 29 | - center: [-0.5, 1.0] #8 30 | size: [0.1, 0.1] 31 | type: box 32 | robots: 33 | - type: double_integrator_0 # 17 34 | start: [0,-1.0,0,0] 35 | goal: [0,2.0,0,0] 36 | - type: double_integrator_0 # 16 37 | start: [0,2.0,0,0] 38 | goal: [0,-1,0,0] 39 | - type: double_integrator_0 # 15 40 | start: [-0.5,2.0,0,0] 41 | goal: [-0.5,-1.0,0,0] 42 | - type: double_integrator_0 # 14 43 | start: [0.5,-1.0,0,0] 44 | goal: [0.5,2.0,0,0] 45 | -------------------------------------------------------------------------------- /example/gen_p10_n2_0_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 0.7793539207021392 11 | - 4.615075282918334 12 | size: 13 | - 0.7107460832990117 14 | - 0.6749330127631459 15 | type: box 16 | - center: 17 | - 2.479716393410817 18 | - 3.0795270437441395 19 | size: 20 | - 0.7680237808796572 21 | - 1.1085426058241086 22 | type: box 23 | - center: 24 | - 2.0293367476644137 25 | - 4.428067239262051 26 | size: 27 | - 0.390880518420744 28 | - 0.4391747463264385 29 | type: box 30 | - center: 31 | - 0.8243262873438465 32 | - 2.6343836647174235 33 | size: 34 | - 0.4944799275830271 35 | - 0.29805866902078176 36 | type: box 37 | - center: 38 | - 2.32943040756011 39 | - 1.1166019800357108 40 | size: 41 | - 0.1262838265074827 42 | - 0.7869828469057034 43 | type: box 44 | robots: 45 | - goal: 46 | - 1.4179018567661927 47 | - 0.5225746057385496 48 | - 2.9740535229003324 49 | start: 50 | - 1.7095233812588484 51 | - 2.2832270717144176 52 | - 1.9482447894005448 53 | type: unicycle_first_order_0 54 | - goal: 55 | - 4.276218755473915 56 | - 2.515329134381654 57 | - 0.0 58 | - 0.0 59 | start: 60 | - 2.3984223059982384 61 | - 3.9036350526340025 62 | - 0.0 63 | - 0.0 64 | type: double_integrator_0 65 | -------------------------------------------------------------------------------- /example/gen_p10_n2_0_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.4245997989638974 11 | - 8.92360057232633 12 | size: 13 | - 0.6298864907595898 14 | - 0.25131804167101324 15 | type: box 16 | - center: 17 | - 1.0011981656722961 18 | - 2.209950009617214 19 | size: 20 | - 0.7423620227748816 21 | - 1.5686223534968402 22 | type: box 23 | - center: 24 | - 3.0503176688763918 25 | - 1.4650314684683292 26 | size: 27 | - 1.0179337101394914 28 | - 0.6372630562100898 29 | type: box 30 | - center: 31 | - 8.095363176492215 32 | - 1.2899880155878876 33 | size: 34 | - 0.32302709610706876 35 | - 0.7583886162471588 36 | type: box 37 | - center: 38 | - 8.257294659112253 39 | - 4.656434199717845 40 | size: 41 | - 0.8189613225135737 42 | - 0.314759462503778 43 | type: box 44 | - center: 45 | - 2.566509796625133 46 | - 7.707323692967853 47 | size: 48 | - 0.9485801997729193 49 | - 0.4190306399667897 50 | type: box 51 | - center: 52 | - 8.953954332343356 53 | - 1.4507198884504502 54 | size: 55 | - 0.6337011626234776 56 | - 0.7686129331665126 57 | type: box 58 | - center: 59 | - 4.748093119186408 60 | - 4.387986203370786 61 | size: 62 | - 0.4913435922432419 63 | - 0.6490416241235928 64 | type: box 65 | - center: 66 | - 6.176468740128045 67 | - 3.9394611948744207 68 | size: 69 | - 0.9808951085083154 70 | - 0.8193068936671404 71 | type: box 72 | - center: 73 | - 4.593362283801277 74 | - 7.224613101266854 75 | size: 76 | - 1.2800649788412564 77 | - 0.6721257414540327 78 | type: box 79 | - center: 80 | - 0.9361835839146839 81 | - 4.145456946687425 82 | size: 83 | - 0.23552907182049398 84 | - 0.18824489396454158 85 | type: box 86 | - center: 87 | - 6.736725260432291 88 | - 0.6977353980663642 89 | size: 90 | - 1.1181964434309146 91 | - 0.7746311516042823 92 | type: box 93 | - center: 94 | - 2.913630746909527 95 | - 2.5255319398290657 96 | size: 97 | - 0.7303838665836991 98 | - 0.6670109210869962 99 | type: box 100 | - center: 101 | - 9.141889803341957 102 | - 4.810160000858339 103 | size: 104 | - 0.9078977483205956 105 | - 0.7293244627713164 106 | type: box 107 | - center: 108 | - 3.372228936429651 109 | - 6.188131618360268 110 | size: 111 | - 0.3589315155471468 112 | - 0.494897806003468 113 | type: box 114 | robots: 115 | - goal: 116 | - 4.83936311907277 117 | - 8.5803539147558 118 | - 1.372538220841883 119 | start: 120 | - 7.2548828822491425 121 | - 1.6832448602702392 122 | - 0.5935584861228871 123 | type: unicycle_first_order_0_sphere 124 | - goal: 125 | - 3.8437080089293825 126 | - 4.035983023079548 127 | - -0.3409446810022918 128 | start: 129 | - 8.50118178290687 130 | - 6.012054574094119 131 | - -0.3852771979154892 132 | type: unicycle_first_order_0_sphere 133 | -------------------------------------------------------------------------------- /example/gen_p10_n2_1_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 0.8472995494534565 11 | - 1.3184095808795475 12 | size: 13 | - 0.8480670740122483 14 | - 1.0505098204671872 15 | type: box 16 | - center: 17 | - 3.15143916144076 18 | - 2.5217602933891152 19 | size: 20 | - 1.0196103167339279 21 | - 0.855455424886546 22 | type: box 23 | - center: 24 | - 4.215106708531248 25 | - 2.636342692720013 26 | size: 27 | - 0.22144413865907175 28 | - 2.1443608485830374 29 | type: box 30 | robots: 31 | - goal: 32 | - 1.7533106459567125 33 | - 0.5034592726131875 34 | - 0.8564178500557835 35 | - 0.9045643798774629 36 | start: 37 | - 2.467931138685434 38 | - 1.6080026777498286 39 | - -1.6943483748279984 40 | - -1.3107072952418442 41 | type: car_first_order_with_1_trailers_0 42 | - goal: 43 | - 2.392983916538232 44 | - 1.4614814511577259 45 | - -0.3941792716439294 46 | start: 47 | - 2.7092531763696415 48 | - 0.626237609546691 49 | - -2.527429379074663 50 | type: unicycle_first_order_0 51 | -------------------------------------------------------------------------------- /example/gen_p10_n2_1_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 4.248019331397998 11 | - 6.719229466170453 12 | size: 13 | - 1.0256018563811748 14 | - 0.5622529772680528 15 | type: box 16 | - center: 17 | - 7.690051818726234 18 | - 7.702176279920642 19 | size: 20 | - 0.515027250725077 21 | - 0.4381055228095511 22 | type: box 23 | - center: 24 | - 1.12948533736215 25 | - 7.193386323334135 26 | size: 27 | - 0.9509991986475603 28 | - 0.34199994716305004 29 | type: box 30 | - center: 31 | - 2.0135943886295373 32 | - 2.506042288663368 33 | size: 34 | - 0.10085661105816635 35 | - 0.24141767781204237 36 | type: box 37 | - center: 38 | - 5.228459032243102 39 | - 8.246627542651803 40 | size: 41 | - 0.9340931015851517 42 | - 1.369331602844871 43 | type: box 44 | - center: 45 | - 9.461950963742597 46 | - 1.257766466414061 47 | size: 48 | - 0.6770735015066768 49 | - 1.3524430888531431 50 | type: box 51 | - center: 52 | - 4.0594712482529856 53 | - 1.0657837761423534 54 | size: 55 | - 0.5158768771227025 56 | - 0.35884905127233896 57 | type: box 58 | - center: 59 | - 6.817567863762249 60 | - 2.572327909780811 61 | size: 62 | - 0.7663334575603664 63 | - 1.146684124746656 64 | type: box 65 | - center: 66 | - 8.543037597767032 67 | - 2.652123951692267 68 | size: 69 | - 0.9521592355737541 70 | - 1.0516115965045527 71 | type: box 72 | - center: 73 | - 1.913382840760627 74 | - 7.757684243422574 75 | size: 76 | - 0.243417468877699 77 | - 0.7081061551395801 78 | type: box 79 | - center: 80 | - 5.687432729700837 81 | - 2.3986751462440115 82 | size: 83 | - 0.4527709572757333 84 | - 1.8413095104926012 85 | type: box 86 | - center: 87 | - 2.5970231067246554 88 | - 0.9995647287433197 89 | size: 90 | - 0.34498025846713315 91 | - 0.8745770166535094 92 | type: box 93 | - center: 94 | - 9.103213771226516 95 | - 8.287508289563643 96 | size: 97 | - 0.5065398089954499 98 | - 0.5806784227824235 99 | type: box 100 | - center: 101 | - 2.0642389531581946 102 | - 6.232764581601981 103 | size: 104 | - 1.1520267185154944 105 | - 1.5473170526254836 106 | type: box 107 | robots: 108 | - goal: 109 | - 6.79731337247359 110 | - 1.0805915687528982 111 | - -2.2656512197045666 112 | start: 113 | - 9.386770626279086 114 | - 5.0746032744866945 115 | - 3.117139013153949 116 | type: unicycle_first_order_0_sphere 117 | - goal: 118 | - 5.3712469400015985 119 | - 6.3488697739905735 120 | - 2.861262486723138 121 | start: 122 | - 4.127806924518117 123 | - 9.381867467639895 124 | - -2.0494022539458987 125 | type: unicycle_first_order_0_sphere 126 | -------------------------------------------------------------------------------- /example/gen_p10_n2_2_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 0.6265793142258037 11 | - 2.845848410139677 12 | size: 13 | - 1.009673105982673 14 | - 0.5353163577181659 15 | type: box 16 | - center: 17 | - 3.4924316038945467 18 | - 0.9761080111673051 19 | size: 20 | - 0.7505450956094744 21 | - 0.32512631269755443 22 | type: box 23 | - center: 24 | - 4.201188645453983 25 | - 1.6892642778410494 26 | size: 27 | - 0.2770928278497157 28 | - 0.404621392389106 29 | type: box 30 | - center: 31 | - 3.0870402820075182 32 | - 2.428116676972748 33 | size: 34 | - 1.4113876612096536 35 | - 0.8611327024120355 36 | type: box 37 | robots: 38 | - goal: 39 | - 1.548099426902389 40 | - 3.113307210828289 41 | - 0.0 42 | - 0.0 43 | start: 44 | - 4.411646138441403 45 | - 3.6809006340232524 46 | - 0.0 47 | - 0.0 48 | type: double_integrator_0 49 | - goal: 50 | - 3.1003548273505785 51 | - 1.6246989306394855 52 | - 3.009970814468317 53 | start: 54 | - 2.3467923643003212 55 | - 3.5155373639442833 56 | - -2.912474470953458 57 | type: unicycle_first_order_0 58 | -------------------------------------------------------------------------------- /example/gen_p10_n2_2_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 7.167929192459297 11 | - 6.855755315920075 12 | size: 13 | - 0.2523018745777972 14 | - 0.5252002837355666 15 | type: box 16 | - center: 17 | - 0.815262638411146 18 | - 8.634814737046362 19 | size: 20 | - 1.0490584830140512 21 | - 0.7528983501425626 22 | type: box 23 | - center: 24 | - 8.485787649054267 25 | - 3.582128171782947 26 | size: 27 | - 1.4814747892363211 28 | - 0.693019342629757 29 | type: box 30 | - center: 31 | - 5.790878510834032 32 | - 4.575672109636137 33 | size: 34 | - 1.3619939519215378 35 | - 1.1469609858813843 36 | type: box 37 | - center: 38 | - 6.6412605069529596 39 | - 8.244424456908218 40 | size: 41 | - 0.1149190854948764 42 | - 0.12355256343051224 43 | type: box 44 | - center: 45 | - 5.975569643654741 46 | - 6.415578700925314 47 | size: 48 | - 0.74349500476246 49 | - 0.6044132808083819 50 | type: box 51 | - center: 52 | - 9.448437912630443 53 | - 5.393711107406893 54 | size: 55 | - 1.0576328040358076 56 | - 1.4677019376160998 57 | type: box 58 | - center: 59 | - 5.53895224448186 60 | - 1.5655060467370687 61 | size: 62 | - 1.3159208625064722 63 | - 0.7318824701228118 64 | type: box 65 | - center: 66 | - 2.073667125217564 67 | - 6.486136052194951 68 | size: 69 | - 0.8224995178305876 70 | - 0.9436002662126897 71 | type: box 72 | - center: 73 | - 7.776057205646128 74 | - 2.2029638888843857 75 | size: 76 | - 0.8192111880590999 77 | - 0.8139972588838911 78 | type: box 79 | - center: 80 | - 9.817504859036003 81 | - 2.785821754244524 82 | size: 83 | - 0.25737568630186936 84 | - 0.24712128873794692 85 | type: box 86 | robots: 87 | - goal: 88 | - 8.377141388258192 89 | - 8.255738876806749 90 | - -1.571798848790354 91 | start: 92 | - 8.785572474329031 93 | - 7.704456044291052 94 | - 1.1218862000216223 95 | type: unicycle_first_order_0_sphere 96 | - goal: 97 | - 4.951222131898105 98 | - 6.13026770678932 99 | - -3.1310294505038603 100 | start: 101 | - 4.290611954088998 102 | - 4.012528143900657 103 | - 1.5369357912762789 104 | type: unicycle_first_order_0_sphere 105 | -------------------------------------------------------------------------------- /example/gen_p10_n2_3_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 4.077623039799258 11 | - 3.4060177995847383 12 | size: 13 | - 0.9746152681026881 14 | - 0.10387682447530933 15 | type: box 16 | - center: 17 | - 1.7705663246104653 18 | - 3.208756998626072 19 | size: 20 | - 0.9830257500010562 21 | - 1.1133350798548785 22 | type: box 23 | - center: 24 | - 2.269391054766108 25 | - 0.998221757502146 26 | size: 27 | - 0.3275923462537843 28 | - 0.3636612040319769 29 | type: box 30 | - center: 31 | - 0.6067212268243568 32 | - 2.5314326915392407 33 | size: 34 | - 0.3399543134642778 35 | - 0.6132659620008291 36 | type: box 37 | - center: 38 | - 4.003932532905944 39 | - 2.0640709369085606 40 | size: 41 | - 0.3760842968435858 42 | - 0.4865428735080591 43 | type: box 44 | - center: 45 | - 1.074464223333525 46 | - 1.6233151284570768 47 | size: 48 | - 1.1884041346736045 49 | - 1.194249981000336 50 | type: box 51 | robots: 52 | - goal: 53 | - 3.5611136916939103 54 | - 2.9581955189813316 55 | - 0.8757328890945901 56 | start: 57 | - 4.089441399344866 58 | - 0.8402669464522328 59 | - 0.7786578928116064 60 | type: unicycle_first_order_0 61 | - goal: 62 | - 3.4643103814067855 63 | - 4.322672162018817 64 | - 0.0 65 | - 0.0 66 | start: 67 | - 2.69140282018562 68 | - 2.128620826730931 69 | - 0.0 70 | - 0.0 71 | type: double_integrator_0 72 | -------------------------------------------------------------------------------- /example/gen_p10_n2_3_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 9.213469243027514 11 | - 2.693700628743072 12 | size: 13 | - 0.7039695973216923 14 | - 0.38695750464192835 15 | type: box 16 | - center: 17 | - 4.204658404613371 18 | - 4.532735953649936 19 | size: 20 | - 0.14840616938462725 21 | - 0.8315206600141775 22 | type: box 23 | - center: 24 | - 8.202908163308964 25 | - 4.9566439360473655 26 | size: 27 | - 0.5518549685661157 28 | - 1.139410039902134 29 | type: box 30 | - center: 31 | - 5.926789658708311 32 | - 1.4559263871409631 33 | size: 34 | - 0.45496525073611993 35 | - 0.41336988854191137 36 | type: box 37 | - center: 38 | - 6.572274957552469 39 | - 1.934701657449911 40 | size: 41 | - 1.25892564826035 42 | - 0.3452714596485619 43 | type: box 44 | - center: 45 | - 0.7593640871965672 46 | - 1.9540285819446415 47 | size: 48 | - 1.0815752260617633 49 | - 1.2795693613733166 50 | type: box 51 | - center: 52 | - 5.567519958342107 53 | - 2.841848239070841 54 | size: 55 | - 1.2391103728022876 56 | - 0.4148104514315079 57 | type: box 58 | - center: 59 | - 1.8215512763506465 60 | - 4.711304105737168 61 | size: 62 | - 0.8811795158127205 63 | - 0.6439451162464985 64 | type: box 65 | - center: 66 | - 4.800046095404351 67 | - 7.039953511837487 68 | size: 69 | - 0.5863742917384859 70 | - 0.7573861237396339 71 | type: box 72 | - center: 73 | - 2.033391212762961 74 | - 8.38040872582065 75 | size: 76 | - 0.7451868084576324 77 | - 1.198829230331028 78 | type: box 79 | - center: 80 | - 2.3361530585676036 81 | - 3.161146806958738 82 | size: 83 | - 2.056382013461332 84 | - 0.9378382634860545 85 | type: box 86 | - center: 87 | - 9.06725391095833 88 | - 3.74973097110957 89 | size: 90 | - 1.3909355560077814 91 | - 1.204587630734912 92 | type: box 93 | robots: 94 | - goal: 95 | - 2.4896571888133714 96 | - 1.7137651363847763 97 | - -1.8883330673007852 98 | start: 99 | - 5.784847534985654 100 | - 7.110907872779101 101 | - 0.671224773123841 102 | type: unicycle_first_order_0_sphere 103 | - goal: 104 | - 6.173448643347594 105 | - 7.6223048829043405 106 | - 0.9886926830265423 107 | start: 108 | - 7.400410324347242 109 | - 3.5958341356179995 110 | - -2.636834469694365 111 | type: unicycle_first_order_0_sphere 112 | -------------------------------------------------------------------------------- /example/gen_p10_n2_4_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 0.4221478415754896 11 | - 4.1830633349689155 12 | size: 13 | - 0.48995089424583094 14 | - 1.1728365907863445 15 | type: box 16 | - center: 17 | - 2.91321723454767 18 | - 3.341908454141392 19 | size: 20 | - 1.3601237160952848 21 | - 0.7181171078024887 22 | type: box 23 | - center: 24 | - 1.8003452367032957 25 | - 0.7993719384063964 26 | size: 27 | - 1.516579625216717 28 | - 0.9263177796050842 29 | type: box 30 | robots: 31 | - goal: 32 | - 2.1292955025353337 33 | - 2.747822212091797 34 | - 0.0 35 | - 0.0 36 | start: 37 | - 4.042532820634779 38 | - 1.520979245336202 39 | - 0.0 40 | - 0.0 41 | type: double_integrator_0 42 | - goal: 43 | - 4.302676591225463 44 | - 0.6347165427131203 45 | - -1.0704372751672975 46 | - -1.2686558974055056 47 | start: 48 | - 2.781089211791855 49 | - 2.3571065799699364 50 | - -0.2822181935672514 51 | - -0.37714606040950527 52 | type: car_first_order_with_1_trailers_0 53 | -------------------------------------------------------------------------------- /example/gen_p10_n2_4_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 7.26663612502195 11 | - 3.460729353923953 12 | size: 13 | - 0.7344862688433518 14 | - 0.4672517249078834 15 | type: box 16 | - center: 17 | - 9.380280011795058 18 | - 5.293713480061301 19 | size: 20 | - 0.6402320831791237 21 | - 0.4864830404505335 22 | type: box 23 | - center: 24 | - 4.246567049532535 25 | - 7.4029325649267275 26 | size: 27 | - 0.7321778900733584 28 | - 0.24125383539715106 29 | type: box 30 | - center: 31 | - 3.5397897903884883 32 | - 2.7334215324381295 33 | size: 34 | - 1.1003295536175568 35 | - 0.692047486199106 36 | type: box 37 | - center: 38 | - 6.034207133158092 39 | - 5.196148836005467 40 | size: 41 | - 0.8209779053555355 42 | - 1.3587321002801807 43 | type: box 44 | - center: 45 | - 6.599304873426756 46 | - 4.775203305858018 47 | size: 48 | - 0.22824781043456188 49 | - 0.5724022517962762 50 | type: box 51 | - center: 52 | - 3.49003853655761 53 | - 6.6142658505296605 54 | size: 55 | - 1.1541540399137085 56 | - 0.35140788615989427 57 | type: box 58 | - center: 59 | - 2.1229881091994316 60 | - 3.072054993406263 61 | size: 62 | - 0.7583256977535784 63 | - 1.3849066576312046 64 | type: box 65 | - center: 66 | - 4.290513082706563 67 | - 2.2450988112474137 68 | size: 69 | - 0.24666404685358845 70 | - 2.0772854311978506 71 | type: box 72 | - center: 73 | - 1.1948375597225156 74 | - 5.903193395208234 75 | size: 76 | - 0.7788223996049289 77 | - 0.7650494740147183 78 | type: box 79 | - center: 80 | - 4.356642721044383 81 | - 5.854743580189023 82 | size: 83 | - 1.0855718108329546 84 | - 0.2649330914144453 85 | type: box 86 | - center: 87 | - 6.014205180521405 88 | - 3.8697532018702643 89 | size: 90 | - 1.440211624959098 91 | - 0.3482839511374457 92 | type: box 93 | - center: 94 | - 6.2735036932843125 95 | - 9.355500027395818 96 | size: 97 | - 0.9711269837181323 98 | - 0.34529005754585657 99 | type: box 100 | - center: 101 | - 3.339809641120077 102 | - 5.226830768930955 103 | size: 104 | - 0.9278832350862466 105 | - 0.5812608799406439 106 | type: box 107 | - center: 108 | - 5.485606998027725 109 | - 7.281062821328589 110 | size: 111 | - 0.5035397876781077 112 | - 0.34297903052543627 113 | type: box 114 | - center: 115 | - 3.185824330776379 116 | - 0.6126365862037225 117 | size: 118 | - 0.8879908514787882 119 | - 0.5648752943129504 120 | type: box 121 | - center: 122 | - 7.42103852823521 123 | - 6.07155339254821 124 | size: 125 | - 0.554199814614889 126 | - 0.9511877222481098 127 | type: box 128 | - center: 129 | - 9.422317735627928 130 | - 2.5736349786548933 131 | size: 132 | - 1.0608181465915714 133 | - 1.1270287259669722 134 | type: box 135 | - center: 136 | - 1.9791744804611988 137 | - 4.870770665232026 138 | size: 139 | - 0.1573833267473831 140 | - 0.2924506766466557 141 | type: box 142 | - center: 143 | - 1.9257523627675857 144 | - 1.003144751737523 145 | size: 146 | - 0.5102316114662866 147 | - 0.4089797404693106 148 | type: box 149 | robots: 150 | - goal: 151 | - 0.7717642026569222 152 | - 6.943980930928482 153 | - 1.7961729924778025 154 | start: 155 | - 7.891672596478639 156 | - 0.8610493609722405 157 | - 0.8279591864337323 158 | type: unicycle_first_order_0_sphere 159 | - goal: 160 | - 8.184240353524164 161 | - 4.5246676777504415 162 | - -2.72490191513355 163 | start: 164 | - 1.61122916359799 165 | - 7.46983878080006 166 | - 0.1637167055908324 167 | type: unicycle_first_order_0_sphere 168 | -------------------------------------------------------------------------------- /example/gen_p10_n2_5_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 3.9415568955148528 11 | - 3.9627751617512095 12 | size: 13 | - 1.454708714130136 14 | - 1.469599259299057 15 | type: box 16 | - center: 17 | - 2.9429641565129003 18 | - 1.7752564177326142 19 | size: 20 | - 0.2505795682032282 21 | - 1.4409178546899035 22 | type: box 23 | robots: 24 | - goal: 25 | - 0.7743010263730699 26 | - 2.549448103718154 27 | - -1.973273786097619 28 | - -1.783119873244376 29 | start: 30 | - 1.895194963896909 31 | - 2.582399978661204 32 | - 1.1006070666853605 33 | - 1.311907466613266 34 | type: car_first_order_with_1_trailers_0 35 | - goal: 36 | - 2.1708145434817347 37 | - 4.050079892562032 38 | - -2.805229202043823 39 | start: 40 | - 2.699526299043827 41 | - 3.0492382723890654 42 | - 2.1785150498150925 43 | type: unicycle_first_order_0 44 | -------------------------------------------------------------------------------- /example/gen_p10_n2_5_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 4.851094260387634 11 | - 6.9349482970582255 12 | size: 13 | - 1.477852935418559 14 | - 1.3464875519347645 15 | type: box 16 | - center: 17 | - 4.677994783691464 18 | - 8.329027361724577 19 | size: 20 | - 1.4265847312417272 21 | - 0.7095117361577601 22 | type: box 23 | - center: 24 | - 4.989787446792535 25 | - 2.5008411853051147 26 | size: 27 | - 0.8456664073215588 28 | - 1.644792206912324 29 | type: box 30 | - center: 31 | - 8.767291250130292 32 | - 9.162900571255795 33 | size: 34 | - 1.3529093849634064 35 | - 0.8916874156246736 36 | type: box 37 | - center: 38 | - 5.691437002395288 39 | - 3.5613037120358304 40 | size: 41 | - 0.5308690406781906 42 | - 0.7289235922351195 43 | type: box 44 | - center: 45 | - 1.716099958128332 46 | - 8.65113047095137 47 | size: 48 | - 0.9621418795586607 49 | - 0.8377520889225842 50 | type: box 51 | - center: 52 | - 2.8209697508550593 53 | - 1.0894492300345688 54 | size: 55 | - 0.8502817889349681 56 | - 1.1296283816655057 57 | type: box 58 | - center: 59 | - 9.022908067938529 60 | - 6.278215842876532 61 | size: 62 | - 0.7856124919681885 63 | - 1.3676964673558012 64 | type: box 65 | - center: 66 | - 6.463413296368959 67 | - 6.690826189511637 68 | size: 69 | - 1.2279405399722227 70 | - 1.0016439451846542 71 | type: box 72 | robots: 73 | - goal: 74 | - 6.625324380868809 75 | - 3.2827531944634636 76 | - 2.1691108701356203 77 | start: 78 | - 8.78799302983002 79 | - 3.199793907140627 80 | - -0.1619503273081313 81 | type: unicycle_first_order_0_sphere 82 | - goal: 83 | - 7.515624141650722 84 | - 3.3988554698855484 85 | - -1.8837898904317523 86 | start: 87 | - 3.691015399897267 88 | - 2.167209391407011 89 | - 1.8169143476354996 90 | type: unicycle_first_order_0_sphere 91 | -------------------------------------------------------------------------------- /example/gen_p10_n2_6_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.6623171741134133 11 | - 2.1713323688517567 12 | size: 13 | - 1.2727910239518416 14 | - 0.4840235959918306 15 | type: box 16 | - center: 17 | - 0.27888790377078243 18 | - 2.8122368485605667 19 | size: 20 | - 0.21535427848074862 21 | - 0.5788387200093782 22 | type: box 23 | - center: 24 | - 1.1428760172101529 25 | - 3.667397407331838 26 | size: 27 | - 0.6701903205310443 28 | - 1.07153080076673 29 | type: box 30 | robots: 31 | - goal: 32 | - 4.074104405639433 33 | - 4.363093401099248 34 | - -1.917722396923347 35 | start: 36 | - 2.9009665731324525 37 | - 2.498839060241297 38 | - 1.0673343675503384 39 | type: unicycle_first_order_0 40 | - goal: 41 | - 1.4580061191130778 42 | - 4.499967832390758 43 | - -0.4279052256892446 44 | - -0.6722868247399805 45 | start: 46 | - 4.174729129801246 47 | - 2.0173969655718613 48 | - 2.8658297138969324 49 | - 2.957713906335664 50 | type: car_first_order_with_1_trailers_0 51 | -------------------------------------------------------------------------------- /example/gen_p10_n2_6_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.2952057018195249 11 | - 3.803483450384797 12 | size: 13 | - 0.538208803363495 14 | - 0.8549025492130807 15 | type: box 16 | - center: 17 | - 4.806759933864999 18 | - 5.808875980729776 19 | size: 20 | - 1.164922398720159 21 | - 0.8478649591331618 22 | type: box 23 | - center: 24 | - 5.302010371292384 25 | - 9.316356631511761 26 | size: 27 | - 1.741452515940564 28 | - 0.9731441320727627 29 | type: box 30 | - center: 31 | - 8.664773189914726 32 | - 4.760003159828639 33 | size: 34 | - 0.9710974671690955 35 | - 1.6146981128799804 36 | type: box 37 | - center: 38 | - 3.599340283383232 39 | - 3.4604496769913786 40 | size: 41 | - 0.5034344894689825 42 | - 1.5161435182041594 43 | type: box 44 | - center: 45 | - 6.34879510125102 46 | - 4.9621436295715045 47 | size: 48 | - 1.1913323995596194 49 | - 0.7309325034531 50 | type: box 51 | - center: 52 | - 8.926832134760778 53 | - 6.131982408046973 54 | size: 55 | - 0.9879106542229974 56 | - 1.0676667896506178 57 | type: box 58 | - center: 59 | - 6.869354056740732 60 | - 2.109113124071356 61 | size: 62 | - 2.1066420071305294 63 | - 0.44523329824366503 64 | type: box 65 | robots: 66 | - goal: 67 | - 7.733900266862826 68 | - 7.081558548870918 69 | - 0.47114065468710775 70 | start: 71 | - 7.016556783317318 72 | - 4.081276258827155 73 | - 1.4854105263045527 74 | type: unicycle_first_order_0_sphere 75 | - goal: 76 | - 5.729818303903333 77 | - 0.5835104513062279 78 | - 1.0381876342782395 79 | start: 80 | - 6.673992418299444 81 | - 1.1797298472694384 82 | - -3.055134045290731 83 | type: unicycle_first_order_0_sphere 84 | -------------------------------------------------------------------------------- /example/gen_p10_n2_7_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.6384075875824133 11 | - 0.9912485110314748 12 | size: 13 | - 0.496859486518913 14 | - 1.006486071595683 15 | type: box 16 | - center: 17 | - 3.181089564071148 18 | - 3.8256620854438292 19 | size: 20 | - 1.3138790910925784 21 | - 0.6329551081140097 22 | type: box 23 | robots: 24 | - goal: 25 | - 4.041262246362788 26 | - 2.046588086293347 27 | - -1.579408445821623 28 | start: 29 | - 1.6276011932298653 30 | - 1.8058921416278522 31 | - 2.440566596480773 32 | type: unicycle_first_order_0 33 | - goal: 34 | - 4.15481715000755 35 | - 0.6001511933123034 36 | - 0.0 37 | - 0.0 38 | start: 39 | - 1.4285762430250482 40 | - 1.0057500972070943 41 | - 0.0 42 | - 0.0 43 | type: double_integrator_0 44 | -------------------------------------------------------------------------------- /example/gen_p10_n2_7_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.694010968780075 11 | - 1.454586827394576 12 | size: 13 | - 0.43726217256351757 14 | - 0.9245358552473301 15 | type: box 16 | - center: 17 | - 7.0314286918480695 18 | - 9.079697829690907 19 | size: 20 | - 0.10976496706688466 21 | - 0.5021852668948494 22 | type: box 23 | - center: 24 | - 7.378018406585155 25 | - 6.29775658929827 26 | size: 27 | - 0.7037630760932444 28 | - 1.1863339245463587 29 | type: box 30 | - center: 31 | - 5.1003118339901565 32 | - 6.81930029058224 33 | size: 34 | - 0.48380206856164126 35 | - 0.9680731621417765 36 | type: box 37 | - center: 38 | - 5.770966963378045 39 | - 0.8783987438733221 40 | size: 41 | - 1.334185440727878 42 | - 0.6905587770022992 43 | type: box 44 | - center: 45 | - 1.508284411162975 46 | - 9.548739478028207 47 | size: 48 | - 1.3484878240322482 49 | - 0.5465758593214464 50 | type: box 51 | - center: 52 | - 1.5635822000259312 53 | - 7.517947661275519 54 | size: 55 | - 0.6313901718974748 56 | - 0.5094399655503441 57 | type: box 58 | - center: 59 | - 6.309054926721535 60 | - 3.184081446447171 61 | size: 62 | - 0.6078771959181384 63 | - 0.3456188739051172 64 | type: box 65 | - center: 66 | - 6.144870223638401 67 | - 9.801419695265555 68 | size: 69 | - 0.6137644866544549 70 | - 0.22143152870494398 71 | type: box 72 | - center: 73 | - 8.153666268883873 74 | - 0.5892528900399098 75 | size: 76 | - 1.219805065134508 77 | - 0.827619707286161 78 | type: box 79 | - center: 80 | - 5.02907704082347 81 | - 3.5491389996835716 82 | size: 83 | - 1.2122479009874274 84 | - 1.1196862700910808 85 | type: box 86 | - center: 87 | - 1.808214813784291 88 | - 2.0233654579268863 89 | size: 90 | - 1.2683146003725758 91 | - 1.095660988056643 92 | type: box 93 | robots: 94 | - goal: 95 | - 3.707884028385714 96 | - 8.418493406670295 97 | - -2.3986052968978138 98 | start: 99 | - 8.981994829884629 100 | - 8.24832889322138 101 | - 1.2265732649745908 102 | type: unicycle_first_order_0_sphere 103 | - goal: 104 | - 2.435344040799277 105 | - 3.984470087304602 106 | - 0.27764711655618557 107 | start: 108 | - 2.628879202221008 109 | - 7.796311489681251 110 | - 0.5437393753040478 111 | type: unicycle_first_order_0_sphere 112 | -------------------------------------------------------------------------------- /example/gen_p10_n2_8_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.497296656440295 11 | - 0.7282899037447521 12 | size: 13 | - 0.38593835501837326 14 | - 1.3984731935824706 15 | type: box 16 | - center: 17 | - 4.435210763836365 18 | - 3.4116185473470297 19 | size: 20 | - 0.46035739689379374 21 | - 1.2241859171683969 22 | type: box 23 | - center: 24 | - 1.3192150278793284 25 | - 0.5273687207104694 26 | size: 27 | - 1.9079653328482278 28 | - 0.7437165001608326 29 | type: box 30 | robots: 31 | - goal: 32 | - 4.162601105235138 33 | - 2.524847986223661 34 | - -0.3783810449727012 35 | start: 36 | - 2.268881398858329 37 | - 2.8946659052183876 38 | - 1.6952960671042048 39 | type: unicycle_first_order_0 40 | - goal: 41 | - 3.0769120053532744 42 | - 2.8793468976007963 43 | - 2.0055022681918535 44 | - 1.8616622343035352 45 | start: 46 | - 1.1621249493978536 47 | - 3.8045953892248274 48 | - 0.8972855115114511 49 | - 1.029184500514054 50 | type: car_first_order_with_1_trailers_0 51 | -------------------------------------------------------------------------------- /example/gen_p10_n2_8_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 9.428152434032294 11 | - 4.085275988112525 12 | size: 13 | - 0.7035044073050637 14 | - 0.3520011016982993 15 | type: box 16 | - center: 17 | - 3.71053941016678 18 | - 2.233999428705235 19 | size: 20 | - 0.3863148219804549 21 | - 0.5947605447526291 22 | type: box 23 | - center: 24 | - 8.09595290321948 25 | - 6.636902262993521 26 | size: 27 | - 0.49883012008136457 28 | - 0.8240269658512098 29 | type: box 30 | - center: 31 | - 5.572795193438041 32 | - 3.432922555121042 33 | size: 34 | - 0.6054574546239325 35 | - 0.9384424305613186 36 | type: box 37 | - center: 38 | - 1.1652240805017744 39 | - 6.566654163887005 40 | size: 41 | - 1.0184514817544978 42 | - 1.957767379340594 43 | type: box 44 | - center: 45 | - 9.174313478711973 46 | - 1.5938995703699834 47 | size: 48 | - 1.5784147532755943 49 | - 1.261711035477889 50 | type: box 51 | - center: 52 | - 3.6555930742382348 53 | - 4.916565114972095 54 | size: 55 | - 1.596512932173026 56 | - 1.676955246836958 57 | type: box 58 | - center: 59 | - 2.3505010181669483 60 | - 7.390281212531023 61 | size: 62 | - 0.7358147787306777 63 | - 0.527399006934212 64 | type: box 65 | - center: 66 | - 2.1912796595889596 67 | - 1.3689412192051027 68 | size: 69 | - 0.9950079667707761 70 | - 0.5446937699773958 71 | type: box 72 | - center: 73 | - 0.826145240724449 74 | - 2.2524792898751747 75 | size: 76 | - 0.6009781620093557 77 | - 1.1581860841627152 78 | type: box 79 | - center: 80 | - 6.4485809861403744 81 | - 7.11441228978346 82 | size: 83 | - 0.39114522124174583 84 | - 0.2268618165543137 85 | type: box 86 | - center: 87 | - 3.9380326735270823 88 | - 8.192401228932686 89 | size: 90 | - 0.2987377584691318 91 | - 0.31785503176894586 92 | type: box 93 | - center: 94 | - 5.781468772836003 95 | - 1.7755385043164207 96 | size: 97 | - 1.3005619356329048 98 | - 0.13897740776616985 99 | type: box 100 | robots: 101 | - goal: 102 | - 4.593511794866816 103 | - 0.7815558644249496 104 | - -2.685712990038201 105 | start: 106 | - 8.361373811381398 107 | - 8.162745700844447 108 | - 2.6946052537942062 109 | type: unicycle_first_order_0_sphere 110 | - goal: 111 | - 4.472189068586552 112 | - 2.5201020925468622 113 | - -1.4233015997674343 114 | start: 115 | - 7.615776274521809 116 | - 2.5636205120362385 117 | - 1.3478001950763883 118 | type: unicycle_first_order_0_sphere 119 | -------------------------------------------------------------------------------- /example/gen_p10_n2_9_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.7554417967511342 11 | - 2.778028758321211 12 | size: 13 | - 1.062426483862367 14 | - 0.20876146770370074 15 | type: box 16 | - center: 17 | - 2.6251036707067694 18 | - 1.4490858089485434 19 | size: 20 | - 1.757353901989979 21 | - 0.5251790128724243 22 | type: box 23 | - center: 24 | - 1.26897928533124 25 | - 4.080851979843224 26 | size: 27 | - 0.9805061088355899 28 | - 1.1012995350326666 29 | type: box 30 | robots: 31 | - goal: 32 | - 4.204113947087743 33 | - 1.7740396630372528 34 | - 1.7017370431151466 35 | - 1.370247025787498 36 | start: 37 | - 3.1375104440310766 38 | - 2.697884012181861 39 | - -1.5431003673228185 40 | - -1.8682142814497151 41 | type: car_first_order_with_1_trailers_0 42 | - goal: 43 | - 0.5545967894793788 44 | - 2.961497779313389 45 | - 1.5869134629127961 46 | start: 47 | - 3.5354867548685354 48 | - 3.849300153794709 49 | - 1.9370715338232172 50 | type: unicycle_first_order_0 51 | -------------------------------------------------------------------------------- /example/gen_p10_n2_9_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 9.134051709803336 11 | - 0.6983747352624791 12 | size: 13 | - 0.39387868091009864 14 | - 0.3199105797476838 15 | type: box 16 | - center: 17 | - 5.3019944913595864 18 | - 1.2869094271555332 19 | size: 20 | - 0.8188309407427069 21 | - 0.9749349473983546 22 | type: box 23 | - center: 24 | - 8.619861023280784 25 | - 6.863379301356417 26 | size: 27 | - 0.7343322546048934 28 | - 1.0930618869074225 29 | type: box 30 | - center: 31 | - 0.40422503807188687 32 | - 4.672378424633702 33 | size: 34 | - 0.7075235608691859 35 | - 0.21302284687656114 36 | type: box 37 | - center: 38 | - 4.543591384095878 39 | - 4.7655593542109616 40 | size: 41 | - 0.4987834522537064 42 | - 0.8640395427774428 43 | type: box 44 | - center: 45 | - 8.835021467632917 46 | - 8.563692907255243 47 | size: 48 | - 2.0934206651486265 49 | - 0.6678225402854084 50 | type: box 51 | - center: 52 | - 1.6595872341077116 53 | - 6.516811336223555 54 | size: 55 | - 0.654459034846232 56 | - 0.4180642782171253 57 | type: box 58 | - center: 59 | - 5.495334119003071 60 | - 5.922941169931979 61 | size: 62 | - 1.3611217570650447 63 | - 0.24169913822720546 64 | type: box 65 | - center: 66 | - 6.6906339383136615 67 | - 2.884410283401153 68 | size: 69 | - 0.9375010364438632 70 | - 1.0639731309915188 71 | type: box 72 | - center: 73 | - 2.137981570682682 74 | - 3.8308388632241313 75 | size: 76 | - 0.7470887570070593 77 | - 1.051878569136896 78 | type: box 79 | - center: 80 | - 7.929011336827505 81 | - 9.47267760798324 82 | size: 83 | - 1.0905396891184167 84 | - 0.9542917222266549 85 | type: box 86 | - center: 87 | - 6.727303842490087 88 | - 4.527654503257962 89 | size: 90 | - 1.6339483030421829 91 | - 1.0716618750596276 92 | type: box 93 | - center: 94 | - 6.086300043088077 95 | - 8.599298281227266 96 | size: 97 | - 1.0557858630633137 98 | - 1.8130505480955292 99 | type: box 100 | robots: 101 | - goal: 102 | - 9.322740647351864 103 | - 5.133041270138016 104 | - -2.6882610005601664 105 | start: 106 | - 4.603523896094413 107 | - 8.728199637976566 108 | - 1.3185326932418358 109 | type: unicycle_first_order_0_sphere 110 | - goal: 111 | - 1.611597380105267 112 | - 0.8271868138693259 113 | - -2.6185585245027867 114 | start: 115 | - 6.606498334564112 116 | - 7.020817456218153 117 | - -2.3410892105565058 118 | type: unicycle_first_order_0_sphere 119 | -------------------------------------------------------------------------------- /example/gen_p10_n4_0_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.7785206804028233 11 | - 0.5577917786411579 12 | size: 13 | - 1.8814342153084045 14 | - 0.6978482041778385 15 | type: box 16 | - center: 17 | - 1.6980363553264957 18 | - 4.019692060610611 19 | size: 20 | - 0.9499717106859356 21 | - 1.4749643728493962 22 | type: box 23 | robots: 24 | - goal: 25 | - 0.6809119636318144 26 | - 3.025934478402576 27 | - -1.3148398170148472 28 | - -1.054143632209348 29 | start: 30 | - 4.296899308478791 31 | - 3.995182395897001 32 | - 1.1362723336197122 33 | - 1.3423354882215068 34 | type: car_first_order_with_1_trailers_0 35 | - goal: 36 | - 3.384535174281746 37 | - 3.6163013412684353 38 | - -1.832021734239632 39 | - -1.646854037637131 40 | start: 41 | - 2.138844760948858 42 | - 2.5475890176597167 43 | - 1.9104516986061917 44 | - 2.0772155163170067 45 | type: car_first_order_with_1_trailers_0 46 | - goal: 47 | - 0.708257050457874 48 | - 0.6204117788143844 49 | - 0.0 50 | - 0.0 51 | start: 52 | - 4.082627158426579 53 | - 1.0289275995989255 54 | - 0.0 55 | - 0.0 56 | type: double_integrator_0 57 | - goal: 58 | - 2.5188642425673997 59 | - 1.4979514166528105 60 | - 1.1946724343511619 61 | start: 62 | - 4.276328468329385 63 | - 2.858254145748287 64 | - -0.7846112399621128 65 | type: unicycle_first_order_0 66 | -------------------------------------------------------------------------------- /example/gen_p10_n4_0_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 8.386772413581692 11 | - 4.691932557175635 12 | size: 13 | - 1.149404277189607 14 | - 1.464735490463894 15 | type: box 16 | - center: 17 | - 4.59797728255327 18 | - 7.087358680194746 19 | size: 20 | - 1.1005727206369218 21 | - 0.4548826616594888 22 | type: box 23 | - center: 24 | - 4.1283315844067054 25 | - 3.5790737787096156 26 | size: 27 | - 1.8523364168818472 28 | - 0.5220860598953213 29 | type: box 30 | - center: 31 | - 3.766413867317185 32 | - 2.1160303863633882 33 | size: 34 | - 0.14723064647705109 35 | - 0.3930451185489878 36 | type: box 37 | - center: 38 | - 4.6620551100304 39 | - 4.217555950260591 40 | size: 41 | - 1.5927385524604993 42 | - 0.60890900102126 43 | type: box 44 | - center: 45 | - 7.165730630286666 46 | - 6.590075112227504 47 | size: 48 | - 0.28491492777153515 49 | - 1.141497297670605 50 | type: box 51 | - center: 52 | - 1.7882830289680043 53 | - 7.9898631136507205 54 | size: 55 | - 1.0677157392085437 56 | - 0.8664226620284688 57 | type: box 58 | - center: 59 | - 0.7738416288341587 60 | - 7.71550793508161 61 | size: 62 | - 0.3889223076309993 63 | - 1.0274261398390494 64 | type: box 65 | - center: 66 | - 1.741590655290902 67 | - 8.77914311737525 68 | size: 69 | - 0.8857744301094942 70 | - 0.3300983467168 71 | type: box 72 | - center: 73 | - 5.172592643719334 74 | - 3.3601791547702007 75 | size: 76 | - 0.2274957745294226 77 | - 0.3422990411296802 78 | type: box 79 | - center: 80 | - 1.8444290557658962 81 | - 5.396861124100922 82 | size: 83 | - 1.067777966866893 84 | - 0.6123885764611531 85 | type: box 86 | - center: 87 | - 3.297963179922907 88 | - 5.764405215138707 89 | size: 90 | - 1.1545436717630215 91 | - 2.169951781687035 92 | type: box 93 | robots: 94 | - goal: 95 | - 3.5031309125523453 96 | - 7.739576407268635 97 | - -1.7733330805278453 98 | start: 99 | - 1.7319513954348589 100 | - 4.555270041236148 101 | - -0.4383179369812775 102 | type: unicycle_first_order_0_sphere 103 | - goal: 104 | - 1.6288589025473517 105 | - 6.902661556688572 106 | - -0.5870004997569991 107 | start: 108 | - 8.077701850836029 109 | - 8.120883154732127 110 | - 2.3715505357753717 111 | type: unicycle_first_order_0_sphere 112 | - goal: 113 | - 4.193964616412323 114 | - 1.5492701835969571 115 | - -1.1699261270351262 116 | start: 117 | - 1.8499751093474832 118 | - 2.3080257069515295 119 | - -1.182794743745073 120 | type: unicycle_first_order_0_sphere 121 | - goal: 122 | - 2.820710277239053 123 | - 8.661240818744783 124 | - 3.0505891481495864 125 | start: 126 | - 3.3162560309709415 127 | - 9.367967290227764 128 | - -2.0675936650902345 129 | type: unicycle_first_order_0_sphere 130 | -------------------------------------------------------------------------------- /example/gen_p10_n4_1_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.22713204443974 11 | - 3.383428115762124 12 | size: 13 | - 0.17111551979066042 14 | - 0.8794957702854026 15 | type: box 16 | - center: 17 | - 0.7136664435529818 18 | - 4.211209055284846 19 | size: 20 | - 0.36087695222715493 21 | - 0.6369046987243093 22 | type: box 23 | - center: 24 | - 3.7620614444428013 25 | - 1.216053180551123 26 | size: 27 | - 0.9806728566437533 28 | - 0.5943286220984154 29 | type: box 30 | - center: 31 | - 3.6815611604825587 32 | - 3.509953151812735 33 | size: 34 | - 1.1083377856127647 35 | - 0.9526967607305349 36 | type: box 37 | robots: 38 | - goal: 39 | - 0.9887306855586986 40 | - 0.5120459680958267 41 | - 0.0 42 | - 0.0 43 | start: 44 | - 3.27964726572634 45 | - 2.6725034797028275 46 | - 0.0 47 | - 0.0 48 | type: double_integrator_0 49 | - goal: 50 | - 3.020898291384628 51 | - 1.1106785350334247 52 | - 0.0 53 | - 0.0 54 | start: 55 | - 1.2520216448580723 56 | - 4.229415318339505 57 | - 0.0 58 | - 0.0 59 | type: double_integrator_0 60 | - goal: 61 | - 2.1385937298705584 62 | - 4.050955649018531 63 | - 0.9258715311084931 64 | - 1.1693621524577376 65 | start: 66 | - 0.8592869189015326 67 | - 1.7798579353275303 68 | - -1.4153433107751696 69 | - -1.745110524574173 70 | type: car_first_order_with_1_trailers_0 71 | - goal: 72 | - 2.8778449558374684 73 | - 3.3637043410797385 74 | - 1.4048248838480744 75 | - 1.0225496465977446 76 | start: 77 | - 1.3163067185948862 78 | - 1.1392868077531424 79 | - -2.8455310244697603 80 | - -2.6754293929268984 81 | type: car_first_order_with_1_trailers_0 82 | -------------------------------------------------------------------------------- /example/gen_p10_n4_1_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 4.642464160558817 11 | - 0.561820141319231 12 | size: 13 | - 0.6753096184769907 14 | - 0.8186509364468783 15 | type: box 16 | - center: 17 | - 9.20096007853723 18 | - 4.364771921955639 19 | size: 20 | - 1.1678085776360076 21 | - 0.9275255799597183 22 | type: box 23 | - center: 24 | - 6.163318163948865 25 | - 8.208140340461508 26 | size: 27 | - 0.3711487094225118 28 | - 1.1635452491807843 29 | type: box 30 | - center: 31 | - 5.449447566611868 32 | - 5.056751655668911 33 | size: 34 | - 0.9350234386364058 35 | - 0.5110758939726425 36 | type: box 37 | - center: 38 | - 0.3920145908772538 39 | - 6.958307847419353 40 | size: 41 | - 0.46859009500862797 42 | - 0.6043431848799251 43 | type: box 44 | - center: 45 | - 1.5087556166508642 46 | - 1.8450993063980052 47 | size: 48 | - 1.2397492972904582 49 | - 0.36436409989410107 50 | type: box 51 | - center: 52 | - 4.739847311256671 53 | - 6.237757295593041 54 | size: 55 | - 1.1414224770938912 56 | - 0.7656998509494143 57 | type: box 58 | - center: 59 | - 3.271383340351271 60 | - 8.511557476030976 61 | size: 62 | - 0.5301360768749623 63 | - 2.0481344862750177 64 | type: box 65 | - center: 66 | - 4.559311924018876 67 | - 5.114919975962423 68 | size: 69 | - 0.8091064189797523 70 | - 1.3922122755682174 71 | type: box 72 | - center: 73 | - 8.959381284085197 74 | - 9.548872795704327 75 | size: 76 | - 0.6993161400201685 77 | - 0.5419459605371777 78 | type: box 79 | - center: 80 | - 8.664135537299154 81 | - 1.5552142563404598 82 | size: 83 | - 1.2278718746437036 84 | - 0.5229843727138712 85 | type: box 86 | - center: 87 | - 0.5447323862467333 88 | - 5.37026893544304 89 | size: 90 | - 0.7955563644322271 91 | - 1.1088865316896264 92 | type: box 93 | - center: 94 | - 1.9504914426511506 95 | - 8.588620203033745 96 | size: 97 | - 1.4636211112976203 98 | - 1.0915344858112066 99 | type: box 100 | robots: 101 | - goal: 102 | - 5.047287598875574 103 | - 1.8382028317789452 104 | - -1.7158430807193785 105 | start: 106 | - 6.73058966875425 107 | - 0.6749668518153052 108 | - 0.10870391872435192 109 | type: unicycle_first_order_0_sphere 110 | - goal: 111 | - 7.3573408884748375 112 | - 3.1686418450954013 113 | - 0.21372147019132903 114 | start: 115 | - 1.3092439392953725 116 | - 3.8654976450715743 117 | - 0.722968220770674 118 | type: unicycle_first_order_0_sphere 119 | - goal: 120 | - 4.934158773067666 121 | - 3.1411372045380235 122 | - 0.1860725896751152 123 | start: 124 | - 7.572363433871907 125 | - 3.836071321315259 126 | - -1.0756380840851887 127 | type: unicycle_first_order_0_sphere 128 | - goal: 129 | - 2.203377465317791 130 | - 6.055145588734843 131 | - -0.33442174415399295 132 | start: 133 | - 1.856106625365967 134 | - 2.903791148839666 135 | - -1.4961281717717194 136 | type: unicycle_first_order_0_sphere 137 | -------------------------------------------------------------------------------- /example/gen_p10_n4_2_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.4265244568482935 11 | - 4.4816050155146545 12 | size: 13 | - 1.2173087072678062 14 | - 0.8366964674490946 15 | type: box 16 | - center: 17 | - 1.346449826940955 18 | - 4.244403075304732 19 | size: 20 | - 0.4180950296162774 21 | - 0.306250325402388 22 | type: box 23 | - center: 24 | - 3.837023091679661 25 | - 4.329826455217548 26 | size: 27 | - 1.239226249854914 28 | - 1.1814657218899294 29 | type: box 30 | robots: 31 | - goal: 32 | - 2.791913375813725 33 | - 0.7784751046696594 34 | - 1.7101923738455103 35 | start: 36 | - 3.2500589551694357 37 | - 2.3414144503825733 38 | - -2.492317938040659 39 | type: unicycle_first_order_0 40 | - goal: 41 | - 3.7416127048955086 42 | - 3.185840751246905 43 | - 2.767789410987521 44 | start: 45 | - 3.8070328993942057 46 | - 2.7371823540976523 47 | - 0.7183496261147986 48 | type: unicycle_first_order_0 49 | - goal: 50 | - 3.757824665042232 51 | - 2.6171953159836194 52 | - 2.403102267808932 53 | - 2.623715147936026 54 | start: 55 | - 1.070318424185452 56 | - 2.7804680915682938 57 | - -2.945886182743746 58 | - -3.0689513911108395 59 | type: car_first_order_with_1_trailers_0 60 | - goal: 61 | - 4.0730665166726245 62 | - 1.459706994102827 63 | - -1.6927577045108624 64 | start: 65 | - 1.1327902585379657 66 | - 1.9014799951407788 67 | - 1.631769727528293 68 | type: unicycle_first_order_0 69 | -------------------------------------------------------------------------------- /example/gen_p10_n4_2_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 3.7376759610849817 11 | - 7.614496025521672 12 | size: 13 | - 0.7816046877611452 14 | - 0.39311277859203814 15 | type: box 16 | - center: 17 | - 8.360623850499827 18 | - 3.3682448070369486 19 | size: 20 | - 0.8809191210466515 21 | - 1.585657755789985 22 | type: box 23 | - center: 24 | - 2.9363642428988825 25 | - 1.6446413389097414 26 | size: 27 | - 1.3737692952676483 28 | - 1.6305111402779175 29 | type: box 30 | - center: 31 | - 8.05289727162969 32 | - 1.1528975895877436 33 | size: 34 | - 2.3026430514007896 35 | - 0.3137349604607707 36 | type: box 37 | - center: 38 | - 8.467467207734213 39 | - 7.5107574517063105 40 | size: 41 | - 0.5596555952240626 42 | - 0.11382468495957554 43 | type: box 44 | - center: 45 | - 6.9415254387923655 46 | - 6.556482193339949 47 | size: 48 | - 1.1921403191823579 49 | - 0.6341081105647223 50 | type: box 51 | - center: 52 | - 7.370076048919633 53 | - 4.925265458584981 54 | size: 55 | - 0.805380994417652 56 | - 0.7806091186272607 57 | type: box 58 | - center: 59 | - 3.661289478964446 60 | - 5.718983746153408 61 | size: 62 | - 1.3809361914728213 63 | - 1.1969647714681717 64 | type: box 65 | - center: 66 | - 5.353053942208728 67 | - 3.362713875325852 68 | size: 69 | - 1.414532496175739 70 | - 1.3425372682762249 71 | type: box 72 | - center: 73 | - 8.214894903272516 74 | - 6.321240672506558 75 | size: 76 | - 0.41310941474617086 77 | - 1.2594852503719394 78 | type: box 79 | robots: 80 | - goal: 81 | - 4.830076091862484 82 | - 4.782799368192627 83 | - 0.6985133835648063 84 | start: 85 | - 5.353785825379115 86 | - 8.365875714141877 87 | - -2.5548178704587157 88 | type: unicycle_first_order_0_sphere 89 | - goal: 90 | - 4.552783486876345 91 | - 8.007902806576944 92 | - -0.9499044562928831 93 | start: 94 | - 4.575801263871986 95 | - 1.1249775433918967 96 | - 2.6661030802232606 97 | type: unicycle_first_order_0_sphere 98 | - goal: 99 | - 3.4213672863007045 100 | - 3.4803645107057206 101 | - -1.7343580556224334 102 | start: 103 | - 1.7326335287424528 104 | - 8.139383012933532 105 | - -2.343585672763927 106 | type: unicycle_first_order_0_sphere 107 | - goal: 108 | - 0.7791103791672196 109 | - 7.609299400004405 110 | - -1.9643330538733679 111 | start: 112 | - 5.573205672663706 113 | - 7.276424398495188 114 | - -1.1008905777223559 115 | type: unicycle_first_order_0_sphere 116 | -------------------------------------------------------------------------------- /example/gen_p10_n4_3_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 4.257242420196058 11 | - 2.130342310086914 12 | size: 13 | - 0.7337403508588117 14 | - 0.5782158660615314 15 | type: box 16 | - center: 17 | - 4.370644134255594 18 | - 4.1169116673297745 19 | size: 20 | - 0.8131214007733075 21 | - 0.5354685997044717 22 | type: box 23 | - center: 24 | - 3.3866915288765047 25 | - 2.6811573732511995 26 | size: 27 | - 0.61709785810183 28 | - 2.0750161429752962 29 | type: box 30 | - center: 31 | - 1.5426514995393743 32 | - 2.080601111556315 33 | size: 34 | - 0.9251473459364975 35 | - 0.6205104610582501 36 | type: box 37 | robots: 38 | - goal: 39 | - 4.242945666985422 40 | - 1.1579292682787736 41 | - 0.0 42 | - 0.0 43 | start: 44 | - 1.0370573214223762 45 | - 3.963036841275954 46 | - 0.0 47 | - 0.0 48 | type: double_integrator_0 49 | - goal: 50 | - 2.8127880694968104 51 | - 4.216124680364015 52 | - -1.8582597272829884 53 | - -2.0233167395632132 54 | start: 55 | - 1.2241788724625429 56 | - 0.5315065416315812 57 | - -2.998087110751067 58 | - -3.086459426673867 59 | type: car_first_order_with_1_trailers_0 60 | - goal: 61 | - 4.49749672430865 62 | - 1.4797046595082732 63 | - 0.0 64 | - 0.0 65 | start: 66 | - 1.666984575279817 67 | - 1.5375404843197562 68 | - 0.0 69 | - 0.0 70 | type: double_integrator_0 71 | - goal: 72 | - 0.7900664477393344 73 | - 1.0970384313718924 74 | - -0.4740903608568261 75 | start: 76 | - 1.7446950421660246 77 | - 3.746968365199759 78 | - 2.587091041781359 79 | type: unicycle_first_order_0 80 | -------------------------------------------------------------------------------- /example/gen_p10_n4_3_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 6.721787620541281 11 | - 4.372065521748432 12 | size: 13 | - 1.129954360534387 14 | - 1.5827021838737458 15 | type: box 16 | - center: 17 | - 6.438885078416535 18 | - 8.67053799070395 19 | size: 20 | - 0.2871534725288352 21 | - 0.9138897226620379 22 | type: box 23 | - center: 24 | - 7.263965410456025 25 | - 2.1198756498671347 26 | size: 27 | - 0.6139866721253199 28 | - 1.5421755742802041 29 | type: box 30 | - center: 31 | - 0.573162509076521 32 | - 7.566574054942877 33 | size: 34 | - 0.9440166116869377 35 | - 0.7995135038755221 36 | type: box 37 | - center: 38 | - 4.502914848121351 39 | - 3.128060872282361 40 | size: 41 | - 0.8091615687138829 42 | - 0.48826418811932476 43 | type: box 44 | - center: 45 | - 0.8158450283397192 46 | - 3.1458080258805206 47 | size: 48 | - 1.1185710051055746 49 | - 0.450153034836215 50 | type: box 51 | - center: 52 | - 4.473354333341447 53 | - 8.334954938569178 54 | size: 55 | - 0.6461296164915593 56 | - 0.3949585026167267 57 | type: box 58 | - center: 59 | - 4.2554386954312795 60 | - 8.622278851516807 61 | size: 62 | - 0.7290561929979078 63 | - 0.13858771595760255 64 | type: box 65 | - center: 66 | - 4.291373909438614 67 | - 1.5752966743186698 68 | size: 69 | - 0.33010856801641847 70 | - 0.8843401412595817 71 | type: box 72 | - center: 73 | - 7.7444944440669214 74 | - 6.821062005511486 75 | size: 76 | - 1.350373433596043 77 | - 0.37953891108933346 78 | type: box 79 | - center: 80 | - 2.406180869373322 81 | - 3.892971973083825 82 | size: 83 | - 0.7315791635725946 84 | - 0.31077440509168525 85 | type: box 86 | - center: 87 | - 9.167955193364495 88 | - 0.4409900815308493 89 | size: 90 | - 0.6194151885698952 91 | - 0.41841003018880607 92 | type: box 93 | robots: 94 | - goal: 95 | - 3.8808160441825823 96 | - 9.339501987261508 97 | - -1.1891566147782817 98 | start: 99 | - 5.551633237760077 100 | - 1.8395262161530688 101 | - 2.5911233418870667 102 | type: unicycle_first_order_0_sphere 103 | - goal: 104 | - 4.101455957778384 105 | - 0.652079810873044 106 | - 1.9018595864915486 107 | start: 108 | - 8.734225271202002 109 | - 5.761175150149388 110 | - -2.703237943457316 111 | type: unicycle_first_order_0_sphere 112 | - goal: 113 | - 6.317135607487911 114 | - 5.995083277569881 115 | - 1.6273921797821425 116 | start: 117 | - 2.7384434093787258 118 | - 7.344176846334688 119 | - -0.31326872426155017 120 | type: unicycle_first_order_0_sphere 121 | - goal: 122 | - 2.8973483280922108 123 | - 6.105228546906387 124 | - 1.55944451796214 125 | start: 126 | - 6.250091656475341 127 | - 2.982824113148953 128 | - -2.9440148022062935 129 | type: unicycle_first_order_0_sphere 130 | -------------------------------------------------------------------------------- /example/gen_p10_n4_4_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.3955701430292624 11 | - 1.3512171379755542 12 | size: 13 | - 1.254723814078956 14 | - 1.0549692596399538 15 | type: box 16 | - center: 17 | - 2.6666932213863186 18 | - 4.014564906710547 19 | size: 20 | - 1.453757847345125 21 | - 1.726507444095696 22 | type: box 23 | robots: 24 | - goal: 25 | - 0.5146331280968308 26 | - 0.6642424186464684 27 | - 0.9517546977620226 28 | start: 29 | - 4.207896025971687 30 | - 1.7173619726809068 31 | - 2.419703007319429 32 | type: unicycle_first_order_0 33 | - goal: 34 | - 1.1267622636303138 35 | - 2.869687796944263 36 | - 1.687550737432435 37 | start: 38 | - 1.5476872166036388 39 | - 4.08335978888101 40 | - -2.9448846950046925 41 | type: unicycle_first_order_0 42 | - goal: 43 | - 1.2967968989221692 44 | - 4.4209604006431675 45 | - 1.1977880923173867 46 | - 0.970178613558152 47 | start: 48 | - 4.166346651835285 49 | - 2.9760114381970415 50 | - 0.5674576008081846 51 | - 0.5646792596403264 52 | type: car_first_order_with_1_trailers_0 53 | - goal: 54 | - 3.6878306393910902 55 | - 4.255216662204486 56 | - 0.0 57 | - 0.0 58 | start: 59 | - 1.193012687388653 60 | - 1.3905042162803323 61 | - 0.0 62 | - 0.0 63 | type: double_integrator_0 64 | -------------------------------------------------------------------------------- /example/gen_p10_n4_4_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.8017647968384276 11 | - 1.1206690335540654 12 | size: 13 | - 0.6923489854093331 14 | - 0.14512718620268117 15 | type: box 16 | - center: 17 | - 9.178043505054422 18 | - 3.0875873968181904 19 | size: 20 | - 0.4997928841458232 21 | - 1.0063200726382087 22 | type: box 23 | - center: 24 | - 6.813681535189126 25 | - 9.11719377035884 26 | size: 27 | - 0.6182774350190543 28 | - 0.5557889270837068 29 | type: box 30 | - center: 31 | - 5.706348317332903 32 | - 5.867274788194312 33 | size: 34 | - 2.042757781846251 35 | - 1.416791439478156 36 | type: box 37 | - center: 38 | - 7.193342119482815 39 | - 2.5288426826177397 40 | size: 41 | - 0.8665801065198431 42 | - 1.0804173608755978 43 | type: box 44 | - center: 45 | - 2.572489145868259 46 | - 8.339296932409447 47 | size: 48 | - 1.4553128911897195 49 | - 1.1804908625597663 50 | type: box 51 | - center: 52 | - 7.5079015696162275 53 | - 6.973789919589576 54 | size: 55 | - 0.8980876113122415 56 | - 0.2562562788132012 57 | type: box 58 | - center: 59 | - 5.39353824386276 60 | - 7.371967568715815 61 | size: 62 | - 0.20200057225647627 63 | - 0.24817002297846702 64 | type: box 65 | - center: 66 | - 9.57799328703854 67 | - 5.1891405673131805 68 | size: 69 | - 0.3796548180346996 70 | - 0.5828178969953216 71 | type: box 72 | robots: 73 | - goal: 74 | - 2.3682436845234602 75 | - 3.8291547258047647 76 | - -2.4996205182439493 77 | start: 78 | - 1.4421515608371027 79 | - 1.802460333507916 80 | - -0.6333995223053628 81 | type: unicycle_first_order_0_sphere 82 | - goal: 83 | - 4.1507914155043135 84 | - 0.8353562561183151 85 | - -2.677752293764361 86 | start: 87 | - 1.689559876051374 88 | - 4.807118780415947 89 | - -2.639005074140617 90 | type: unicycle_first_order_0_sphere 91 | - goal: 92 | - 1.1392643204133923 93 | - 4.338303376420055 94 | - 0.7156995811804916 95 | start: 96 | - 5.176109142410458 97 | - 0.6238918185064627 98 | - 1.8120714250057128 99 | type: unicycle_first_order_0_sphere 100 | - goal: 101 | - 8.872827201412353 102 | - 6.308863794516419 103 | - 1.7188280877419722 104 | start: 105 | - 3.299375629197441 106 | - 3.97017850938062 107 | - -2.109425478646786 108 | type: unicycle_first_order_0_sphere 109 | -------------------------------------------------------------------------------- /example/gen_p10_n4_5_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 3.620464488127669 11 | - 3.277392385679505 12 | size: 13 | - 1.6960964529096463 14 | - 1.6197722217277888 15 | type: box 16 | robots: 17 | - goal: 18 | - 1.4233443097690426 19 | - 4.127373706492773 20 | - -3.0297409174836183 21 | start: 22 | - 2.272944203371335 23 | - 3.98641009636059 24 | - 0.5278663832205561 25 | type: unicycle_first_order_0 26 | - goal: 27 | - 3.372352247302091 28 | - 0.6930927596529743 29 | - -2.745891939583432 30 | - -2.850180183005986 31 | start: 32 | - 3.98590506256074 33 | - 1.010157884608637 34 | - 0.7727537893398018 35 | - 0.7166395511147443 36 | type: car_first_order_with_1_trailers_0 37 | - goal: 38 | - 2.2712486919252246 39 | - 3.0867877545523137 40 | - 1.8421198405773493 41 | start: 42 | - 3.549519332847732 43 | - 1.4583038377368105 44 | - 1.3080101214786692 45 | type: unicycle_first_order_0 46 | - goal: 47 | - 2.130911699283116 48 | - 1.6143660421348947 49 | - 0.9540727645079174 50 | start: 51 | - 1.8254443233469835 52 | - 2.273621869493382 53 | - -1.4052352825463392 54 | type: unicycle_first_order_0 55 | -------------------------------------------------------------------------------- /example/gen_p10_n4_5_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 4.718219646104046 11 | - 4.953642373288481 12 | size: 13 | - 0.9274179951470405 14 | - 1.184356517904474 15 | type: box 16 | - center: 17 | - 1.3692162731755586 18 | - 2.9720888712998517 19 | size: 20 | - 0.7769551567886565 21 | - 1.2657644337477914 22 | type: box 23 | - center: 24 | - 4.925368179215845 25 | - 6.297538696472196 26 | size: 27 | - 0.9548718681311139 28 | - 0.503050060169344 29 | type: box 30 | - center: 31 | - 8.320021645794634 32 | - 0.8151535348310839 33 | size: 34 | - 0.6535964928873478 35 | - 0.2738693015117535 36 | type: box 37 | - center: 38 | - 8.385647164174971 39 | - 1.3222440851640536 40 | size: 41 | - 0.9019425554244367 42 | - 0.5864644391203633 43 | type: box 44 | - center: 45 | - 3.866195080561763 46 | - 7.2266479588873205 47 | size: 48 | - 1.0943176040811555 49 | - 1.5387829670140005 50 | type: box 51 | - center: 52 | - 8.139016687195735 53 | - 2.5484756211373623 54 | size: 55 | - 0.7114475735884842 56 | - 0.5562530421557976 57 | type: box 58 | - center: 59 | - 9.48324256549142 60 | - 8.861732279179533 61 | size: 62 | - 0.9093927865818104 63 | - 0.3284855606599859 64 | type: box 65 | - center: 66 | - 3.5015870880265627 67 | - 5.776791594041605 68 | size: 69 | - 0.9374754429561692 70 | - 1.1892821288253148 71 | type: box 72 | - center: 73 | - 7.323490691668197 74 | - 7.9796131503347505 75 | size: 76 | - 0.9045412596894384 77 | - 1.213333789073676 78 | type: box 79 | robots: 80 | - goal: 81 | - 6.274181621799077 82 | - 1.7189366773144563 83 | - 1.5073785380952351 84 | start: 85 | - 6.55005750799176 86 | - 4.953750132222987 87 | - 3.012880666117269 88 | type: unicycle_first_order_0_sphere 89 | - goal: 90 | - 6.3901712092016245 91 | - 5.020365702210674 92 | - 0.9327812645402078 93 | start: 94 | - 7.161375311315346 95 | - 3.163894006127692 96 | - 1.6635862853129906 97 | type: unicycle_first_order_0_sphere 98 | - goal: 99 | - 4.620805653462342 100 | - 3.1788009793473306 101 | - -1.6304546569062877 102 | start: 103 | - 7.274792498538937 104 | - 2.0875603868814765 105 | - -2.639459040441251 106 | type: unicycle_first_order_0_sphere 107 | - goal: 108 | - 5.6239490716455345 109 | - 6.995865778102012 110 | - 1.7476444628286067 111 | start: 112 | - 2.5865967569670754 113 | - 1.8388206245118832 114 | - -0.9038105916718577 115 | type: unicycle_first_order_0_sphere 116 | -------------------------------------------------------------------------------- /example/gen_p10_n4_6_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.7003191905594104 11 | - 2.0484084825705238 12 | size: 13 | - 0.3601317541181328 14 | - 1.6385716232740146 15 | type: box 16 | robots: 17 | - goal: 18 | - 1.3633189424196015 19 | - 4.414688064569729 20 | - 0.0 21 | - 0.0 22 | start: 23 | - 4.450113184584514 24 | - 2.8787486348890186 25 | - 0.0 26 | - 0.0 27 | type: double_integrator_0 28 | - goal: 29 | - 3.212914489077259 30 | - 4.375734615062134 31 | - -2.531659641525909 32 | start: 33 | - 3.265766208399374 34 | - 0.8846916082373912 35 | - -1.358159213297863 36 | type: unicycle_first_order_0 37 | - goal: 38 | - 3.1533418699013818 39 | - 2.624399613370018 40 | - -2.796290387211519 41 | - -3.0692170810346413 42 | start: 43 | - 1.0470948299226257 44 | - 2.9033676868327762 45 | - -1.660212691173688 46 | - -1.4160428990685037 47 | type: car_first_order_with_1_trailers_0 48 | - goal: 49 | - 3.218553217453147 50 | - 1.0952282667081938 51 | - -0.8412217807320741 52 | start: 53 | - 4.2286396296011235 54 | - 1.1934852170905934 55 | - 2.7278691844776244 56 | type: unicycle_first_order_0 57 | -------------------------------------------------------------------------------- /example/gen_p10_n4_6_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 3.07879257239369 11 | - 3.427998757558354 12 | size: 13 | - 1.3112649196686883 14 | - 0.3768067424725341 15 | type: box 16 | - center: 17 | - 0.33395392818266795 18 | - 5.749281299624941 19 | size: 20 | - 0.5876138297012361 21 | - 0.46642486993275156 22 | type: box 23 | - center: 24 | - 1.6118938918065655 25 | - 2.237294999985745 26 | size: 27 | - 1.7035354158742264 28 | - 0.7196141976686353 29 | type: box 30 | - center: 31 | - 6.174976778742372 32 | - 0.6687987344573993 33 | size: 34 | - 0.9615565858550241 35 | - 0.3384199660934323 36 | type: box 37 | - center: 38 | - 1.3304027383296395 39 | - 1.0108162942479864 40 | size: 41 | - 1.4074485045326102 42 | - 0.7435662949061095 43 | type: box 44 | - center: 45 | - 7.36241662152921 46 | - 0.3800439181835247 47 | size: 48 | - 0.7765507946248152 49 | - 0.7020757122895289 50 | type: box 51 | - center: 52 | - 6.234703888703124 53 | - 4.760928437933716 54 | size: 55 | - 1.2201762307597173 56 | - 1.0747122208192241 57 | type: box 58 | - center: 59 | - 9.212787290127874 60 | - 8.01098536780565 61 | size: 62 | - 0.11152369171213505 63 | - 1.3365177512337822 64 | type: box 65 | - center: 66 | - 0.9204741642063548 67 | - 6.492065805240794 68 | size: 69 | - 0.776020314260364 70 | - 0.3346077803003027 71 | type: box 72 | - center: 73 | - 0.5652303620657957 74 | - 7.042091296357809 75 | size: 76 | - 0.5008875478794275 77 | - 0.5662088696816385 78 | type: box 79 | - center: 80 | - 8.459855432014727 81 | - 3.494551290502203 82 | size: 83 | - 0.7634048631335115 84 | - 1.1302532054821701 85 | type: box 86 | - center: 87 | - 6.524691168808076 88 | - 6.61651898564103 89 | size: 90 | - 1.6082202587825558 91 | - 0.10736115670612023 92 | type: box 93 | - center: 94 | - 3.0542041863223997 95 | - 1.5969755507025138 96 | size: 97 | - 0.3539142545139007 98 | - 1.2404235619079136 99 | type: box 100 | robots: 101 | - goal: 102 | - 1.2704960897182982 103 | - 7.400814413626875 104 | - -2.3519727961541816 105 | start: 106 | - 6.126687756377513 107 | - 1.519961819710125 108 | - -0.6947545120989931 109 | type: unicycle_first_order_0_sphere 110 | - goal: 111 | - 3.8684654638052245 112 | - 6.415694546476315 113 | - -2.400988724313117 114 | start: 115 | - 9.34894219260869 116 | - 4.39154901929081 117 | - -0.41577613954567383 118 | type: unicycle_first_order_0_sphere 119 | - goal: 120 | - 8.952594269151488 121 | - 0.5290741630665231 122 | - -2.5141484874476294 123 | start: 124 | - 1.8990064527076747 125 | - 5.872113704098335 126 | - -0.961812464425789 127 | type: unicycle_first_order_0_sphere 128 | - goal: 129 | - 4.884753894833255 130 | - 8.26693973972959 131 | - -1.1548871370653397 132 | start: 133 | - 7.839730610555602 134 | - 9.32287288500918 135 | - 1.7068028640393438 136 | type: unicycle_first_order_0_sphere 137 | -------------------------------------------------------------------------------- /example/gen_p10_n4_7_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.750244412890249 11 | - 1.830752886332518 12 | size: 13 | - 0.19629307719041678 14 | - 1.2050283728772773 15 | type: box 16 | - center: 17 | - 0.5831018565802548 18 | - 1.5138150058004318 19 | size: 20 | - 0.3048079063198993 21 | - 1.635582369852211 22 | type: box 23 | - center: 24 | - 4.08412260146177 25 | - 1.3027222777051974 26 | size: 27 | - 0.30150274938821087 28 | - 1.7014647178439541 29 | type: box 30 | robots: 31 | - goal: 32 | - 4.082116235403415 33 | - 3.659571422028294 34 | - -0.8127849015107689 35 | start: 36 | - 0.5530478788499513 37 | - 3.3875551743840693 38 | - 2.7435245492418696 39 | type: unicycle_first_order_0 40 | - goal: 41 | - 1.5763809546844563 42 | - 4.007954895170385 43 | - -0.3060856343974381 44 | start: 45 | - 0.6900327297737276 46 | - 3.6217587818338823 47 | - 2.7437301322786354 48 | type: unicycle_first_order_0 49 | - goal: 50 | - 1.8025174574503975 51 | - 0.9925325937492864 52 | - 2.418191797902832 53 | start: 54 | - 2.926781591784653 55 | - 4.463704843990548 56 | - 3.114743045775107 57 | type: unicycle_first_order_0 58 | - goal: 59 | - 3.4987070999152055 60 | - 3.827365594537408 61 | - 0.0 62 | - 0.0 63 | start: 64 | - 1.3953589566937876 65 | - 4.138550676585822 66 | - 0.0 67 | - 0.0 68 | type: double_integrator_0 69 | -------------------------------------------------------------------------------- /example/gen_p10_n4_7_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 3.155365653737781 11 | - 2.4010484291110807 12 | size: 13 | - 0.9171661735712142 14 | - 1.0309425946057602 15 | type: box 16 | - center: 17 | - 7.815897763515854 18 | - 1.4893586892779547 19 | size: 20 | - 0.7440224354931144 21 | - 0.5256670203922181 22 | type: box 23 | - center: 24 | - 6.7743945367531175 25 | - 8.18423523557791 26 | size: 27 | - 0.14692923014387627 28 | - 1.360178303594597 29 | type: box 30 | - center: 31 | - 1.384317122199556 32 | - 3.9289230354995337 33 | size: 34 | - 0.9565882876353411 35 | - 1.6389642291127995 36 | type: box 37 | - center: 38 | - 8.015399284970572 39 | - 6.639503978012063 40 | size: 41 | - 1.712501508791974 42 | - 0.8223186554190612 43 | type: box 44 | - center: 45 | - 9.404587713436287 46 | - 3.1860294168816914 47 | size: 48 | - 0.5122021843530975 49 | - 0.73243091572218 50 | type: box 51 | - center: 52 | - 4.35913673576019 53 | - 4.044050006131207 54 | size: 55 | - 1.1300424798141586 56 | - 0.12354708406877535 57 | type: box 58 | - center: 59 | - 3.7327946206772245 60 | - 0.6097551136296291 61 | size: 62 | - 0.6247546231471595 63 | - 0.28356055826422083 64 | type: box 65 | - center: 66 | - 6.397325984482772 67 | - 3.5002724028315564 68 | size: 69 | - 1.1491912243555955 70 | - 0.422945533996447 71 | type: box 72 | - center: 73 | - 3.1946543606212265 74 | - 4.892683107233674 75 | size: 76 | - 0.513673910056455 77 | - 0.7090177190174016 78 | type: box 79 | - center: 80 | - 2.3589226668976284 81 | - 1.228705761337298 82 | size: 83 | - 1.7162649322020842 84 | - 1.2214883835816657 85 | type: box 86 | - center: 87 | - 7.593290667982028 88 | - 0.6666689471348319 89 | size: 90 | - 0.24732364889955571 91 | - 0.4034516567533817 92 | type: box 93 | robots: 94 | - goal: 95 | - 8.634147494860866 96 | - 0.6100236713348235 97 | - -0.13908763398145352 98 | start: 99 | - 9.387367805123883 100 | - 7.898090830237723 101 | - 0.450788588093642 102 | type: unicycle_first_order_0_sphere 103 | - goal: 104 | - 9.046797576109254 105 | - 2.0490527299370966 106 | - 1.347371882231596 107 | start: 108 | - 2.052282413221169 109 | - 9.325922513337849 110 | - 0.7274319219360792 111 | type: unicycle_first_order_0_sphere 112 | - goal: 113 | - 3.5659458442481293 114 | - 7.684304216808587 115 | - -0.43302088795959337 116 | start: 117 | - 1.237072443149708 118 | - 7.997372166693413 119 | - -0.5607767595346917 120 | type: unicycle_first_order_0_sphere 121 | - goal: 122 | - 4.814462949215385 123 | - 8.897178191630296 124 | - 0.9717217457617373 125 | start: 126 | - 5.912627466268789 127 | - 8.620946425595847 128 | - -1.4114738550670596 129 | type: unicycle_first_order_0_sphere 130 | -------------------------------------------------------------------------------- /example/gen_p10_n4_8_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.2590296809467696 11 | - 2.9354241248990482 12 | size: 13 | - 0.9486329638296692 14 | - 1.6296016868682799 15 | type: box 16 | - center: 17 | - 3.5712770735668005 18 | - 1.923863730902455 19 | size: 20 | - 0.801057579191631 21 | - 0.7109800689103268 22 | type: box 23 | - center: 24 | - 2.72065709772497 25 | - 4.139723276503191 26 | size: 27 | - 0.3237630797100256 28 | - 0.7316021063107313 29 | type: box 30 | - center: 31 | - 0.7860293216414429 32 | - 1.383045975829101 33 | size: 34 | - 0.9483790769762764 35 | - 1.191171082247667 36 | type: box 37 | robots: 38 | - goal: 39 | - 4.262819543518651 40 | - 3.6622938841487307 41 | - 1.1288217637822244 42 | start: 43 | - 1.9076881753848283 44 | - 1.0901166916705312 45 | - -3.0599859565624716 46 | type: unicycle_first_order_0 47 | - goal: 48 | - 0.5283636304778403 49 | - 4.065020019618498 50 | - 0.8372510889734928 51 | - 1.1886472784735378 52 | start: 53 | - 1.0314704465020506 54 | - 3.878363427389378 55 | - 3.1054377484379154 56 | - 3.216310882952117 57 | type: car_first_order_with_1_trailers_0 58 | - goal: 59 | - 4.26491453352598 60 | - 0.5592277531439307 61 | - 1.475879146653698 62 | - 1.6531283362303706 63 | start: 64 | - 4.04878733834958 65 | - 3.2557359383606506 66 | - 3.0202922538743717 67 | - 2.846711648397328 68 | type: car_first_order_with_1_trailers_0 69 | - goal: 70 | - 1.243363917335305 71 | - 4.401961091342544 72 | - -1.3103497989891315 73 | - -1.4189709493810971 74 | start: 75 | - 0.8437594877487125 76 | - 2.5164244908376836 77 | - -0.47113448128902435 78 | - -0.3864453403363819 79 | type: car_first_order_with_1_trailers_0 80 | -------------------------------------------------------------------------------- /example/gen_p10_n4_8_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 7.946335683187384 11 | - 2.946539891717225 12 | size: 13 | - 0.769912802898525 14 | - 0.5517994196900887 15 | type: box 16 | - center: 17 | - 4.603048172589686 18 | - 4.424399119903343 19 | size: 20 | - 1.564261259376122 21 | - 1.3133822039896323 22 | type: box 23 | - center: 24 | - 2.6191759999443622 25 | - 4.510650702676082 26 | size: 27 | - 0.8451986924107273 28 | - 1.2600818211557532 29 | type: box 30 | - center: 31 | - 3.5916622398621496 32 | - 1.2349277066440996 33 | size: 34 | - 0.6263102365072271 35 | - 0.6829840940180907 36 | type: box 37 | - center: 38 | - 8.452998321026401 39 | - 1.3536474442403525 40 | size: 41 | - 1.1074398963495968 42 | - 1.7223065527865995 43 | type: box 44 | - center: 45 | - 1.418718948964579 46 | - 3.3391572579390463 47 | size: 48 | - 1.3390542009173938 49 | - 0.7702316564885783 50 | type: box 51 | - center: 52 | - 6.264058949543737 53 | - 3.493391355522749 54 | size: 55 | - 0.9745592855273182 56 | - 0.6826720105864603 57 | type: box 58 | - center: 59 | - 4.857571029834407 60 | - 7.204265282463814 61 | size: 62 | - 0.8312046158622713 63 | - 0.7850405609649856 64 | type: box 65 | - center: 66 | - 2.685769710740716 67 | - 7.767464301746043 68 | size: 69 | - 1.0359130066374067 70 | - 0.541464705062039 71 | type: box 72 | - center: 73 | - 3.5318802388036317 74 | - 2.628637320799217 75 | size: 76 | - 0.885057748529336 77 | - 1.4259448873168847 78 | type: box 79 | robots: 80 | - goal: 81 | - 1.6639075475482366 82 | - 5.99866504929827 83 | - -2.6572259482036356 84 | start: 85 | - 8.710570272094996 86 | - 8.431626207321424 87 | - -2.6061405639753987 88 | type: unicycle_first_order_0_sphere 89 | - goal: 90 | - 1.9404646009981548 91 | - 9.301043968251644 92 | - -2.153647636306234 93 | start: 94 | - 8.024102454246059 95 | - 9.006670198462285 96 | - -3.1273616094118184 97 | type: unicycle_first_order_0_sphere 98 | - goal: 99 | - 5.11831120820451 100 | - 1.4649847408409247 101 | - -0.2936111037143174 102 | start: 103 | - 7.761845893440829 104 | - 5.567154192000684 105 | - -1.4623587312059387 106 | type: unicycle_first_order_0_sphere 107 | - goal: 108 | - 3.7569505724206547 109 | - 7.3542968372680555 110 | - -1.8542387320108211 111 | start: 112 | - 9.108661012402015 113 | - 2.7996192275774803 114 | - 1.695327680451034 115 | type: unicycle_first_order_0_sphere 116 | -------------------------------------------------------------------------------- /example/gen_p10_n4_9_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.0301909807915 11 | - 3.516492410369035 12 | size: 13 | - 2.035445332545076 14 | - 1.1284475650429289 15 | type: box 16 | - center: 17 | - 1.3178128108373686 18 | - 2.4732115529487726 19 | size: 20 | - 0.9485263602885519 21 | - 0.5129900977527686 22 | type: box 23 | robots: 24 | - goal: 25 | - 2.1566097626933094 26 | - 1.9923447059285952 27 | - -0.1509488372918173 28 | start: 29 | - 1.5765578657061918 30 | - 0.5624139288417065 31 | - -2.5455806290733407 32 | type: unicycle_first_order_0 33 | - goal: 34 | - 3.959383130290802 35 | - 3.776367499799822 36 | - 0.0 37 | - 0.0 38 | start: 39 | - 3.356043146157348 40 | - 1.304280361458813 41 | - 0.0 42 | - 0.0 43 | type: double_integrator_0 44 | - goal: 45 | - 3.0164976778473314 46 | - 1.612341814685946 47 | - -1.0669406497759848 48 | start: 49 | - 1.1688622910214157 50 | - 0.704656567913712 51 | - -1.6488809298434195 52 | type: unicycle_first_order_0 53 | - goal: 54 | - 0.573186420951707 55 | - 1.1478852960116632 56 | - 2.707232794313419 57 | start: 58 | - 3.8391797266958565 59 | - 1.4237436066269447 60 | - 0.6541185914100733 61 | type: unicycle_first_order_0 62 | -------------------------------------------------------------------------------- /example/gen_p10_n4_9_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.201319696417258 11 | - 0.10918017722729756 12 | size: 13 | - 0.7263999020569044 14 | - 0.13778285490930198 15 | type: box 16 | - center: 17 | - 3.642940789078706 18 | - 7.505004592857322 19 | size: 20 | - 0.6190578218831954 21 | - 1.6239575473664414 22 | type: box 23 | - center: 24 | - 7.784915325398715 25 | - 1.2547320048531287 26 | size: 27 | - 0.8471237005498718 28 | - 0.9664086577056106 29 | type: box 30 | - center: 31 | - 4.774911531148934 32 | - 2.8596514773634967 33 | size: 34 | - 0.8745484464915169 35 | - 0.7592501425340407 36 | type: box 37 | - center: 38 | - 4.869903274746837 39 | - 4.516183459400802 40 | size: 41 | - 0.7412515552780439 42 | - 0.9307444936627363 43 | type: box 44 | - center: 45 | - 8.957823580575038 46 | - 3.872839203114077 47 | size: 48 | - 1.0547640154705848 49 | - 1.172192119899325 50 | type: box 51 | - center: 52 | - 0.9083344261941035 53 | - 8.86666916348929 54 | size: 55 | - 0.16818310068008058 56 | - 1.528149485773105 57 | type: box 58 | - center: 59 | - 1.189969628073161 60 | - 4.620561175578528 61 | size: 62 | - 1.6660694484352794 63 | - 0.942553190363376 64 | type: box 65 | - center: 66 | - 5.48224054117722 67 | - 4.251038443739054 68 | size: 69 | - 0.4510736807350055 70 | - 1.276906831040976 71 | type: box 72 | - center: 73 | - 5.168799059976014 74 | - 7.124403036978922 75 | size: 76 | - 0.4366053312040125 77 | - 0.9896641392450236 78 | type: box 79 | - center: 80 | - 6.782522631022661 81 | - 4.532219482061143 82 | size: 83 | - 0.8622702503168159 84 | - 0.4808245594227755 85 | type: box 86 | robots: 87 | - goal: 88 | - 2.776627045834947 89 | - 3.670100847685835 90 | - 0.41386489078644306 91 | start: 92 | - 7.448293237911716 93 | - 3.352274719160739 94 | - -1.9618238026206662 95 | type: unicycle_first_order_0_sphere 96 | - goal: 97 | - 6.569352694109998 98 | - 1.2644929581300564 99 | - 3.0504526264964156 100 | start: 101 | - 1.6691029938236093 102 | - 6.438255742447136 103 | - 1.7297312769873177 104 | type: unicycle_first_order_0_sphere 105 | - goal: 106 | - 7.589758842159212 107 | - 2.433906903440538 108 | - 1.6618260528619686 109 | start: 110 | - 6.1029265264713315 111 | - 8.42620117590753 112 | - 0.08218574027266268 113 | type: unicycle_first_order_0_sphere 114 | - goal: 115 | - 7.978039512303625 116 | - 7.473896690214687 117 | - 0.2055344637853298 118 | start: 119 | - 2.042394480194229 120 | - 2.7675964353320865 121 | - -2.815706264353705 122 | type: unicycle_first_order_0_sphere 123 | -------------------------------------------------------------------------------- /example/gen_p10_n8_0_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.140128543938917 11 | - 3.7567346715166594 12 | size: 13 | - 1.150588445763527 14 | - 0.7010188552624659 15 | type: box 16 | - center: 17 | - 4.225703208392316 18 | - 1.816749031491978 19 | size: 20 | - 0.6151396302802175 21 | - 0.4601612152896595 22 | type: box 23 | - center: 24 | - 2.957811862870456 25 | - 4.734259186830463 26 | size: 27 | - 0.5299437291943896 28 | - 0.4042847737245557 29 | type: box 30 | - center: 31 | - 1.1276466411506987 32 | - 3.639969477986199 33 | size: 34 | - 0.6696261170801144 35 | - 1.5032922806377824 36 | type: box 37 | robots: 38 | - goal: 39 | - 3.947974172187418 40 | - 1.3408253397229175 41 | - 0.0 42 | - 0.0 43 | start: 44 | - 1.473224662562278 45 | - 0.8564836540490588 46 | - 0.0 47 | - 0.0 48 | type: double_integrator_0 49 | - goal: 50 | - 3.290256329508076 51 | - 1.2200632345041424 52 | - 1.8344092627036508 53 | - 1.4731989366939142 54 | start: 55 | - 3.8195573545220864 56 | - 0.9321400259686943 57 | - -0.49465234552993564 58 | - -0.865627731350606 59 | type: car_first_order_with_1_trailers_0 60 | - goal: 61 | - 2.294880908228542 62 | - 2.62910878413347 63 | - -0.480505154601945 64 | - -0.10053572953621404 65 | start: 66 | - 0.8093235037090776 67 | - 0.9837492365331264 68 | - -2.8286396951894424 69 | - -2.5845944553234546 70 | type: car_first_order_with_1_trailers_0 71 | - goal: 72 | - 2.4248522753391417 73 | - 2.0920175141086306 74 | - 0.0 75 | - 0.0 76 | start: 77 | - 4.390985139373709 78 | - 3.2152211192428837 79 | - 0.0 80 | - 0.0 81 | type: double_integrator_0 82 | - goal: 83 | - 2.890585756323203 84 | - 0.884249519703677 85 | - 1.8238331860031005 86 | - 1.5158721982084553 87 | start: 88 | - 1.1182655171535307 89 | - 2.5201681621120193 90 | - 1.8408024435720698 91 | - 2.1343819146038574 92 | type: car_first_order_with_1_trailers_0 93 | - goal: 94 | - 3.58371310141121 95 | - 3.961642733593945 96 | - 2.0277612315697873 97 | - 2.1924478176177673 98 | start: 99 | - 2.8204294116401436 100 | - 2.141364838248208 101 | - -0.009570705183843398 102 | - 0.29474920799170967 103 | type: car_first_order_with_1_trailers_0 104 | - goal: 105 | - 0.5536463906550013 106 | - 3.96434383055173 107 | - 0.0 108 | - 0.0 109 | start: 110 | - 1.9080350372725543 111 | - 1.9507834342288444 112 | - 0.0 113 | - 0.0 114 | type: double_integrator_0 115 | - goal: 116 | - 2.9275903955595917 117 | - 1.7516453161802907 118 | - 0.7583241162045851 119 | - 0.5944528759084188 120 | start: 121 | - 2.243172325281992 122 | - 0.5819581586760236 123 | - 2.021306348339867 124 | - 1.7690066170068885 125 | type: car_first_order_with_1_trailers_0 126 | -------------------------------------------------------------------------------- /example/gen_p10_n8_0_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 7.717991565475449 11 | - 6.420349637103256 12 | size: 13 | - 0.962795835827724 14 | - 0.33084594733938605 15 | type: box 16 | - center: 17 | - 3.2419070319829397 18 | - 8.675037292304507 19 | size: 20 | - 0.2992241759703243 21 | - 0.1347511209185419 22 | type: box 23 | - center: 24 | - 7.410412653140067 25 | - 8.365115837928617 26 | size: 27 | - 0.9118668983496713 28 | - 1.5299071436848026 29 | type: box 30 | - center: 31 | - 5.403921496083837 32 | - 3.2417546645871336 33 | size: 34 | - 1.0966291164965458 35 | - 0.23639494466098598 36 | type: box 37 | - center: 38 | - 6.958878667107608 39 | - 2.438940971234908 40 | size: 41 | - 0.9140679538020043 42 | - 0.33073475469177627 43 | type: box 44 | - center: 45 | - 4.726955489405507 46 | - 4.872075514852145 47 | size: 48 | - 1.38654010794626 49 | - 0.920835807263661 50 | type: box 51 | - center: 52 | - 7.848544067170998 53 | - 9.387792484387354 54 | size: 55 | - 1.1870726357345847 56 | - 0.43582037720010036 57 | type: box 58 | - center: 59 | - 7.85531865127587 60 | - 2.924181473793166 61 | size: 62 | - 0.6165559256738146 63 | - 0.2340631165921513 64 | type: box 65 | - center: 66 | - 2.240464362342775 67 | - 7.092891889353159 68 | size: 69 | - 1.4307353128740619 70 | - 0.55793368907295 71 | type: box 72 | - center: 73 | - 8.350081973794723 74 | - 7.956942334588864 75 | size: 76 | - 0.2351324751964028 77 | - 0.7634557425648892 78 | type: box 79 | - center: 80 | - 3.5083431752043723 81 | - 1.4122275793047498 82 | size: 83 | - 0.5274721308537622 84 | - 0.14980853701032093 85 | type: box 86 | - center: 87 | - 8.357257979550397 88 | - 0.6967414165001715 89 | size: 90 | - 1.1017747856644133 91 | - 0.7423266519023389 92 | type: box 93 | robots: 94 | - goal: 95 | - 4.3990515857221135 96 | - 6.937636631475059 97 | - -2.1042488932528163 98 | start: 99 | - 7.460228520827291 100 | - 5.249219181345689 101 | - -1.186029352263994 102 | type: unicycle_first_order_0_sphere 103 | - goal: 104 | - 1.8051445112729243 105 | - 1.9460111241178466 106 | - -2.1514628867224483 107 | start: 108 | - 5.83160993785382 109 | - 6.33928641438477 110 | - -2.8649345798470147 111 | type: unicycle_first_order_0_sphere 112 | - goal: 113 | - 3.407261669346833 114 | - 6.265957311517591 115 | - -1.5708026014472096 116 | start: 117 | - 2.4138241349562213 118 | - 8.304772341673903 119 | - 1.640729381478156 120 | type: unicycle_first_order_0_sphere 121 | - goal: 122 | - 6.019943957113316 123 | - 1.0708793203642892 124 | - -1.6002616203743407 125 | start: 126 | - 6.958896630904719 127 | - 7.049663107316776 128 | - 1.546640965046632 129 | type: unicycle_first_order_0_sphere 130 | - goal: 131 | - 8.407425924609448 132 | - 5.269162242925539 133 | - -0.33919048050068223 134 | start: 135 | - 3.563585560328981 136 | - 7.980682708587129 137 | - -2.718437254356812 138 | type: unicycle_first_order_0_sphere 139 | - goal: 140 | - 9.275460400652541 141 | - 4.391217293026753 142 | - -1.744060171969284 143 | start: 144 | - 3.308476003358405 145 | - 4.557165856442027 146 | - 2.552099236170017 147 | type: unicycle_first_order_0_sphere 148 | - goal: 149 | - 6.161167765981977 150 | - 9.381913969129155 151 | - -0.06766420961411335 152 | start: 153 | - 8.668383970357946 154 | - 1.6824959870268512 155 | - 2.635999025661265 156 | type: unicycle_first_order_0_sphere 157 | - goal: 158 | - 9.43355227935852 159 | - 2.874717288155127 160 | - -2.626850913044588 161 | start: 162 | - 2.9205621485467916 163 | - 6.151721958568775 164 | - 2.0666972223765283 165 | type: unicycle_first_order_0_sphere 166 | -------------------------------------------------------------------------------- /example/gen_p10_n8_1_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 3.5654325958817816 11 | - 3.1479264240007327 12 | size: 13 | - 0.29880881307112167 14 | - 0.9867103419733676 15 | type: box 16 | - center: 17 | - 3.673488686184617 18 | - 1.126112661092503 19 | size: 20 | - 0.9667536075734001 21 | - 1.2622157625683614 22 | type: box 23 | robots: 24 | - goal: 25 | - 2.7868660207913063 26 | - 1.306046715210027 27 | - 0.6295479964050932 28 | start: 29 | - 4.284180480151418 30 | - 2.0186842753059935 31 | - -2.8757394445684388 32 | type: unicycle_first_order_0 33 | - goal: 34 | - 1.842890505609342 35 | - 1.6966015824145044 36 | - 2.704690287034346 37 | start: 38 | - 2.702026370874633 39 | - 4.235293177141129 40 | - -0.01506238415454808 41 | type: unicycle_first_order_0 42 | - goal: 43 | - 3.000350595479547 44 | - 3.1829497739835046 45 | - 0.0 46 | - 0.0 47 | start: 48 | - 0.9463838854995177 49 | - 3.1333785289761784 50 | - 0.0 51 | - 0.0 52 | type: double_integrator_0 53 | - goal: 54 | - 1.389613957692267 55 | - 1.808256493599821 56 | - 1.1927418679064354 57 | - 1.4577009876551226 58 | start: 59 | - 2.1987254723007044 60 | - 2.414941402720078 61 | - -1.8866527634690888 62 | - -1.8469327897853667 63 | type: car_first_order_with_1_trailers_0 64 | - goal: 65 | - 0.5337665437088357 66 | - 2.1617231758080067 67 | - -2.43099573952503 68 | - -2.7944520841231437 69 | start: 70 | - 4.354437241630009 71 | - 3.3091429370606154 72 | - -2.728115061432088 73 | - -2.3482407676952386 74 | type: car_first_order_with_1_trailers_0 75 | - goal: 76 | - 3.6372074555686096 77 | - 4.20832981093209 78 | - 3.0864566833858618 79 | start: 80 | - 0.9507187432939381 81 | - 1.4087653496241108 82 | - 0.76823199697195 83 | type: unicycle_first_order_0 84 | - goal: 85 | - 2.772047386326051 86 | - 4.034801595967856 87 | - 0.0 88 | - 0.0 89 | start: 90 | - 0.6410733325892477 91 | - 2.9654709762333744 92 | - 0.0 93 | - 0.0 94 | type: double_integrator_0 95 | - goal: 96 | - 2.4150152421078044 97 | - 4.416378837618684 98 | - -2.6518067712733093 99 | - -2.469201930645875 100 | start: 101 | - 1.5936412197908223 102 | - 2.7097647991201788 103 | - 0.9836850042089722 104 | - 0.9642691266245189 105 | type: car_first_order_with_1_trailers_0 106 | -------------------------------------------------------------------------------- /example/gen_p10_n8_2_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 0.24145835494306692 11 | - 3.656313802181155 12 | size: 13 | - 0.4730461171742548 14 | - 0.25155025846602497 15 | type: box 16 | - center: 17 | - 1.6497082326317483 18 | - 1.7797343655474347 19 | size: 20 | - 0.3078445740429138 21 | - 0.6880129010465804 22 | type: box 23 | - center: 24 | - 3.917333978085557 25 | - 3.6657283506794855 26 | size: 27 | - 0.1709507809880525 28 | - 0.7104881361546074 29 | type: box 30 | - center: 31 | - 0.9599620094177328 32 | - 4.018264012323244 33 | size: 34 | - 0.5565644779905502 35 | - 0.35008846054277004 36 | type: box 37 | - center: 38 | - 4.202067419639406 39 | - 4.581595667270962 40 | size: 41 | - 0.8605125830136984 42 | - 0.4043274299818017 43 | type: box 44 | - center: 45 | - 2.628057474246631 46 | - 2.2087465307474865 47 | size: 48 | - 0.9274853214606515 49 | - 0.458190360953424 50 | type: box 51 | robots: 52 | - goal: 53 | - 1.6848406425314986 54 | - 0.5576709013155692 55 | - -2.20262179484917 56 | start: 57 | - 2.6330745728504685 58 | - 3.1990833383745114 59 | - -2.5873722036312317 60 | type: unicycle_first_order_0 61 | - goal: 62 | - 1.631640368356989 63 | - 2.6510436250210367 64 | - -1.7736330554972974 65 | - -1.934173325694565 66 | start: 67 | - 4.398946800120227 68 | - 1.3776084686613816 69 | - 2.0384056234597576 70 | - 1.8185325450799399 71 | type: car_first_order_with_1_trailers_0 72 | - goal: 73 | - 3.528858958318971 74 | - 3.076022549697229 75 | - -3.1044774982501955 76 | start: 77 | - 0.9995672021355579 78 | - 0.6044172038219409 79 | - -1.9775068082523746 80 | type: unicycle_first_order_0 81 | - goal: 82 | - 3.927403326657771 83 | - 1.3434118240626902 84 | - 0.0 85 | - 0.0 86 | start: 87 | - 2.464123456674883 88 | - 3.4358542464977053 89 | - 0.0 90 | - 0.0 91 | type: double_integrator_0 92 | - goal: 93 | - 3.277862051590517 94 | - 4.400953826513019 95 | - 0.0 96 | - 0.0 97 | start: 98 | - 2.812641076954631 99 | - 0.7876247528800637 100 | - 0.0 101 | - 0.0 102 | type: double_integrator_0 103 | - goal: 104 | - 2.2866010119789015 105 | - 4.281839773103094 106 | - 1.5552068796253833 107 | - 1.8401093581309518 108 | start: 109 | - 0.8072536588727592 110 | - 2.003003294334984 111 | - -2.119471190436671 112 | - -1.9244562139700594 113 | type: car_first_order_with_1_trailers_0 114 | - goal: 115 | - 4.458346431198562 116 | - 0.5674058145663081 117 | - 0.0 118 | - 0.0 119 | start: 120 | - 1.9245119673661502 121 | - 3.090421314703792 122 | - 0.0 123 | - 0.0 124 | type: double_integrator_0 125 | - goal: 126 | - 3.930264153140249 127 | - 2.780253807618154 128 | - 0.43415587566747593 129 | - 0.6598220126651746 130 | start: 131 | - 2.213921446905957 132 | - 4.174076988306965 133 | - -2.1811149756190353 134 | - -1.834317522044944 135 | type: car_first_order_with_1_trailers_0 136 | -------------------------------------------------------------------------------- /example/gen_p10_n8_3_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 4.457630185393278 11 | - 2.1115718402469223 12 | size: 13 | - 0.8204986116421243 14 | - 1.003280621780129 15 | type: box 16 | - center: 17 | - 4.568780727327582 18 | - 3.896262426653958 19 | size: 20 | - 0.21705612258009943 21 | - 0.9572864061424864 22 | type: box 23 | - center: 24 | - 1.7244423305561343 25 | - 3.603537585014181 26 | size: 27 | - 0.23732285125113706 28 | - 0.16603680884304783 29 | type: box 30 | - center: 31 | - 3.347373816354182 32 | - 1.1758339142410699 33 | size: 34 | - 0.8387645478440403 35 | - 0.7647730190048481 36 | type: box 37 | robots: 38 | - goal: 39 | - 1.2086672632550406 40 | - 3.88010299101065 41 | - 1.3358190100583043 42 | - 1.639533373444047 43 | start: 44 | - 3.40037597097991 45 | - 3.4356626337785703 46 | - 0.07945359143763486 47 | - 0.08147532155580184 48 | type: car_first_order_with_1_trailers_0 49 | - goal: 50 | - 3.6401810577047553 51 | - 3.929642950404332 52 | - 1.2671934922057604 53 | - 1.0804205971778698 54 | start: 55 | - 2.4638455354941446 56 | - 1.1199761195157132 57 | - -1.582215971987642 58 | - -1.3484335918396422 59 | type: car_first_order_with_1_trailers_0 60 | - goal: 61 | - 0.9204419088966649 62 | - 0.8529565291684369 63 | - 0.0 64 | - 0.0 65 | start: 66 | - 3.688590178794093 67 | - 2.9861840204082033 68 | - 0.0 69 | - 0.0 70 | type: double_integrator_0 71 | - goal: 72 | - 0.9836736758816014 73 | - 4.293707526593529 74 | - 1.2498287687561724 75 | - 1.3687167889324372 76 | start: 77 | - 1.0843506261451288 78 | - 4.078989610099473 79 | - -1.4589514152276988 80 | - -1.5409891394272957 81 | type: car_first_order_with_1_trailers_0 82 | - goal: 83 | - 0.526261429901437 84 | - 4.288880255883481 85 | - 3.130625813670849 86 | start: 87 | - 1.6458026674796349 88 | - 1.3035261873502098 89 | - 0.7604126702407061 90 | type: unicycle_first_order_0 91 | - goal: 92 | - 3.4193983398778762 93 | - 1.8009733931460286 94 | - 0.0 95 | - 0.0 96 | start: 97 | - 2.756506846553084 98 | - 4.153848448447077 99 | - 0.0 100 | - 0.0 101 | type: double_integrator_0 102 | - goal: 103 | - 2.529875289391564 104 | - 3.5930264373525027 105 | - 0.0 106 | - 0.0 107 | start: 108 | - 0.5799723507082466 109 | - 3.0831414048054455 110 | - 0.0 111 | - 0.0 112 | type: double_integrator_0 113 | - goal: 114 | - 0.9663264548448827 115 | - 1.5982496887738669 116 | - 0.0 117 | - 0.0 118 | start: 119 | - 4.066442797131079 120 | - 0.783697948634507 121 | - 0.0 122 | - 0.0 123 | type: double_integrator_0 124 | -------------------------------------------------------------------------------- /example/gen_p10_n8_4_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.3851299714780043 11 | - 1.7579466641262496 12 | size: 13 | - 0.7275622828317296 14 | - 0.7986846015512448 15 | type: box 16 | - center: 17 | - 2.045933126455031 18 | - 4.068687300695435 19 | size: 20 | - 0.7564172722159029 21 | - 0.6201765057203966 22 | type: box 23 | robots: 24 | - goal: 25 | - 0.7489391705203596 26 | - 3.9581253205695917 27 | - 0.18369582621612857 28 | - 0.16883142272001983 29 | start: 30 | - 4.065061472214568 31 | - 3.647393117485589 32 | - 2.964116032548608 33 | - 3.0290016124396595 34 | type: car_first_order_with_1_trailers_0 35 | - goal: 36 | - 4.3497003252234805 37 | - 1.7740910265619774 38 | - 0.7392658155621299 39 | start: 40 | - 2.0874749492526594 41 | - 0.941002149924171 42 | - -0.8173683354751264 43 | type: unicycle_first_order_0 44 | - goal: 45 | - 2.7788370025085865 46 | - 3.690029819115549 47 | - 0.30524386871462594 48 | - 0.3423299709964285 49 | start: 50 | - 3.677049434520165 51 | - 1.3384942251895748 52 | - -2.642759637649984 53 | - -2.380483497865392 54 | type: car_first_order_with_1_trailers_0 55 | - goal: 56 | - 3.070745986756602 57 | - 4.461731841175031 58 | - 0.0 59 | - 0.0 60 | start: 61 | - 3.807167100217823 62 | - 1.0456162417325907 63 | - 0.0 64 | - 0.0 65 | type: double_integrator_0 66 | - goal: 67 | - 2.1341692223338167 68 | - 2.364722060963219 69 | - -2.876575993644126 70 | start: 71 | - 2.9473137820868933 72 | - 0.5669562681591294 73 | - -1.8778731072719244 74 | type: unicycle_first_order_0 75 | - goal: 76 | - 3.939683189026904 77 | - 3.4897685719445577 78 | - -1.2573599944648228 79 | start: 80 | - 4.317592466240148 81 | - 0.5071719832948185 82 | - 2.3523448309636574 83 | type: unicycle_first_order_0 84 | - goal: 85 | - 3.805456337237684 86 | - 0.8817672067155531 87 | - 0.0 88 | - 0.0 89 | start: 90 | - 0.8440125022911085 91 | - 4.379569323653549 92 | - 0.0 93 | - 0.0 94 | type: double_integrator_0 95 | - goal: 96 | - 3.0923118796944338 97 | - 0.9674344762709968 98 | - 2.433335078539878 99 | start: 100 | - 4.274192591587896 101 | - 3.1496527322957304 102 | - 1.9871274877934777 103 | type: unicycle_first_order_0 104 | -------------------------------------------------------------------------------- /example/gen_p10_n8_4_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 0.9194854660465173 11 | - 2.6838645873682045 12 | size: 13 | - 0.7876164896226727 14 | - 0.9504017871727217 15 | type: box 16 | - center: 17 | - 4.1037017796680075 18 | - 2.8860949409134395 19 | size: 20 | - 0.8356738582169405 21 | - 0.474338152481754 22 | type: box 23 | - center: 24 | - 2.9605820741031783 25 | - 6.8093747648692755 26 | size: 27 | - 1.6486703444956343 28 | - 1.696999325543401 29 | type: box 30 | - center: 31 | - 7.30875780812558 32 | - 8.339336459874396 33 | size: 34 | - 0.8188368538290657 35 | - 0.8821230422357806 36 | type: box 37 | - center: 38 | - 7.33447504518997 39 | - 4.301659161204812 40 | size: 41 | - 1.7036838297353987 42 | - 0.9777815296378303 43 | type: box 44 | - center: 45 | - 8.155132806907588 46 | - 1.8690255824976159 47 | size: 48 | - 0.38097344980923104 49 | - 0.7099863208953686 50 | type: box 51 | - center: 52 | - 8.999145248753708 53 | - 7.558740345554432 54 | size: 55 | - 0.649083704421021 56 | - 0.6347842042220826 57 | type: box 58 | - center: 59 | - 7.623358893459836 60 | - 6.816728533419977 61 | size: 62 | - 0.1304149745487836 63 | - 1.499032948303532 64 | type: box 65 | - center: 66 | - 1.793085910625136 67 | - 7.195391393714357 68 | size: 69 | - 0.31116605859789853 70 | - 0.6715372172339882 71 | type: box 72 | - center: 73 | - 5.826169502053348 74 | - 8.870890120646898 75 | size: 76 | - 0.5366309894761292 77 | - 0.5176386615208627 78 | type: box 79 | - center: 80 | - 2.975765350071332 81 | - 3.108344158680155 82 | size: 83 | - 0.5062375823886383 84 | - 1.4825263958970156 85 | type: box 86 | robots: 87 | - goal: 88 | - 4.821720310051122 89 | - 0.7363766098493366 90 | - 2.2402566044485273 91 | start: 92 | - 4.41312010365578 93 | - 5.463026656341151 94 | - -2.3524214729700246 95 | type: unicycle_first_order_0_sphere 96 | - goal: 97 | - 6.57307131984419 98 | - 6.808709762708169 99 | - 2.255102279634311 100 | start: 101 | - 1.0214111922324234 102 | - 7.544556344464381 103 | - 2.5361233777653434 104 | type: unicycle_first_order_0_sphere 105 | - goal: 106 | - 0.8365636041686861 107 | - 9.19924730455105 108 | - 1.6596211826736509 109 | start: 110 | - 4.787544774005589 111 | - 0.8623734306496212 112 | - 3.1177935816416165 113 | type: unicycle_first_order_0_sphere 114 | - goal: 115 | - 7.875808333843605 116 | - 9.316835507132101 117 | - 1.0166118390966101 118 | start: 119 | - 0.7067110695287918 120 | - 6.231238855547565 121 | - 0.6770074859469082 122 | type: unicycle_first_order_0_sphere 123 | - goal: 124 | - 0.7147838332254113 125 | - 3.7460218766417865 126 | - -0.015288439875088766 127 | start: 128 | - 6.108153400289863 129 | - 2.239253743058775 130 | - 0.1540612056160211 131 | type: unicycle_first_order_0_sphere 132 | - goal: 133 | - 3.991339252218375 134 | - 1.106320864914969 135 | - 0.8853170803064012 136 | start: 137 | - 9.213950321791238 138 | - 5.42796507113864 139 | - -0.6802518903981514 140 | type: unicycle_first_order_0_sphere 141 | - goal: 142 | - 8.815589853902521 143 | - 3.5845025411039995 144 | - -0.9298033968722033 145 | start: 146 | - 2.000964444572196 147 | - 1.1935081342265903 148 | - -1.629360216742622 149 | type: unicycle_first_order_0_sphere 150 | - goal: 151 | - 3.359294332040335 152 | - 4.928392675677183 153 | - -0.5158370364462868 154 | start: 155 | - 8.846135758471641 156 | - 0.8173735872741605 157 | - 0.5799530556075747 158 | type: unicycle_first_order_0_sphere 159 | -------------------------------------------------------------------------------- /example/gen_p10_n8_5_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.5314013294520707 11 | - 1.8129962673712754 12 | size: 13 | - 0.16940781576024178 14 | - 0.5859469971967813 15 | type: box 16 | - center: 17 | - 2.548838025193625 18 | - 1.0938891896365093 19 | size: 20 | - 1.1835591967021535 21 | - 0.7064887001107404 22 | type: box 23 | - center: 24 | - 2.5770509473337295 25 | - 2.892732168217372 26 | size: 27 | - 0.7205820461648359 28 | - 0.8760805361855015 29 | type: box 30 | - center: 31 | - 2.874980711705666 32 | - 4.451586691960043 33 | size: 34 | - 1.1532487613298301 35 | - 0.10566657365719934 36 | type: box 37 | - center: 38 | - 4.3257513014296425 39 | - 3.796533800633758 40 | size: 41 | - 1.3170990046533124 42 | - 1.4168656678230285 43 | type: box 44 | robots: 45 | - goal: 46 | - 0.7765497359772384 47 | - 2.270964002304999 48 | - -2.462962966468114 49 | - -2.819172575532073 50 | start: 51 | - 1.3420881852725839 52 | - 2.56749194044098 53 | - 2.3815965506956536 54 | - 2.3506748944500715 55 | type: car_first_order_with_1_trailers_0 56 | - goal: 57 | - 0.6899039852838671 58 | - 1.0671353469792129 59 | - 0.0 60 | - 0.0 61 | start: 62 | - 3.3957841249680634 63 | - 2.4233519532898034 64 | - 0.0 65 | - 0.0 66 | type: double_integrator_0 67 | - goal: 68 | - 1.1149514358260735 69 | - 2.885825316753763 70 | - -1.954625129827918 71 | - -1.9082573558307412 72 | start: 73 | - 0.5075307115792582 74 | - 2.011555067885766 75 | - 2.8764235306978616 76 | - 2.9238739888009917 77 | type: car_first_order_with_1_trailers_0 78 | - goal: 79 | - 3.246923650064512 80 | - 4.07980743731963 81 | - 0.0 82 | - 0.0 83 | start: 84 | - 4.072785476308471 85 | - 0.6245777772213774 86 | - 0.0 87 | - 0.0 88 | type: double_integrator_0 89 | - goal: 90 | - 0.7009180134949533 91 | - 0.655845165424255 92 | - 0.5660830749596255 93 | - 0.19282787905350723 94 | start: 95 | - 1.8918793171929624 96 | - 3.753026107948073 97 | - -2.2643679727267667 98 | - -2.55351998697655 99 | type: car_first_order_with_1_trailers_0 100 | - goal: 101 | - 1.5022853844810506 102 | - 1.2140057648267266 103 | - 0.0 104 | - 0.0 105 | start: 106 | - 3.461524048601772 107 | - 3.981347333480301 108 | - 0.0 109 | - 0.0 110 | type: double_integrator_0 111 | - goal: 112 | - 2.2023769756674936 113 | - 3.84379399386462 114 | - 0.0 115 | - 0.0 116 | start: 117 | - 0.5494490906962302 118 | - 1.6583644444834076 119 | - 0.0 120 | - 0.0 121 | type: double_integrator_0 122 | - goal: 123 | - 1.00865805510842 124 | - 3.819213493801476 125 | - -0.6172574916816078 126 | start: 127 | - 3.4625607686423474 128 | - 1.5823574868780113 129 | - 2.22415503918622 130 | type: unicycle_first_order_0 131 | -------------------------------------------------------------------------------- /example/gen_p10_n8_5_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 4.675079858569682 11 | - 8.671406485024187 12 | size: 13 | - 0.37041535172342505 14 | - 0.6270606706793987 15 | type: box 16 | - center: 17 | - 4.597494122917991 18 | - 4.721916256115972 19 | size: 20 | - 0.5231527306155352 21 | - 0.9794943993541068 22 | type: box 23 | - center: 24 | - 3.25660801453505 25 | - 5.262822747162993 26 | size: 27 | - 0.9616745493135566 28 | - 1.264209545026524 29 | type: box 30 | - center: 31 | - 6.550682838675052 32 | - 5.3217576549610035 33 | size: 34 | - 0.3681828871627117 35 | - 0.7696665234783817 36 | type: box 37 | - center: 38 | - 5.984814364723676 39 | - 1.4481421369617014 40 | size: 41 | - 0.6268169044497468 42 | - 0.5996283087342515 43 | type: box 44 | - center: 45 | - 4.990752157325606 46 | - 1.8975513508024116 47 | size: 48 | - 0.324772603853404 49 | - 1.0656525369879188 50 | type: box 51 | - center: 52 | - 1.768369093826014 53 | - 3.4561706919636936 54 | size: 55 | - 0.3255006532323505 56 | - 1.5410016257754253 57 | type: box 58 | - center: 59 | - 3.3682109207778055 60 | - 2.5954430409685045 61 | size: 62 | - 0.933139126018097 63 | - 1.898684833136351 64 | type: box 65 | - center: 66 | - 5.2613606374009505 67 | - 3.142042740573313 68 | size: 69 | - 1.6881652640060314 70 | - 1.1215723560384574 71 | type: box 72 | - center: 73 | - 0.8122421595041316 74 | - 4.41282165308014 75 | size: 76 | - 0.18011481430387222 77 | - 0.7456597354385024 78 | type: box 79 | - center: 80 | - 0.7222248870254038 81 | - 2.898796861051155 82 | size: 83 | - 1.376368737276108 84 | - 0.4909411513661847 85 | type: box 86 | - center: 87 | - 9.21122814270632 88 | - 9.224765534947668 89 | size: 90 | - 1.4654320668734933 91 | - 0.9422129922833868 92 | type: box 93 | robots: 94 | - goal: 95 | - 8.785187065374398 96 | - 4.6845625839322675 97 | - 0.14655421600993135 98 | start: 99 | - 9.167727441722151 100 | - 0.6808240399674891 101 | - -0.7338475204042427 102 | type: unicycle_first_order_0_sphere 103 | - goal: 104 | - 3.7662672796130945 105 | - 7.097661164210506 106 | - -2.603564789113001 107 | start: 108 | - 5.643721009629802 109 | - 4.418177920964124 110 | - 1.0810176504290707 111 | type: unicycle_first_order_0_sphere 112 | - goal: 113 | - 1.4989986209226385 114 | - 9.205569478679804 115 | - 2.272897977430392 116 | start: 117 | - 1.9235265754675708 118 | - 6.073980084366941 119 | - 1.2578459250284357 120 | type: unicycle_first_order_0_sphere 121 | - goal: 122 | - 0.8421195728536868 123 | - 1.9095887519828143 124 | - 1.4925537148180767 125 | start: 126 | - 3.0927229892687427 127 | - 7.757175988680437 128 | - -2.006347545717892 129 | type: unicycle_first_order_0_sphere 130 | - goal: 131 | - 5.405706053124501 132 | - 8.008653198867902 133 | - -2.6431375874069714 134 | start: 135 | - 2.822262417786101 136 | - 9.276003606511177 137 | - -0.5392986247201925 138 | type: unicycle_first_order_0_sphere 139 | - goal: 140 | - 9.200391574928489 141 | - 3.981206621813433 142 | - -0.6854854534282842 143 | start: 144 | - 7.956738392320644 145 | - 8.938935343519155 146 | - 1.2369928505439782 147 | type: unicycle_first_order_0_sphere 148 | - goal: 149 | - 6.4022593789212605 150 | - 7.549118877460961 151 | - -0.6799045378773498 152 | start: 153 | - 2.436648858416424 154 | - 2.3328153099900404 155 | - -0.6254745021946104 156 | type: unicycle_first_order_0_sphere 157 | - goal: 158 | - 2.389460896006393 159 | - 6.823948558342255 160 | - 0.9012960932654472 161 | start: 162 | - 6.357199018172442 163 | - 9.309804709843661 164 | - -1.117579684521174 165 | type: unicycle_first_order_0_sphere 166 | -------------------------------------------------------------------------------- /example/gen_p10_n8_6_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.0181870850397847 11 | - 4.549078938601746 12 | size: 13 | - 0.3588447855482 14 | - 0.23581593466325357 15 | type: box 16 | - center: 17 | - 0.5642427184110241 18 | - 1.884809100029417 19 | size: 20 | - 0.8327257085058737 21 | - 0.30754044129860303 22 | type: box 23 | - center: 24 | - 3.52967978772933 25 | - 1.865548504396061 26 | size: 27 | - 0.11979715877038655 28 | - 1.413140704193205 29 | type: box 30 | - center: 31 | - 3.985423565685542 32 | - 4.077190655809827 33 | size: 34 | - 1.4843937842648862 35 | - 0.341556929135524 36 | type: box 37 | robots: 38 | - goal: 39 | - 0.6638153528420436 40 | - 1.0142607697315666 41 | - 0.48942539917276395 42 | - 0.5959719079941104 43 | start: 44 | - 1.9574787869495882 45 | - 3.0615391378458074 46 | - -1.4349801076535489 47 | - -1.3598031537721313 48 | type: car_first_order_with_1_trailers_0 49 | - goal: 50 | - 3.351066130784097 51 | - 3.254300778014108 52 | - 0.6910172771221137 53 | - 1.0548523056549461 54 | start: 55 | - 2.5454995758110806 56 | - 4.483574569422646 57 | - -1.481000478534816 58 | - -1.164924928531593 59 | type: car_first_order_with_1_trailers_0 60 | - goal: 61 | - 1.5544599560796764 62 | - 3.3652925902751827 63 | - 0.0 64 | - 0.0 65 | start: 66 | - 1.3504728222758056 67 | - 1.0323763878514565 68 | - 0.0 69 | - 0.0 70 | type: double_integrator_0 71 | - goal: 72 | - 1.94129882083315 73 | - 2.5034473793217322 74 | - -2.0371314589745104 75 | - -2.004923509877527 76 | start: 77 | - 4.359445714125777 78 | - 2.0076855663118574 79 | - 2.2974106257230655 80 | - 2.696985657247626 81 | type: car_first_order_with_1_trailers_0 82 | - goal: 83 | - 4.367548248849347 84 | - 1.0824848738541615 85 | - 1.5809232305623029 86 | - 1.875504446808347 87 | start: 88 | - 0.5788637195692785 89 | - 1.3080916035365169 90 | - 0.4881582832499962 91 | - 0.7681302014124181 92 | type: car_first_order_with_1_trailers_0 93 | - goal: 94 | - 0.559511132409944 95 | - 3.386644978184425 96 | - 1.1733899359179016 97 | - 1.0261293366255155 98 | start: 99 | - 3.8863313382084304 100 | - 3.1292974206282484 101 | - -1.8266900346910986 102 | - -2.1071218374379717 103 | type: car_first_order_with_1_trailers_0 104 | - goal: 105 | - 2.710373947216639 106 | - 0.6495790474263035 107 | - 1.3153486013175115 108 | - 1.5200655160441543 109 | start: 110 | - 1.0378667516352822 111 | - 3.319667060659613 112 | - 1.0989648311260076 113 | - 1.1795887336495552 114 | type: car_first_order_with_1_trailers_0 115 | - goal: 116 | - 1.4641366789438623 117 | - 3.74576951228878 118 | - 0.0 119 | - 0.0 120 | start: 121 | - 4.031385885426671 122 | - 1.597943456781834 123 | - 0.0 124 | - 0.0 125 | type: double_integrator_0 126 | -------------------------------------------------------------------------------- /example/gen_p10_n8_7_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.5581863182932412 11 | - 3.4632204431682894 12 | size: 13 | - 0.959456021400919 14 | - 0.26184339602176604 15 | type: box 16 | - center: 17 | - 4.3145125526185515 18 | - 0.4702000293419579 19 | size: 20 | - 0.6384127426354259 21 | - 0.7315091321701551 22 | type: box 23 | - center: 24 | - 0.8495157778276125 25 | - 4.182762458561169 26 | size: 27 | - 1.12382899127526 28 | - 0.9080848619714939 29 | type: box 30 | - center: 31 | - 4.092362809547691 32 | - 4.145175620381892 33 | size: 34 | - 0.6044379541555394 35 | - 0.5636304870093437 36 | type: box 37 | robots: 38 | - goal: 39 | - 4.272777026903064 40 | - 3.0433678906588546 41 | - 0.6611830983179705 42 | start: 43 | - 2.4601836218461925 44 | - 4.111922965343404 45 | - -3.0822560808139112 46 | type: unicycle_first_order_0 47 | - goal: 48 | - 3.655904897911516 49 | - 0.7269052864828343 50 | - 0.0 51 | - 0.0 52 | start: 53 | - 0.9888067058856445 54 | - 1.3356879097807233 55 | - 0.0 56 | - 0.0 57 | type: double_integrator_0 58 | - goal: 59 | - 3.36770177624219 60 | - 0.8107439697060723 61 | - 1.2053991273381284 62 | start: 63 | - 2.2764947027573315 64 | - 1.2212775656067199 65 | - 2.852498634894938 66 | type: unicycle_first_order_0 67 | - goal: 68 | - 2.4070153558538276 69 | - 1.906649105183046 70 | - 0.9139481864529326 71 | start: 72 | - 3.4180132046590006 73 | - 0.8840625729568199 74 | - 2.5474386367076995 75 | type: unicycle_first_order_0 76 | - goal: 77 | - 1.2505513346564512 78 | - 1.9742456821402299 79 | - 0.0 80 | - 0.0 81 | start: 82 | - 3.346452975548462 83 | - 3.8447007654700625 84 | - 0.0 85 | - 0.0 86 | type: double_integrator_0 87 | - goal: 88 | - 2.250449399269758 89 | - 0.8690331342459707 90 | - 2.8474511576757804 91 | - 3.2403597313128683 92 | start: 93 | - 1.4947583336143508 94 | - 1.4056630567072124 95 | - 1.7708578467502383 96 | - 1.8324509260964594 97 | type: car_first_order_with_1_trailers_0 98 | - goal: 99 | - 2.6157056414197823 100 | - 2.5737318285399886 101 | - 0.5165453917198506 102 | start: 103 | - 1.9501222123587048 104 | - 4.468971308302153 105 | - -0.5432647772368759 106 | type: unicycle_first_order_0 107 | - goal: 108 | - 2.930470591791656 109 | - 2.2129411510789034 110 | - 2.4234640385888797 111 | - 2.1713588185874166 112 | start: 113 | - 3.978672535879057 114 | - 1.6300932951854668 115 | - 1.2661363877944423 116 | - 1.0320874613086244 117 | type: car_first_order_with_1_trailers_0 118 | -------------------------------------------------------------------------------- /example/gen_p10_n8_7_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 2.1488344802870754 11 | - 7.4023288004872265 12 | size: 13 | - 0.9290598941428896 14 | - 0.16373489087146542 15 | type: box 16 | - center: 17 | - 6.041993349000525 18 | - 7.898418793846759 19 | size: 20 | - 0.45234461993294267 21 | - 0.42218336517921423 22 | type: box 23 | - center: 24 | - 8.840890111374849 25 | - 7.224289607436423 26 | size: 27 | - 0.24810656093530914 28 | - 0.7153556121421284 29 | type: box 30 | - center: 31 | - 4.350825791608188 32 | - 8.644358849411068 33 | size: 34 | - 1.166994607849592 35 | - 1.5819785809594393 36 | type: box 37 | - center: 38 | - 4.673683130080272 39 | - 6.334975890477374 40 | size: 41 | - 0.7038314684883802 42 | - 1.2267815158710058 43 | type: box 44 | - center: 45 | - 9.09411873887336 46 | - 9.676457051808471 47 | size: 48 | - 1.683295913955901 49 | - 0.3975797075190076 50 | type: box 51 | - center: 52 | - 3.266194924094151 53 | - 5.531106106520941 54 | size: 55 | - 1.2193856788580597 56 | - 1.2128133312222196 57 | type: box 58 | - center: 59 | - 0.947329825593271 60 | - 2.7807275293979847 61 | size: 62 | - 1.029942925891691 63 | - 0.7656639592177773 64 | type: box 65 | - center: 66 | - 6.920133367793005 67 | - 2.7794229484437047 68 | size: 69 | - 0.49169078971194874 70 | - 0.2709331126346756 71 | type: box 72 | - center: 73 | - 3.7759091179953876 74 | - 2.3720826688559744 75 | size: 76 | - 1.4478843305880413 77 | - 1.5304096376131864 78 | type: box 79 | robots: 80 | - goal: 81 | - 5.208852854502035 82 | - 1.095529605404999 83 | - 1.5668884987151923 84 | start: 85 | - 9.115899013989779 86 | - 8.689274440887381 87 | - -3.1143713111532505 88 | type: unicycle_first_order_0_sphere 89 | - goal: 90 | - 9.41249078256347 91 | - 3.2277528595596525 92 | - 2.491440952939941 93 | start: 94 | - 9.484267102414858 95 | - 2.8027621545758175 96 | - -1.1055298080509774 97 | type: unicycle_first_order_0_sphere 98 | - goal: 99 | - 5.423005902353912 100 | - 4.316663936896073 101 | - 0.5881647599928006 102 | start: 103 | - 8.89291306928184 104 | - 5.063500922232192 105 | - -0.791277901545326 106 | type: unicycle_first_order_0_sphere 107 | - goal: 108 | - 3.452815040117936 109 | - 3.5933417694152086 110 | - 1.4386846205362485 111 | start: 112 | - 0.9164396596995306 113 | - 4.592717209634258 114 | - 0.5374072784878474 115 | type: unicycle_first_order_0_sphere 116 | - goal: 117 | - 8.198899804323183 118 | - 7.249387897963904 119 | - -2.9274890344385343 120 | start: 121 | - 7.531459609034929 122 | - 1.7094555323713876 123 | - -1.6148777937683862 124 | type: unicycle_first_order_0_sphere 125 | - goal: 126 | - 7.061239702493078 127 | - 8.936466565775234 128 | - -1.095251068196606 129 | start: 130 | - 5.739080484871019 131 | - 2.1013136774509196 132 | - -1.7297961829322888 133 | type: unicycle_first_order_0_sphere 134 | - goal: 135 | - 2.528676420653109 136 | - 2.9586524366902704 137 | - 2.782376066227731 138 | start: 139 | - 5.495171350451486 140 | - 2.917772828190589 141 | - -2.3720435261776522 142 | type: unicycle_first_order_0_sphere 143 | - goal: 144 | - 0.8661686509139439 145 | - 4.696076509350506 146 | - -1.2221775014732532 147 | start: 148 | - 2.4920322514880318 149 | - 8.566950254154875 150 | - 1.4259451617157382 151 | type: unicycle_first_order_0_sphere 152 | -------------------------------------------------------------------------------- /example/gen_p10_n8_8_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 0.8379129650654904 11 | - 1.4023288973451913 12 | size: 13 | - 1.3045480401929566 14 | - 1.3327068698670046 15 | type: box 16 | - center: 17 | - 4.027114212373061 18 | - 4.676374753042897 19 | size: 20 | - 0.6814927001597255 21 | - 0.5249266086290231 22 | type: box 23 | - center: 24 | - 3.2862125623477563 25 | - 3.065536893585406 26 | size: 27 | - 0.9888559441475824 28 | - 0.623559069466851 29 | type: box 30 | robots: 31 | - goal: 32 | - 2.163218414209021 33 | - 2.5375047131370216 34 | - 0.0 35 | - 0.0 36 | start: 37 | - 3.2351965752066225 38 | - 0.5779505298195065 39 | - 0.0 40 | - 0.0 41 | type: double_integrator_0 42 | - goal: 43 | - 0.945612133650771 44 | - 4.3588757112644 45 | - 0.4042167984003462 46 | - 0.3149526777970225 47 | start: 48 | - 3.1362945993300837 49 | - 4.009767006707326 50 | - -1.0584687493713654 51 | - -1.3842823229976322 52 | type: car_first_order_with_1_trailers_0 53 | - goal: 54 | - 4.106822994778279 55 | - 1.3265438898562159 56 | - 1.3562809481766251 57 | start: 58 | - 1.6241441859287176 59 | - 2.4021208582088973 60 | - 0.26299249140748593 61 | type: unicycle_first_order_0 62 | - goal: 63 | - 2.5828977842024736 64 | - 1.944992719978882 65 | - -3.0891432432339365 66 | start: 67 | - 0.5241320996178036 68 | - 2.5152921736650318 69 | - 0.9367074732836205 70 | type: unicycle_first_order_0 71 | - goal: 72 | - 2.854384517623551 73 | - 3.7305819447471436 74 | - -0.1657821850475214 75 | - 0.05977406579639 76 | start: 77 | - 2.080382430139591 78 | - 1.8597421097059894 79 | - -2.117833959762179 80 | - -2.147006037055935 81 | type: car_first_order_with_1_trailers_0 82 | - goal: 83 | - 3.351963709229255 84 | - 1.7662329615266068 85 | - 0.4920794034429159 86 | start: 87 | - 0.5836486595112089 88 | - 4.396755891313637 89 | - 0.7232996457802878 90 | type: unicycle_first_order_0 91 | - goal: 92 | - 1.53464467279505 93 | - 4.008973876976151 94 | - -1.8309662226071626 95 | - -1.607062651606936 96 | start: 97 | - 2.5698294164840823 98 | - 0.5152123484199032 99 | - 2.9648149814332374 100 | - 2.6119660585922726 101 | type: car_first_order_with_1_trailers_0 102 | - goal: 103 | - 1.2966314707339297 104 | - 3.1603409745640416 105 | - 1.9842881347491925 106 | start: 107 | - 2.3756071572976163 108 | - 3.7304350431783835 109 | - 0.0060082663810612225 110 | type: unicycle_first_order_0 111 | -------------------------------------------------------------------------------- /example/gen_p10_n8_9_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 5 4 | - 5 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 1.38662844357154 11 | - 3.1416987425962706 12 | size: 13 | - 1.1818383495036833 14 | - 0.9255039301795499 15 | type: box 16 | - center: 17 | - 1.2655678949976215 18 | - 1.5683006707410714 19 | size: 20 | - 0.35971577934831095 21 | - 1.0932912949456963 22 | type: box 23 | - center: 24 | - 2.3807488831713473 25 | - 1.0273675067388681 26 | size: 27 | - 0.25289345334578195 28 | - 0.928090303907481 29 | type: box 30 | - center: 31 | - 3.070227057830659 32 | - 1.0443221631501975 33 | size: 34 | - 0.19828857818932122 35 | - 0.4080074336408507 36 | type: box 37 | robots: 38 | - goal: 39 | - 3.0384273040941476 40 | - 2.4424420938373315 41 | - 2.581815902857035 42 | start: 43 | - 2.903403192490577 44 | - 3.0865982171827917 45 | - 0.6437688179947947 46 | type: unicycle_first_order_0 47 | - goal: 48 | - 0.5225416833229417 49 | - 1.0134915854821211 50 | - 0.19620274020703654 51 | start: 52 | - 1.562496089031208 53 | - 4.309200983344921 54 | - -2.1106544237274987 55 | type: unicycle_first_order_0 56 | - goal: 57 | - 3.9178224955439864 58 | - 3.6490623629187207 59 | - 1.9475628519247214 60 | - 2.0578924355431907 61 | start: 62 | - 2.193465177311689 63 | - 3.9771476014110143 64 | - -1.323976171707945 65 | - -1.2801816007710665 66 | type: car_first_order_with_1_trailers_0 67 | - goal: 68 | - 3.763481625823573 69 | - 1.7562018741974943 70 | - 3.02073927367213 71 | - 2.8629626543811475 72 | start: 73 | - 3.776724231975103 74 | - 2.723843710764065 75 | - 0.5091557993783615 76 | - 0.10934446593803915 77 | type: car_first_order_with_1_trailers_0 78 | - goal: 79 | - 3.690133242650304 80 | - 1.0649877255507323 81 | - 1.0610922048906852 82 | - 0.8475839707518147 83 | start: 84 | - 0.510044683762199 85 | - 0.9005306672216729 86 | - 1.2059891305595896 87 | - 1.3792567951228345 88 | type: car_first_order_with_1_trailers_0 89 | - goal: 90 | - 1.7843256675969212 91 | - 4.263150855859494 92 | - 1.78682361834698 93 | start: 94 | - 3.84110247330217 95 | - 1.5276230834480136 96 | - -0.5403360129342918 97 | type: unicycle_first_order_0 98 | - goal: 99 | - 4.379081058572378 100 | - 1.9791435040970495 101 | - 0.0 102 | - 0.0 103 | start: 104 | - 3.223554545016939 105 | - 4.287591218915413 106 | - 0.0 107 | - 0.0 108 | type: double_integrator_0 109 | - goal: 110 | - 4.179524964762998 111 | - 4.166191257075409 112 | - -1.8993039675753074 113 | start: 114 | - 4.350017561108035 115 | - 2.249110825459442 116 | - 2.179267262758894 117 | type: unicycle_first_order_0 118 | -------------------------------------------------------------------------------- /example/gen_p10_n8_9_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | max: 3 | - 10 4 | - 10 5 | min: 6 | - 0 7 | - 0 8 | obstacles: 9 | - center: 10 | - 5.601342287509137 11 | - 7.805005299900614 12 | size: 13 | - 0.5882881375275875 14 | - 0.7827293319632659 15 | type: box 16 | - center: 17 | - 4.527073737737527 18 | - 5.843995246867635 19 | size: 20 | - 1.2414236321614758 21 | - 1.3144298782877306 22 | type: box 23 | - center: 24 | - 6.04634444429279 25 | - 2.922274823690975 26 | size: 27 | - 0.8812032005306492 28 | - 0.3343215944674272 29 | type: box 30 | - center: 31 | - 7.253632691058551 32 | - 1.3248329801626166 33 | size: 34 | - 1.232240715718643 35 | - 1.1589680378924883 36 | type: box 37 | - center: 38 | - 1.706786035965735 39 | - 5.726342367026366 40 | size: 41 | - 0.667805979443137 42 | - 1.207241362933587 43 | type: box 44 | - center: 45 | - 2.4521465483792286 46 | - 2.181425430224344 47 | size: 48 | - 0.9488988800665437 49 | - 0.8570884296457156 50 | type: box 51 | - center: 52 | - 4.874593376616927 53 | - 2.794949582633522 54 | size: 55 | - 0.5124502997899905 56 | - 0.6599584939729708 57 | type: box 58 | - center: 59 | - 9.44486154526589 60 | - 8.035291594170806 61 | size: 62 | - 1.087429346601942 63 | - 1.3114571147995047 64 | type: box 65 | - center: 66 | - 8.556731710519507 67 | - 6.237824531986552 68 | size: 69 | - 0.6974216179568722 70 | - 0.49621635044788415 71 | type: box 72 | robots: 73 | - goal: 74 | - 7.528699038978422 75 | - 7.681920240089982 76 | - 1.1565359122652703 77 | start: 78 | - 6.173656176588277 79 | - 6.125766837125896 80 | - 0.1848128010082526 81 | type: unicycle_first_order_0_sphere 82 | - goal: 83 | - 1.8900074259200483 84 | - 9.336644601822666 85 | - 1.8532764767725167 86 | start: 87 | - 6.273198354120755 88 | - 8.479212023109465 89 | - 0.03077255971244952 90 | type: unicycle_first_order_0_sphere 91 | - goal: 92 | - 2.845863183030177 93 | - 6.958221469321365 94 | - -1.437533250135222 95 | start: 96 | - 7.580863423752818 97 | - 3.570794083412707 98 | - -0.4039891819775603 99 | type: unicycle_first_order_0_sphere 100 | - goal: 101 | - 7.952678363506572 102 | - 2.406544048539363 103 | - -1.4231920505304205 104 | start: 105 | - 7.387741218362297 106 | - 6.199222364647284 107 | - 2.962303978945762 108 | type: unicycle_first_order_0_sphere 109 | - goal: 110 | - 5.616264765010069 111 | - 9.1592126329594 112 | - -2.419318138838341 113 | start: 114 | - 1.770365184420412 115 | - 1.0803929468073363 116 | - 1.1989614448640111 117 | type: unicycle_first_order_0_sphere 118 | - goal: 119 | - 8.746119252968208 120 | - 5.305628151822579 121 | - 0.9308091299123733 122 | start: 123 | - 1.1248963467069721 124 | - 8.156609109014259 125 | - -0.04194089675282875 126 | type: unicycle_first_order_0_sphere 127 | - goal: 128 | - 1.8876190841488758 129 | - 3.19137932572519 130 | - -0.6117865021944242 131 | start: 132 | - 8.301303734417008 133 | - 8.967519352009544 134 | - -0.9565048199690467 135 | type: unicycle_first_order_0_sphere 136 | - goal: 137 | - 8.611926607807643 138 | - 1.873525921176031 139 | - 1.9776876513249153 140 | start: 141 | - 3.0445128334361327 142 | - 3.6611083804839573 143 | - -1.8642574785734354 144 | type: unicycle_first_order_0_sphere 145 | -------------------------------------------------------------------------------- /example/infeasible_0.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | 6 | # 2 robots with the same goal position 7 | robots: 8 | - type: unicycle_first_order_0_sphere 9 | start: [1,2.5,0] 10 | goal: [2.5,2.5,0] 11 | - type: unicycle_first_order_0_sphere 12 | start: [4,2.5,3.14] 13 | goal: [2.5,2.5,3.14] 14 | -------------------------------------------------------------------------------- /example/makespan_vs_soc_0.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_first_order_0_sphere 7 | start: [1,1,0] 8 | goal: [2,1,0] 9 | - type: unicycle_first_order_0_sphere 10 | start: [1,3,0] 11 | goal: [4,3,0] 12 | -------------------------------------------------------------------------------- /example/makespan_vs_soc_1.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_first_order_0_sphere 7 | start: [1,2.5,0] 8 | goal: [2,2.5,0] 9 | - type: unicycle_first_order_0_sphere 10 | start: [4,2.5,3.14] 11 | goal: [1,2.5,3.14] 12 | -------------------------------------------------------------------------------- /example/parallelpark.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [6, 6] 4 | obstacles: 5 | - type: box 6 | center: [3.1, 2.1] 7 | size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: unicycle_first_order_0 13 | start: [1,1,0] # x,y,theta 14 | goal: [4,2,0] # x,y,theta 15 | - type: unicycle_first_order_0 16 | start: [4,1,0] # x,y,theta 17 | goal: [2,1,0] # x,y,theta 18 | -------------------------------------------------------------------------------- /example/straight.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [6, 6] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_first_order_0 7 | start: [1,1,0] # x,y,theta 8 | goal: [2,1,0] # x,y,theta 9 | - type: unicycle_first_order_0 10 | start: [1,2,0] # x,y,theta 11 | goal: [4,2,0] # x,y,theta 12 | -------------------------------------------------------------------------------- /example/swap1_double_integrator.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: double_integrator_0 7 | start: [1,2.5,0,0] 8 | goal: [4,2.5,0,0] 9 | -------------------------------------------------------------------------------- /example/swap1_trailer.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: car_first_order_with_1_trailers_0 7 | start: [1,2.5,0,0] 8 | goal: [4,2.5,0,0] 9 | -------------------------------------------------------------------------------- /example/swap1_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_first_order_0 7 | start: [1,2.5,0] 8 | goal: [4,2.5,0] 9 | -------------------------------------------------------------------------------- /example/swap1_unicycle2.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_second_order_0 7 | start: [1,2.5,0,0,0] 8 | goal: [4,2.5,0,0,0] -------------------------------------------------------------------------------- /example/swap1_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_first_order_0_sphere 7 | start: [1,2.5,0] 8 | goal: [4,2.5,0] 9 | -------------------------------------------------------------------------------- /example/swap2_demo.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [-1, -1.5] 3 | max: [1.5, 1] 4 | obstacles: [] 5 | robots: 6 | - type: double_integrator_0 7 | start: [1,0,0,0] 8 | goal: [-0.5,0,0,0] 9 | - type: double_integrator_0 10 | start: [-0.5,0,0,0] 11 | goal: [1,0,0,0] 12 | -------------------------------------------------------------------------------- /example/swap2_double_integrator.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: double_integrator_0 7 | start: [1,2.5,0,0] 8 | goal: [4,2.5,0,0] 9 | - type: double_integrator_0 10 | start: [4,2.5,0,0] 11 | goal: [1,2.5,0,0] 12 | -------------------------------------------------------------------------------- /example/swap2_hetero.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: unicycle_first_order_0 13 | start: [1,2.5,0] 14 | goal: [4,2.5,0] 15 | - type: car_first_order_with_1_trailers_0 16 | start: [4,2.5,3.14,3.14] 17 | goal: [1,2.5,3.14,3.14] 18 | -------------------------------------------------------------------------------- /example/swap2_trailer.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: car_first_order_with_1_trailers_0 7 | start: [1,2.5,0,0] 8 | goal: [4,2.5,0,0] 9 | - type: car_first_order_with_1_trailers_0 10 | start: [4,2.5,3.14,3.14] 11 | goal: [1,2.5,3.14,3.14] 12 | -------------------------------------------------------------------------------- /example/swap2_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_first_order_0 7 | start: [1,2.5,0] 8 | goal: [4,2.5,0] 9 | - type: unicycle_first_order_0 10 | start: [4,2.5,3.14] 11 | goal: [1,2.5,3.14] 12 | -------------------------------------------------------------------------------- /example/swap2_unicycle2.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_second_order_0 7 | start: [1,2.5,0,0,0] 8 | goal: [4,2.5,0,0,0] 9 | - type: unicycle_second_order_0 10 | start: [4,2.5,3.14,0,0] 11 | goal: [1,2.5,3.14,0,0] -------------------------------------------------------------------------------- /example/swap2_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: unicycle_first_order_0_sphere 7 | start: [1,2.5,0] 8 | goal: [4,2.5,0] 9 | - type: unicycle_first_order_0_sphere 10 | start: [4,2.5,3.14] 11 | goal: [1,2.5,3.14] 12 | -------------------------------------------------------------------------------- /example/swap3_double_integrator.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: double_integrator_0 13 | start: [1,2.5,0,0] 14 | goal: [4,2.5,0,0] 15 | - type: double_integrator_0 16 | start: [4,2.5,0,0] 17 | goal: [1,2.5,0,0] 18 | - type: double_integrator_0 19 | start: [2.5,1,0,0] 20 | goal: [2.5,4,0,0] -------------------------------------------------------------------------------- /example/swap3_trailer.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | robots: 6 | - type: car_first_order_with_1_trailers_0 7 | start: [1,2.5,0,0] 8 | goal: [4,2.5,0,0] 9 | - type: car_first_order_with_1_trailers_0 10 | start: [4,2.5,3.14,3.14] 11 | goal: [1,2.5,3.14,3.14] 12 | - type: car_first_order_with_1_trailers_0 13 | start: [2.5,1,1.57,1.57] 14 | goal: [2.5,4,1.57,1.57] -------------------------------------------------------------------------------- /example/swap3_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: unicycle_first_order_0 13 | start: [1,2.5,0] 14 | goal: [4,2.5,0] 15 | - type: unicycle_first_order_0 16 | start: [4,2.5,3.14] 17 | goal: [1,2.5,3.14] 18 | - type: unicycle_first_order_0 19 | start: [2.5,1,1.57] 20 | goal: [2.5,4,1.57] -------------------------------------------------------------------------------- /example/swap3_unicycle2.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: unicycle_second_order_0 13 | start: [1,2.5,0,0,0] 14 | goal: [4,2.5,0,0,0] 15 | - type: unicycle_second_order_0 16 | start: [4,2.5,3.14,0,0] 17 | goal: [1,2.5,3.14,0,0] 18 | - type: unicycle_second_order_0 19 | start: [2.5,1,1.57,0,0] 20 | goal: [2.5,4,1.57,0,0] -------------------------------------------------------------------------------- /example/swap3_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: unicycle_first_order_0_sphere 13 | start: [1,2.5,0] 14 | goal: [4,2.5,0] 15 | - type: unicycle_first_order_0_sphere 16 | start: [4,2.5,3.14] 17 | goal: [1,2.5,3.14] 18 | - type: unicycle_first_order_0_sphere 19 | start: [2.5,1,1.57] 20 | goal: [2.5,4,1.57] -------------------------------------------------------------------------------- /example/swap4_demo.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [-1, -1.5] 3 | max: [1.5, 1] 4 | obstacles: [] 5 | robots: 6 | - type: double_integrator_0 7 | start: [1.0,0,0,0] 8 | goal: [-1.0,0,0,0] 9 | - type: double_integrator_0 10 | start: [0,-1.0,0,0] 11 | goal: [0,1.0,0,0] 12 | - type: double_integrator_0 13 | start: [-1.0,0,0,0] 14 | goal: [1.0,0,0,0] 15 | - type: double_integrator_0 16 | start: [0,1.0,0,0] 17 | goal: [0,-1.0,0,0] 18 | -------------------------------------------------------------------------------- /example/swap4_double_integrator.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: double_integrator_0 13 | start: [1,2.5,0,0] 14 | goal: [4,2.5,0,0] 15 | - type: double_integrator_0 16 | start: [4,2.5,0,0] 17 | goal: [1,2.5,0,0] 18 | - type: double_integrator_0 19 | start: [2.5,1,0,0] 20 | goal: [2.5,4,0,0] 21 | - type: double_integrator_0 22 | start: [2.5,4,0,0] 23 | goal: [2.5,1,0,0] 24 | -------------------------------------------------------------------------------- /example/swap4_trailer.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: car_first_order_with_1_trailers_0 13 | start: [1,2.5,0,0] 14 | goal: [4,2.5,0,0] 15 | - type: car_first_order_with_1_trailers_0 16 | start: [4,2.5,3.14,3.14] 17 | goal: [1,2.5,3.14,3.14] 18 | - type: car_first_order_with_1_trailers_0 19 | start: [2.5,1,1.57,1.57] 20 | goal: [2.5,4,1.57,1.57] 21 | - type: car_first_order_with_1_trailers_0 22 | start: [2.5,4,-1.57,-1.57] 23 | goal: [2.5,1,-1.57,-1.57] 24 | -------------------------------------------------------------------------------- /example/swap4_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: unicycle_first_order_0 13 | start: [1,2.5,0] 14 | goal: [4,2.5,0] 15 | - type: unicycle_first_order_0 16 | start: [4,2.5,3.14] 17 | goal: [1,2.5,3.14] 18 | - type: unicycle_first_order_0 19 | start: [2.5,1,1.57] 20 | goal: [2.5,4,1.57] 21 | - type: unicycle_first_order_0 22 | start: [2.5,4,-1.57] 23 | goal: [2.5,1,-1.57] 24 | -------------------------------------------------------------------------------- /example/swap4_unicycle2.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: unicycle_second_order_0 13 | start: [1,2.5,0,0,0] 14 | goal: [4,2.5,0,0,0] 15 | - type: unicycle_second_order_0 16 | start: [4,2.5,3.14,0,0] 17 | goal: [1,2.5,3.14,0,0] 18 | - type: unicycle_second_order_0 19 | start: [2.5,1,1.57,0,0] 20 | goal: [2.5,4,1.57,0,0] 21 | - type: unicycle_second_order_0 22 | start: [2.5,4,-1.57,0,0] 23 | goal: [2.5,1,-1.57,0,0] 24 | -------------------------------------------------------------------------------- /example/swap4_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0, 0] 3 | max: [5, 5] 4 | obstacles: [] 5 | # - type: box 6 | # center: [3.1, 1.1] 7 | # size: [0.5, 0.25] 8 | # - type: box 9 | # center: [1.5, 1.3] 10 | # size: [0.5, 0.25] 11 | robots: 12 | - type: unicycle_first_order_0_sphere 13 | start: [1,2.5,0] 14 | goal: [4,2.5,0] 15 | - type: unicycle_first_order_0_sphere 16 | start: [4,2.5,3.14] 17 | goal: [1,2.5,3.14] 18 | - type: unicycle_first_order_0_sphere 19 | start: [2.5,1,1.57] 20 | goal: [2.5,4,1.57] 21 | - type: unicycle_first_order_0_sphere 22 | start: [2.5,4,-1.57] 23 | goal: [2.5,1,-1.57] 24 | -------------------------------------------------------------------------------- /example/wall.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [6, 6] 4 | obstacles: 5 | - type: box 6 | center: [3., 5.20] 7 | size: [0.2, 1.8] 8 | - type: box 9 | center: [3., 3.0] 10 | size: [0.2, 1.8] 11 | - type: box 12 | center: [3., 0.80] 13 | size: [0.2, 1.8] 14 | robots: 15 | - type: single_integrator 16 | start: [1.8,2] 17 | goal: [5.2,3] 18 | - type: single_integrator 19 | start: [5.0,2.0] # x,y 20 | goal: [1.3,3.0] # x,y 21 | -------------------------------------------------------------------------------- /example/window2_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [5, 5] 4 | obstacles: 5 | - type: box 6 | center: [2.5, 3.9] 7 | size: [0.2, 2.2] 8 | - type: box 9 | center: [2.5, 1.1] 10 | size: [0.2, 2.2] 11 | robots: 12 | - type: unicycle_first_order_0 13 | start: [1,1,0] 14 | goal: [4,3,0] 15 | - type: unicycle_first_order_0 16 | start: [4,3,0] 17 | goal: [1,1,0] -------------------------------------------------------------------------------- /example/window3_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [5, 5] 4 | obstacles: 5 | - type: box 6 | center: [2.5, 3.9] 7 | size: [0.2, 2.2] 8 | - type: box 9 | center: [2.5, 1.1] 10 | size: [0.2, 2.2] 11 | robots: 12 | - type: unicycle_first_order_0 13 | start: [1,1,0] 14 | goal: [4,3,0] 15 | - type: unicycle_first_order_0 16 | start: [1,3,0] 17 | goal: [4,2,0] 18 | - type: unicycle_first_order_0 19 | start: [4,4,0] 20 | goal: [1,4,0] 21 | -------------------------------------------------------------------------------- /example/window4_demo.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [-1, -1.5] 3 | max: [1.5, 1] 4 | obstacles: 5 | - type: box 6 | center: [-0.5,0] 7 | size: [1,0.1] 8 | - type: box 9 | center: [1,0] 10 | size: [1,0.1] 11 | robots: 12 | - type: double_integrator_0 13 | start: [-0.8,0.8,0,0] 14 | goal: [1,-0.5,0,0] 15 | 16 | - type: double_integrator_0 17 | start: [0.5,-0.5,0,0] 18 | goal: [-0.5,0.8,0,0] 19 | 20 | - type: double_integrator_0 21 | start: [0.5,-1,0,0] 22 | goal: [0.5,0.5,0,0] 23 | 24 | - type: double_integrator_0 25 | start: [0.8,0.5,0,0] 26 | goal: [-0.8,-1.2,0,0] 27 | -------------------------------------------------------------------------------- /example/window4_double_integrator.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [5, 5] 4 | obstacles: 5 | - type: box 6 | center: [2.5, 3.9] 7 | size: [0.2, 2.2] 8 | - type: box 9 | center: [2.5, 1.1] 10 | size: [0.2, 2] 11 | robots: 12 | - type: double_integrator_0 13 | start: [1,1,0,0] 14 | goal: [4,3,0,0] 15 | - type: double_integrator_0 16 | start: [1,3,0,0] 17 | goal: [4,2,0,0] 18 | - type: double_integrator_0 19 | start: [4,4,0,0] 20 | goal: [1,4,0,0] 21 | - type: double_integrator_0 22 | start: [3,2.5,0,0] 23 | goal: [1,2.5,0,0] 24 | -------------------------------------------------------------------------------- /example/window4_trailer.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [5, 5] 4 | obstacles: 5 | - type: box 6 | center: [2.5, 3.9] 7 | size: [0.2, 2.2] 8 | - type: box 9 | center: [2.5, 1.1] 10 | size: [0.2, 2] 11 | robots: 12 | - type: car_first_order_with_1_trailers_0 13 | start: [1,1,0,0] 14 | goal: [4,3,0,0] 15 | - type: car_first_order_with_1_trailers_0 16 | start: [1,3,0,0] 17 | goal: [4,2,0,0] 18 | - type: car_first_order_with_1_trailers_0 19 | start: [4,4,3.14,3.14] 20 | goal: [1,4,3.14,3.14] 21 | - type: car_first_order_with_1_trailers_0 22 | start: [3,2.5,3.14,3.14] 23 | goal: [1,2.5,3.14,3.14] 24 | -------------------------------------------------------------------------------- /example/window4_unicycle.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [5, 5] 4 | obstacles: 5 | - type: box 6 | center: [2.5, 3.9] 7 | size: [0.2, 2.2] 8 | - type: box 9 | center: [2.5, 1.1] 10 | size: [0.2, 2] 11 | robots: 12 | - type: unicycle_first_order_0 13 | start: [1,1,0] 14 | goal: [4,3,0] 15 | - type: unicycle_first_order_0 16 | start: [1,3,0] 17 | goal: [4,2,0] 18 | - type: unicycle_first_order_0 19 | start: [4,4,3.14] 20 | goal: [1,4,3.14] 21 | - type: unicycle_first_order_0 22 | start: [3,2.5,3.14] 23 | goal: [1,2.5,3.14] 24 | -------------------------------------------------------------------------------- /example/window4_unicycle2.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [5, 5] 4 | obstacles: 5 | - type: box 6 | center: [2.5, 3.9] 7 | size: [0.2, 2] 8 | - type: box 9 | center: [2.5, 1.1] 10 | size: [0.2, 2] 11 | robots: 12 | - type: unicycle_second_order_0 13 | start: [1,1,0,0,0] 14 | goal: [4,3,0,0,0] 15 | - type: unicycle_second_order_0 16 | start: [1,3,0,0,0] 17 | goal: [4,2,0,0,0] 18 | - type: unicycle_second_order_0 19 | start: [4,4,3.14,0,0] 20 | goal: [1,4,3.14,0,0] 21 | - type: unicycle_second_order_0 22 | start: [3,2.5,3.14,0,0] 23 | goal: [1,2.5,3.14,0,0] 24 | -------------------------------------------------------------------------------- /example/window4_unicycle_sphere.yaml: -------------------------------------------------------------------------------- 1 | environment: 2 | min: [0.0, 0.0] 3 | max: [5, 5] 4 | obstacles: 5 | - type: box 6 | center: [2.5, 4.1] 7 | size: [0.2, 2.0] 8 | - type: box 9 | center: [2.5, 1.1] 10 | size: [0.2, 2.0] 11 | robots: 12 | - type: unicycle_first_order_0_sphere 13 | start: [1,2,0] 14 | goal: [4,3,0] 15 | - type: unicycle_first_order_0_sphere 16 | start: [1,3,0] 17 | goal: [4,2,0] 18 | - type: unicycle_first_order_0_sphere 19 | start: [4,4,3.14] 20 | goal: [1,4,3.14] 21 | - type: unicycle_first_order_0_sphere 22 | start: [3,2.5,3.14] 23 | goal: [1,2.5,3.14] 24 | -------------------------------------------------------------------------------- /k_cbs/.gitignore: -------------------------------------------------------------------------------- 1 | ompl/ -------------------------------------------------------------------------------- /scripts/benchmark_stats.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | from pathlib import Path 3 | import plot_stats 4 | import argparse 5 | 6 | 7 | def run_benchmark_stats(instances, algs, trials, T): 8 | results_path = Path("../results") 9 | 10 | report = plot_stats.Report(results_path / "stats.pdf", trials, T, dt=0.1) 11 | 12 | for instance in instances: 13 | for alg in algs: 14 | result_folder = results_path / instance / alg 15 | stat_files = [str(p) for p in result_folder.glob("**/stats.yaml")] 16 | if len(stat_files) > 0: 17 | report.load_stat_files(instance, alg, stat_files) 18 | 19 | report.add_barplot_initial_cost_plot(instances) 20 | for instance in instances: 21 | report.add_success_and_cost_over_time_plot(instance) 22 | # report.add_time_cost_plot(instance) 23 | # report.add_success_over_time_plot(instance) 24 | # report.add_initial_time_cost_plot(instance) 25 | # # report.add_success_rate_plot(instance) 26 | # report.add_boxplot_initial_time_plot(instance) 27 | # report.add_boxplot_initial_cost_plot([instance]) 28 | 29 | report.close() 30 | 31 | def main(): 32 | parser = argparse.ArgumentParser() 33 | parser.add_argument('instances', help="instances") 34 | parser.add_argument('algs', help="algorithms") 35 | args = parser.parse_args() 36 | 37 | trials = 1 38 | timelimit = 5*60 39 | 40 | run_benchmark_stats(args.instances, args.algs, trials, timelimit) 41 | 42 | if __name__ == '__main__': 43 | main() 44 | -------------------------------------------------------------------------------- /scripts/checker.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import yaml 3 | import numpy as np 4 | import sys 5 | import os 6 | sys.path.append(os.getcwd()) 7 | import robots 8 | from motionplanningutils import CollisionChecker 9 | 10 | def check(filename_env: str, filename_result: str, file = None, expected_T=None) -> bool: 11 | 12 | def check_array(a, b, msg): 13 | success = np.allclose(a, b, rtol=0.01, atol=1e-2) 14 | if not success: 15 | print("{} Is: {} Should: {} Delta: {}".format(msg, a, b, a-b), file=file) 16 | return success 17 | 18 | 19 | with open(filename_env) as f: 20 | env = yaml.safe_load(f) 21 | 22 | with open(filename_result) as f: 23 | result = yaml.safe_load(f) 24 | 25 | success = True 26 | cost = 0. 27 | dt = 0.1 28 | for i in range(len(env["robots"])): 29 | robot_node = env["robots"][i] 30 | robot = robots.create_robot(robot_node["type"]) 31 | 32 | x0 = np.array(robot_node["start"]) 33 | xf = np.array(robot_node["goal"]) 34 | cc = CollisionChecker() 35 | cc.load(filename_env) 36 | 37 | states = np.array(result["result"][i]["states"]) 38 | actions = np.array(result["result"][i]["actions"]) 39 | 40 | if states.shape[1] != len(robot.state_desc): 41 | print("Wrong state dimension!", file=file) 42 | success = False 43 | if actions.shape[1] != len(robot.action_desc): 44 | print("Wrong action dimension!", file=file) 45 | success = False 46 | if states.shape[0] != actions.shape[0] + 1: 47 | print("number of actions not number of states - 1!", file=file) 48 | success = False 49 | 50 | success &= check_array(states[0], x0, "start state") 51 | success &= check_array(states[-1], xf, "end state") 52 | # dynamics 53 | T = states.shape[0] 54 | for t in range(T-1): 55 | state_desired = robot.step(states[t], actions[t]) 56 | success &= check_array(states[t+1], state_desired, "Wrong dynamics at t={}".format(t)) 57 | # state limits 58 | for t in range(T): 59 | if not robot.valid_state(states[t]): 60 | print("State invalid at t={} ({})".format(t, states[t]), file=file) 61 | success = False 62 | # action limits 63 | for t in range(T-1): 64 | if (actions[t] > robot.max_u + 1e-2).any() or (actions[t] < robot.min_u - 1e-2).any(): 65 | print("Action outside bounds at t={} ({})".format(t, actions[t]), file=file) 66 | success = False 67 | # collisions 68 | for t in range(T): 69 | dist, _, _ = cc.distance(states[t]) 70 | if dist < -0.03: # allow up to 3cm violation 71 | print("Collision at t={} ({})".format(t, dist), file=file) 72 | success = False 73 | 74 | if expected_T is not None: 75 | if T-1 not in expected_T: 76 | print("Expected T to be in {}, but is {}".format(expected_T, T-1), file=file) 77 | success = False 78 | # cost 79 | cost += len(actions) * dt 80 | if success: 81 | print("Cost={}".format(cost), file=file) 82 | return success 83 | 84 | def main() -> None: 85 | parser = argparse.ArgumentParser() 86 | parser.add_argument("env", help="file containing the environment (YAML)") 87 | parser.add_argument("result", help="file containing the result (YAML)") 88 | args = parser.parse_args() 89 | 90 | print(check(args.env, args.result)) 91 | 92 | 93 | if __name__ == "__main__": 94 | main() -------------------------------------------------------------------------------- /scripts/example.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import argparse 3 | import shutil 4 | from pathlib import Path 5 | import subprocess 6 | from main_ompl import run_ompl 7 | 8 | def run_visualize(script, filename_env, filename_result): 9 | subprocess.run(["python3", 10 | script, 11 | filename_env, 12 | "--result", filename_result, 13 | "--video", filename_result.with_suffix(".mp4")]) 14 | 15 | def run_example(env,result_folder,timelimit,cfg): 16 | result_folder = Path(result_folder) 17 | if result_folder.exists(): 18 | print("Warning! {} exists already. Deleting...".format(result_folder)) 19 | shutil.rmtree(result_folder) 20 | result_folder.mkdir(parents=True, exist_ok=False) 21 | with open(cfg) as f: 22 | cfg = yaml.safe_load(f) 23 | mycfg = cfg['sst'] 24 | mycfg = mycfg['default'] 25 | run_ompl(env, str(result_folder), timelimit, mycfg) 26 | 27 | visualize_files = [p.name for p in result_folder.glob('result_*')] 28 | vis_script = Path(env).parent / "visualize.py" 29 | for file in visualize_files: 30 | run_visualize(vis_script, env, result_folder / file) 31 | 32 | def main(): 33 | parser = argparse.ArgumentParser() 34 | parser.add_argument('env', help="path to env.yaml file") 35 | parser.add_argument('result_folder', help="output folder") 36 | parser.add_argument('timelimit', default = 5 * 60, help="timelimit") 37 | parser.add_argument('cfg', help="path to algorithm cfgs") 38 | # parser.add_argument('robot_number') 39 | 40 | args = parser.parse_args() 41 | run_example(args.env, args.result_folder,args.timelimit,args.cfg) 42 | 43 | 44 | if __name__ == "__main__": 45 | main() -------------------------------------------------------------------------------- /scripts/gen_motion_primitive.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scp import SCP 3 | import robots 4 | import yaml 5 | import multiprocessing as mp 6 | import tqdm 7 | import itertools 8 | import argparse 9 | 10 | import sys, os 11 | sys.path.append(os.getcwd()) 12 | from motionplanningutils import RobotHelper 13 | 14 | 15 | # two point boundary value problem 16 | def TPBVP_fixed_time(robot, x0, xf, T): 17 | scp = SCP(robot) 18 | 19 | # initialize with random rollout 20 | states = np.empty((T,len(robot.state_desc))) 21 | states[0] = x0 22 | actions = np.empty((T-1,len(robot.action_desc))) 23 | for k in range(T-1): 24 | actions[k] = np.random.uniform(robot.min_u, robot.max_u) 25 | states[k+1] = robot.step(states[k], actions[k]) 26 | 27 | # states = np.tile(x0, (T, 1)) 28 | # actions = np.zeros((T-1, 2)) 29 | # states[1:] += np.random.normal(0, 0.001, states.shape)[1:] 30 | # actions += np.random.normal(0, 0.001, actions.shape) 31 | X, U, val = scp.min_xf(states, actions, x0, xf, 10, trust_x=0.1, trust_u=0.1) 32 | if len(X) > 1: 33 | return X[-1], U[-1], val 34 | 35 | return None, None, None 36 | 37 | def gen_random_motion(robot_type): 38 | robot = robots.create_robot(robot_type) 39 | rh = RobotHelper(robot_type) 40 | while True: 41 | x0 = np.array(rh.sampleUniform()) 42 | x0[0:2] = 0 # set position part to 0 43 | xf = np.array(rh.sampleUniform()) 44 | T = np.random.choice([8, 16, 32]) 45 | print(T) 46 | 47 | X, U, _ = TPBVP_fixed_time(robot, x0, xf, T) 48 | print(X) 49 | if X is not None: 50 | r = dict() 51 | r['x0'] = x0.tolist() 52 | r['xf'] = X[-1].tolist() 53 | r['states'] = X.tolist() 54 | r['actions'] = U.tolist() 55 | r['T'] = int(T) 56 | return r 57 | 58 | 59 | def gen_motion(robot, x0, xf): 60 | print("Try ", xf) 61 | for T in range(2, 32): 62 | # for T in [32]: 63 | X, U, val = TPBVP_fixed_time(robot, x0, xf, T) 64 | if X is not None and val < 1e6: 65 | r = dict() 66 | r['x0'] = x0.tolist() 67 | r['xf'] = X[-1].tolist() 68 | r['states'] = X.tolist() 69 | r['actions'] = U.tolist() 70 | r['T'] = int(T) 71 | return r 72 | 73 | def main(): 74 | parser = argparse.ArgumentParser() 75 | parser.add_argument("robot_type", help="name of robot type to generate motions for") 76 | parser.add_argument("output", help="output file (YAML)") 77 | parser.add_argument("--N", help="number of motions", default=100, type=int) 78 | args = parser.parse_args() 79 | 80 | rh = RobotHelper(args.robot_type) 81 | 82 | motions = [] 83 | # tasks = list(itertools.repeat(robot, N)) 84 | # print(tasks) 85 | # for k in range(N): 86 | # print(k) 87 | # motion = gen_random_motion(robot) 88 | # motions.append(motion) 89 | 90 | mp.set_start_method('spawn') 91 | with mp.Pool() as p: 92 | for motion in tqdm.tqdm(p.imap_unordered(gen_random_motion, itertools.repeat(args.robot_type, args.N))): 93 | motion['distance'] = rh.distance(motion['x0'], motion['xf']) 94 | motion['name'] = 'm{}'.format(len(motions)) 95 | motions.append(motion) 96 | 97 | # for x in [-0.25, 0, 0.25]: 98 | # for y in [-0.25, 0, 0.25]: 99 | # for yaw in np.linspace(-np.pi, np.pi, 8): 100 | # motion = gen_motion(robot, 101 | # np.array([0, 0, 0], dtype=np.float32), 102 | # np.array([x, y, yaw], dtype=np.float32)) 103 | # if motion is not None: 104 | # print(x,y, yaw) 105 | # motions.append(motion) 106 | 107 | with open(args.output, 'w') as file: 108 | yaml.dump(motions, file) 109 | 110 | 111 | if __name__ == '__main__': 112 | main() -------------------------------------------------------------------------------- /scripts/main_dbcbs.py: -------------------------------------------------------------------------------- 1 | import shutil 2 | import subprocess 3 | import time 4 | import shutil 5 | import tempfile 6 | from pathlib import Path 7 | import sys 8 | import os 9 | import yaml 10 | # import msgpack 11 | 12 | 13 | sys.path.append(os.getcwd()) 14 | 15 | 16 | def run_dbcbs(filename_env, folder, timelimit, cfg): 17 | with tempfile.TemporaryDirectory() as tmpdirname: 18 | p = Path(tmpdirname) 19 | filename_cfg = p / "cfg.yaml" 20 | with open(filename_cfg, 'w') as f: 21 | yaml.dump(cfg, f, Dumper=yaml.CSafeDumper) 22 | 23 | print(filename_env) 24 | filename_stats = "{}/stats.yaml".format(folder) 25 | start = time.time() 26 | duration_dbcbs = 0 27 | with open(filename_stats, 'w') as stats: 28 | stats.write("stats:\n") 29 | 30 | filename_result_dbcbs = Path(folder) / "result_dbcbs.yaml" 31 | filename_result_dbcbs_joint = Path(folder) / "dbcbs_joint.yaml" 32 | filename_result_dbcbs_opt = Path(folder) / "result_dbcbs_opt.yaml" 33 | t_dbcbs_start = time.time() 34 | 35 | cmd = ["./db_cbs", 36 | "-i", filename_env, 37 | "-o", filename_result_dbcbs, 38 | "--joint", filename_result_dbcbs_joint, 39 | "--opt", filename_result_dbcbs_opt, 40 | "-c", str(filename_cfg)] 41 | print(subprocess.list2cmdline(cmd)) 42 | try: 43 | with open("{}/log.txt".format(folder), 'w') as logfile: 44 | result = subprocess.run(cmd, timeout=timelimit, stdout=logfile, stderr=logfile) 45 | t_dbcbs_stop = time.time() 46 | duration_dbcbs += t_dbcbs_stop - t_dbcbs_start 47 | if result.returncode != 0: 48 | print("db-cbs failed ", result.returncode) 49 | else: 50 | # shutil.copyfile(filename_result_dbcbs_opt, "{}/result_dbcbs_opt.yaml".format(folder)) 51 | cost = 0 52 | with open(filename_result_dbcbs_opt) as f: 53 | result = yaml.safe_load(f) 54 | for r in result["result"]: 55 | cost += len(r["actions"]) * 0.1 56 | 57 | # cost = result["cost"] # cost*2 58 | now = time.time() 59 | t = now - start 60 | print("success!", cost, t) 61 | stats.write(" - t: {}\n".format(t)) 62 | stats.write(" cost: {}\n".format(cost)) 63 | stats.write(" duration_dbcbs: {}\n".format(duration_dbcbs)) 64 | stats.flush() 65 | except: 66 | print("Failure!") 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /scripts/main_kcbs.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import subprocess 3 | import tempfile 4 | from pathlib import Path 5 | import yaml 6 | 7 | def run_kcbs(filename_env, folder, timelimit, cfg): 8 | 9 | with tempfile.TemporaryDirectory() as tmpdirname: 10 | p = Path(tmpdirname) 11 | filename_cfg = p / "cfg.yaml" 12 | with open(filename_cfg, 'w') as f: 13 | yaml.dump(cfg, f, Dumper=yaml.CSafeDumper) 14 | with open("{}/log.txt".format(folder), 'w') as logfile: 15 | result = subprocess.run(["./main_kcbs", 16 | "-i", filename_env, 17 | "-o", "{}/result_kcbs.yaml".format(folder), 18 | "--stats", "{}/stats.yaml".format(folder), 19 | "--timelimit", str(timelimit), 20 | "-p", "k-cbs", 21 | "-c", str(filename_cfg)], 22 | stdout=logfile, stderr=logfile) 23 | if result.returncode != 0: 24 | print("KCBS failed") 25 | 26 | def main(): 27 | parser = argparse.ArgumentParser() 28 | parser.add_argument("env", help="file containing the environment (YAML)") 29 | args = parser.parse_args() 30 | 31 | for i in range(1): 32 | run_kcbs(args.env, i) 33 | 34 | 35 | if __name__ == '__main__': 36 | main() 37 | -------------------------------------------------------------------------------- /scripts/main_ompl.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import subprocess 3 | # import main_scp 4 | # import main_komo 5 | import tempfile 6 | from pathlib import Path 7 | import yaml 8 | 9 | def run_ompl(filename_env, folder, timelimit, cfg): 10 | 11 | with tempfile.TemporaryDirectory() as tmpdirname: 12 | p = Path(tmpdirname) 13 | filename_cfg = p / "cfg.yaml" 14 | with open(filename_cfg, 'w') as f: 15 | yaml.dump(cfg, f, Dumper=yaml.CSafeDumper) 16 | 17 | with open("{}/log.txt".format(folder), 'w') as logfile: 18 | result = subprocess.run(["./main_ompl", 19 | "-i", filename_env, 20 | "-o", "{}/result_ompl.yaml".format(folder), 21 | "--stats", "{}/stats.yaml".format(folder), 22 | "--timelimit", str(timelimit), 23 | "-p", "sst", 24 | "-c", str(filename_cfg)], 25 | stdout=logfile, stderr=logfile) 26 | if result.returncode != 0: 27 | print("OMPL failed") 28 | 29 | def main(): 30 | parser = argparse.ArgumentParser() 31 | parser.add_argument("env", help="file containing the environment (YAML)") 32 | args = parser.parse_args() 33 | 34 | for i in range(1): 35 | run_ompl(args.env, i) 36 | 37 | 38 | if __name__ == '__main__': 39 | main() 40 | -------------------------------------------------------------------------------- /scripts/main_s2m2.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import subprocess 3 | from pathlib import Path 4 | 5 | 6 | def run_s2m2(filename_env, folder, timelimit, cfg): 7 | s2sm_script = Path().resolve().parent / "s2m2/main_s2m2_original.py" 8 | with open("{}/log.txt".format(folder), 'w') as logfile: 9 | result = subprocess.run(["python3", 10 | s2sm_script, 11 | filename_env, 12 | folder, 13 | str(timelimit), 14 | str(cfg), 15 | ], 16 | stdout=logfile, stderr=logfile) 17 | if result.returncode != 0: 18 | print("S2SM failed") 19 | 20 | def main(): 21 | parser = argparse.ArgumentParser() 22 | parser.add_argument("env", help="file containing the environment (YAML)") 23 | args = parser.parse_args() 24 | 25 | for i in range(1): 26 | run_s2m2(args.env, i) 27 | 28 | 29 | if __name__ == '__main__': 30 | main() 31 | -------------------------------------------------------------------------------- /scripts/utils_optimization.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import numpy as np 3 | 4 | import sys, os 5 | sys.path.append(os.getcwd()) 6 | from motionplanningutils import RobotHelper 7 | 8 | 9 | class UtilsSolutionFile: 10 | def __init__(self, robot_type: str) -> None: 11 | self.rh = RobotHelper(robot_type) 12 | 13 | def load(self, filename: str) -> None: 14 | with open(filename) as f: 15 | self.file = yaml.safe_load(f) 16 | self.states = np.array(self.file['result'][0]['states']) 17 | if 'actions' in self.file['result'][0]: 18 | self.actions = np.array(self.file['result'][0]['actions']) 19 | 20 | def T(self) -> int: 21 | return self.states.shape[0] - 1 22 | 23 | def save_rescaled(self, filename:str, T: int) -> None: 24 | T_orig = self.T() 25 | state_dim = self.states.shape[1] 26 | states_interp = np.zeros((T+1, state_dim)) 27 | # t_rescaled = np.linspace(0,1,T+1) 28 | # t_orig = np.linspace(0, 1, T_orig+1) 29 | for k in range(T+1): 30 | t_rescaled = k / T # in range [0,1] 31 | idx_orig = int(np.floor(t_rescaled * T_orig)) 32 | idx_orig_next = int(np.ceil(t_rescaled * T_orig)) 33 | t_orig = idx_orig / T_orig 34 | t_orig_next = idx_orig_next / T_orig 35 | if t_orig_next > t_orig: 36 | rel_t = (t_rescaled - t_orig) / (t_orig_next - t_orig) 37 | else: 38 | rel_t = 0 39 | # print(idx_orig, idx_orig_next, t_orig, t_orig_next, t_rescaled, rel_t) 40 | states_interp[k] = self.rh.interpolate(self.states[idx_orig], self.states[idx_orig_next], rel_t) 41 | # print(self.states[orig_idx], self.states[orig_idx_next], states_interp[t]) 42 | 43 | # exit() 44 | 45 | # for k in range(state_dim): 46 | # states_interp[:,k] = np.interp(np.linspace(0,1,T+1), np.linspace(0, 1, T_orig+1), self.states[:,k]) 47 | 48 | if 'actions' in self.file['result'][0]: 49 | action_dim = self.actions.shape[1] 50 | actions_interp = np.empty((T, action_dim)) 51 | for k in range(self.actions.shape[1]): 52 | actions_interp[:,k] = np.interp(np.linspace(0,1,T), np.linspace(0, 1, T_orig), self.actions[:,k]) 53 | 54 | with open(filename, 'w') as f: 55 | self.file['result'][0]['states'] = states_interp.tolist() 56 | if 'actions' in self.file['result'][0]: 57 | self.file['result'][0]['actions'] = actions_interp.tolist() 58 | yaml.dump(self.file, f) 59 | 60 | 61 | def main(): 62 | rh = RobotHelper("unicycle_first_order_0") 63 | a = [0,1,2] #rh.sampleUniform() 64 | b = [3,4,5] #rh.sampleUniform() 65 | c = rh.interpolate(a, b, 0.5) 66 | print(a,b,c) 67 | 68 | if __name__ == '__main__': 69 | main() -------------------------------------------------------------------------------- /src/fclHelper.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | template 6 | void shiftAABB(fcl::AABB& aabb, const fcl::Vector3& offset) 7 | { 8 | aabb.min_ += offset; 9 | aabb.max_ += offset; 10 | } 11 | 12 | template 13 | class ShiftableDynamicAABBTreeCollisionManager : public fcl::DynamicAABBTreeCollisionManager 14 | { 15 | public: 16 | void shift(const fcl::Vector3 &offset) 17 | { 18 | // Use const-cast, since DynamicAABBTreeCollisionManager has tree as a private member 19 | const auto& tree = this->getTree(); 20 | // std::cout << "b: " << tree.getRoot()->bv.min_ << std::endl; 21 | shift_recursive(const_cast> *>(tree.getRoot()), offset); 22 | // std::cout << "a: " << tree.getRoot()->bv.min_ << std::endl; 23 | // std::cout << "s: " << this->size() << std::endl; 24 | } 25 | 26 | protected: 27 | void shift_recursive(fcl::detail::NodeBase> *node, const fcl::Vector3 &offset) 28 | { 29 | if (node == nullptr) { 30 | return; 31 | } 32 | shiftAABB(node->bv, offset); 33 | if (node->isLeaf()) { 34 | fcl::CollisionObject *obj = static_cast *>(node->data); 35 | if (!obj->isFree()) { 36 | obj->setTranslation(obj->getTranslation() + offset); 37 | obj->computeAABB(); 38 | // assert(node->bv.equal(obj->getAABB())); 39 | // std::cout << "ot" << std::endl; 40 | } 41 | } else { 42 | shift_recursive(node->children[0], offset); 43 | shift_recursive(node->children[1], offset); 44 | } 45 | } 46 | }; -------------------------------------------------------------------------------- /src/fclStateValidityChecker.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // #include "environment.h" 4 | #include "robots.h" 5 | 6 | #include 7 | 8 | class fclStateValidityChecker 9 | : public ompl::base::StateValidityChecker 10 | { 11 | public: 12 | fclStateValidityChecker( 13 | ompl::base::SpaceInformationPtr si, 14 | std::shared_ptr col_mng_environment, 15 | std::shared_ptr robot, 16 | bool check_parts=false) 17 | : StateValidityChecker(si) 18 | , robot_(robot) 19 | , col_mng_environment_(col_mng_environment) 20 | , check_parts_(check_parts) 21 | { 22 | col_mng_parts_ = std::make_shared(); 23 | for (size_t part = 0; part < robot_->numParts(); ++part){ 24 | auto robot_part = new fcl::CollisionObjectf(robot_->getCollisionGeometry(part)); //, robot_->getTransform(state)); 25 | robot_part->computeAABB(); 26 | part_objs_.push_back(robot_part); 27 | } 28 | col_mng_parts_->registerObjects(part_objs_); 29 | col_mng_parts_->setup(); 30 | } 31 | 32 | ~fclStateValidityChecker() 33 | { 34 | for (auto obj : part_objs_) { 35 | delete obj; 36 | } 37 | } 38 | 39 | bool isValid(const ompl::base::State* state) const override 40 | { 41 | if (!si_->satisfiesBounds(state)) { 42 | return false; 43 | } 44 | 45 | // part/environment checking 46 | for (size_t part = 0; part < robot_->numParts(); ++part) { 47 | const auto& transform = robot_->getTransform(state, part); 48 | auto robot = part_objs_[part]; 49 | robot->setTranslation(transform.translation()); 50 | robot->setRotation(transform.rotation()); 51 | robot->computeAABB(); 52 | fcl::DefaultCollisionData collision_data; 53 | col_mng_environment_->collide(robot, &collision_data, fcl::DefaultCollisionFunction); 54 | if (collision_data.result.isCollision()) { 55 | return false; 56 | } 57 | } 58 | 59 | // part/part checking 60 | if (check_parts_) { 61 | fcl::DefaultCollisionData collision_data; 62 | col_mng_parts_->collide(&collision_data, fcl::DefaultCollisionFunction); 63 | if (collision_data.result.isCollision()) { 64 | return false; 65 | } 66 | } 67 | 68 | return true; 69 | } 70 | protected: 71 | std::shared_ptr robot_; 72 | std::shared_ptr col_mng_environment_; 73 | std::shared_ptr col_mng_parts_; 74 | std::vector part_objs_; 75 | // std::shared_ptr robot_; 76 | bool check_parts_; 77 | }; 78 | -------------------------------------------------------------------------------- /src/planresult.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | template 6 | struct LowLevelPlan { 7 | // std::vector plan; 8 | std::vector trajectory; 9 | std::vector actions; 10 | float cost; 11 | }; 12 | -------------------------------------------------------------------------------- /src/robotStatePropagator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "ompl/control/StatePropagator.h" 4 | #include "robots.h" 5 | 6 | class RobotStatePropagator : public ompl::control::StatePropagator 7 | { 8 | public: 9 | RobotStatePropagator( 10 | const ompl::control::SpaceInformationPtr& si, 11 | std::shared_ptr robot) 12 | : ompl::control::StatePropagator(si) 13 | , robot_(robot) 14 | { 15 | } 16 | 17 | ~RobotStatePropagator() override = default; 18 | 19 | void propagate( 20 | const ompl::base::State *state, 21 | const ompl::control::Control *control, 22 | double duration, 23 | ompl::base::State *result) const override 24 | { 25 | // propagate state 26 | robot_->propagate(state, control, duration, result); 27 | } 28 | 29 | bool canPropagateBackward() const override 30 | { 31 | return false; 32 | } 33 | 34 | bool canSteer() const override 35 | { 36 | return false; 37 | } 38 | 39 | protected: 40 | std::shared_ptr robot_; 41 | }; 42 | -------------------------------------------------------------------------------- /src/robots.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | // OMPL headers 4 | #include 5 | #include 6 | #include 7 | // FCL 8 | #include 9 | 10 | class Robot 11 | { 12 | public: 13 | Robot() 14 | { 15 | } 16 | 17 | virtual void propagate( 18 | const ompl::base::State *start, 19 | const ompl::control::Control *control, 20 | const double duration, 21 | ompl::base::State *result) = 0; 22 | 23 | virtual fcl::Transform3f getTransform( 24 | const ompl::base::State *state, 25 | size_t part = 0) = 0; 26 | 27 | virtual void setPosition(ompl::base::State* state, const fcl::Vector3f position, size_t part = 0) = 0; 28 | 29 | virtual size_t numParts() 30 | { 31 | return 1; 32 | } 33 | 34 | virtual std::shared_ptr getCollisionGeometry(size_t part = 0) 35 | { 36 | return geom_[part]; 37 | } 38 | 39 | std::shared_ptr getSpaceInformation() 40 | { 41 | return si_; 42 | } 43 | 44 | float dt() const 45 | { 46 | return dt_; 47 | } 48 | 49 | float is2D() const 50 | { 51 | return is2D_; 52 | } 53 | 54 | float maxSpeed() const 55 | { 56 | return max_speed_; 57 | } 58 | 59 | protected: 60 | std::vector> geom_; 61 | std::shared_ptr si_; 62 | float dt_; 63 | bool is2D_; 64 | float max_speed_; 65 | }; 66 | 67 | class MultiRobotGoalState: public ompl::base::GoalState 68 | { 69 | public: 70 | MultiRobotGoalState(const ompl::base::SpaceInformationPtr &si): 71 | ompl::base::GoalState(si) 72 | { 73 | } 74 | 75 | size_t numRobots() const 76 | { 77 | auto csi = dynamic_cast(si_.get()); 78 | auto csp = csi->getStateSpace()->as(); 79 | return csp->getSubspaceCount(); 80 | } 81 | 82 | double distanceGoal(const ompl::base::State* st, int robot_idx) const 83 | { 84 | auto csi = dynamic_cast(si_.get()); 85 | auto csp = csi->getStateSpace()->as(); 86 | 87 | // auto csi = dynamic_cast(si_.get()); 88 | auto cst = st->as(); 89 | auto cgoal_state = state_->as(); 90 | auto si_k = csp->getSubspace(robot_idx); 91 | auto st_k = (*cst)[robot_idx]; 92 | auto goal_state_k = (*cgoal_state)[robot_idx]; 93 | double dist = si_k->distance(st_k, goal_state_k); 94 | 95 | return dist; 96 | } 97 | 98 | // distance is the max of the individual robot distances 99 | double distanceGoal(const ompl::base::State *st) const 100 | { 101 | double result = 0; 102 | for (size_t k = 0; k < numRobots(); ++k) { 103 | result = fmax(result, distanceGoal(st, k)); 104 | } 105 | return result; 106 | } 107 | 108 | bool isSatisfied(const ompl::base::State* st, int robot_idx) 109 | { 110 | double dist = distanceGoal(st, robot_idx); 111 | return dist < threshold_; 112 | } 113 | 114 | }; 115 | 116 | // Factory Method 117 | std::shared_ptr create_robot( 118 | const std::string& robotType, 119 | const ompl::base::RealVectorBounds& positionBounds); 120 | 121 | std::shared_ptr create_joint_robot( 122 | std::vector> robots); 123 | 124 | // HACK for multi-robot 125 | void setMultiRobotGoals(std::shared_ptr, std::shared_ptr goals); -------------------------------------------------------------------------------- /tuning/unicycle_first_order_0/algorithms.yaml: -------------------------------------------------------------------------------- 1 | sst: 2 | default: 3 | goal_epsilon: 0.1 4 | goal_bias: 0.05 5 | selection_radius: 0.2 # delta_BN in paper 6 | pruning_radius: 0.1 # delta_s in paper 7 | propagation_step_size: 0.1 #s 8 | control_duration: [1, 10] # multiples of step size 9 | rai_cfg: | 10 | opt/verbose: 4 11 | KOMO/verbose: 3 12 | parallelpark_0: 13 | goal_epsilon: 0.05 14 | 15 | dbAstar-komo: 16 | default: 17 | add_primitives_per_iteration: 500 18 | desired_branching_factor: 16 19 | suboptimality_bound: 1.0 20 | alpha: 0.3 21 | filter_duplicates: False 22 | # Config file (rai.cfg) that will be used 23 | rai_cfg: | 24 | opt/verbose: 4 25 | KOMO/verbose: 3 26 | opt/stopTolerance: 0.001 27 | add_init_noise: 0.05 28 | 29 | dbAstar-scp: 30 | default: 31 | add_primitives_per_iteration: 100 32 | desired_branching_factor: 16 33 | suboptimality_bound: 1.0 34 | 35 | dbAstar: 36 | default: 37 | add_primitives_per_iteration: 500 38 | desired_branching_factor: 16 39 | suboptimality_bound: 1.0 40 | alpha: 0.3 41 | filter_duplicates: False 42 | 43 | sbpl: 44 | default: 45 | dummy: 0 46 | 47 | komo: 48 | default: 49 | # Config file (rai.cfg) that will be used 50 | rai_cfg: | 51 | opt/verbose: 4 52 | KOMO/verbose: 3 53 | opt/stopTolerance: 0.001 54 | add_init_noise: 0.1 55 | 56 | scp: 57 | default: 58 | dummy: 0 59 | 60 | gen-motion: 61 | timelimit: 120 # 2 min 62 | search: "linear" 63 | env_limit: 2 # m 64 | rai_cfg: | 65 | opt/stopTolerance: 0.001 66 | add_init_noise: 0.1 67 | --------------------------------------------------------------------------------