├── .clang-format ├── .clang-format-ignore.txt ├── .github └── workflows │ ├── build.yml │ └── documentation.yml ├── .gitignore ├── CITATION.CFF ├── CMakeLists.txt ├── LICENSE ├── README.md ├── benchmarking ├── CMakeLists.txt ├── README.md ├── include │ └── benchmarking │ │ ├── OMPLPlanner.h │ │ ├── TPRMPlanner.h │ │ ├── base.h │ │ ├── benchmark.h │ │ ├── factory.h │ │ ├── obstacle.h │ │ └── result_handler.h ├── scripts │ ├── README.md │ ├── create_movie.py │ └── create_movie_3D.py ├── src │ ├── OMPLPlanner.cpp │ ├── TPRMPlanner.cpp │ ├── base.cpp │ ├── benchmark.cpp │ ├── factory.cpp │ └── result_handler.cpp └── study │ ├── narrow_gap.cpp │ ├── study_dynamic_obstacles.cpp │ └── study_static_obstacles.cpp ├── cmake └── FindOMPL.cmake ├── docs └── Doxyfile.in ├── examples ├── CMakeLists.txt ├── dynamic_avoidance.cpp └── static_avoidance.cpp ├── include └── tprm │ ├── config.h │ ├── obstacle_base.h │ ├── obstacle_impl.h │ ├── temporal_graph.h │ └── temporal_prm.h └── src ├── config.cpp ├── obstacle_base.cpp ├── obstacle_impl.cpp ├── temporal_graph.cpp └── temporal_prm.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | # Use the Google style in this project. 2 | BasedOnStyle: Google 3 | UseTab: Never 4 | IndentWidth: 4 5 | ColumnLimit: 160 6 | AccessModifierOffset: -4 7 | ReflowComments: false 8 | AllowShortIfStatementsOnASingleLine: false 9 | AllowShortLoopsOnASingleLine: false 10 | AllowShortCaseLabelsOnASingleLine: false 11 | AllowShortBlocksOnASingleLine: false 12 | AllowShortFunctionsOnASingleLine: Empty -------------------------------------------------------------------------------- /.clang-format-ignore.txt: -------------------------------------------------------------------------------- 1 | # build directories 2 | ./build/* 3 | ./cmake-build-*/* 4 | ./.vscode/* 5 | ./.github/* 6 | ./.git/* -------------------------------------------------------------------------------- /.github/workflows/build.yml: -------------------------------------------------------------------------------- 1 | name: Build 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | runs-on: ${{ matrix.os }} 8 | strategy: 9 | matrix: 10 | os: [ubuntu-latest] 11 | configs: [Release, Debug] 12 | steps: 13 | - name: Checkout 14 | uses: actions/checkout@v2.0.0 15 | - name: Build project 16 | uses: nicledomaS/cmake_build_action@v1.4 17 | with: 18 | config: ${{ matrix.configs }} 19 | -------------------------------------------------------------------------------- /.github/workflows/documentation.yml: -------------------------------------------------------------------------------- 1 | name: Creates and deploys Documentation 2 | 3 | on: 4 | push: 5 | branches: main 6 | jobs: 7 | build: 8 | runs-on: ubuntu-latest 9 | steps: 10 | - name: Checkout 11 | uses: actions/checkout@v2 12 | - name: Build Documentation 13 | uses: mattnotmitt/doxygen-action@v1.2.3 14 | with: 15 | working-directory: 'docs' 16 | doxyfile-path: 'Doxyfile.in' 17 | - name: Publishing Documentation 18 | uses: peaceiris/actions-gh-pages@v3 19 | with: 20 | github_token: ${{ secrets.GITHUB_TOKEN }} 21 | publish_dir: docs/html -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Folders 2 | build/* 3 | .vscode/* 4 | 5 | -------------------------------------------------------------------------------- /CITATION.CFF: -------------------------------------------------------------------------------- 1 | # YAML 1.2 2 | --- 3 | authors: 4 | - 5 | affiliation: "Department of Mechanical and Process Engineering, ETH, Zurich, Switzerland" 6 | family-names: Hüppi 7 | given-names: Matthias 8 | - 9 | affiliation: "Department of Mechanical and Process Engineering, ETH, Zurich, Switzerland" 10 | family-names: Bartolomei 11 | given-names: Luca 12 | - 13 | affiliation: "Department of Mechanical and Process Engineering, ETH, Zurich, Switzerland" 14 | family-names: Mascaro 15 | given-names: Ruben 16 | - 17 | affiliation: "Department of Mechanical and Process Engineering, ETH, Zurich, Switzerland" 18 | family-names: Chli 19 | given-names: Margarita 20 | cff-version: "1.1.0" 21 | keywords: 22 | - "Motion and Path Planning" 23 | - "Collision Avoidance" 24 | message: "If you use this software, please cite it using these metadata." 25 | repository-code: "https://github.com/VIS4ROB-lab/t_prm.git" 26 | title: "T-PRM: Temporal Probabilistic Road-Map for Path Planning in Dynamic Environments" 27 | ... 28 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11) 2 | 3 | project(TPRM) 4 | set(CMAKE_BUILD_TYPE Release) 5 | 6 | set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) 7 | 8 | include(FetchContent) 9 | macro(fetch what) 10 | FetchContent_GetProperties("${what}") 11 | if(NOT ${${what}_POPULATED}) 12 | message(STATUS "fetching ${what} ...") 13 | FetchContent_Populate(${what}) 14 | endif() 15 | mark_as_advanced(${${what}_SOURCE_DIR}) 16 | endmacro() 17 | 18 | set(CMAKE_CXX_STANDARD 17) 19 | 20 | find_package (Eigen3 3.3 NO_MODULE) 21 | if(NOT TARGET Eigen3::Eigen) 22 | # Remove entry that has not been found 23 | unset(Eigen3_DIR CACHE) 24 | 25 | # Now download eigen3 26 | # download as zip, so it's a bit smaller than downloading the whole repo 27 | FetchContent_Declare( 28 | eigen # 29 | URL https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.zip # 30 | URL_HASH MD5=994092410ba29875184f7725e0371596 # 31 | ) 32 | fetch(eigen) 33 | add_library(eigen INTERFACE) 34 | add_library (Eigen3::Eigen ALIAS eigen) 35 | target_include_directories(eigen INTERFACE ${eigen_SOURCE_DIR}) 36 | endif(NOT TARGET Eigen3::Eigen) 37 | 38 | find_package(OpenMP REQUIRED) 39 | 40 | file(GLOB TPRM_SRC_FILES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp) 41 | file(GLOB TPRM_HEADER_FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h) 42 | 43 | add_library(${PROJECT_NAME} ${TPRM_SRC_FILES} ${TPRM_HEADER_FILES}) 44 | target_include_directories(${PROJECT_NAME} PUBLIC "include") 45 | 46 | target_link_libraries(${PROJECT_NAME} Eigen3::Eigen OpenMP::OpenMP_CXX) 47 | 48 | add_subdirectory(examples) 49 | 50 | option(BUILD_BENCHMARKING "Build benchmarking" OFF) 51 | 52 | if(${BUILD_BENCHMARKING}) 53 | add_subdirectory(benchmarking) 54 | endif() 55 | 56 | # Header files 57 | install(FILES ${TPRM_HEADER_FILES} DESTINATION include) 58 | 59 | # Compiled library 60 | install(TARGETS ${PROJECT_NAME} 61 | RUNTIME DESTINATION bin 62 | LIBRARY DESTINATION lib 63 | ARCHIVE DESTINATION lib/${PROJECT_NAME}) 64 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # T-PRM: Temporal Probabilistic Roadmap for Path Planning in Dynamic Environments 2 | This repository contains the source code for the T-PRM algorithm for path planning in dynamic environments. 3 | If you use this code in your academic work, please cite ([PDF](https://www.research-collection.ethz.ch/handle/20.500.11850/560560), [Video](https://youtu.be/Eh6brn_dVlU)): 4 | 5 | @inproceedings{hueppi2022tprm, 6 | title={T-PRM: Temporal Probabilistic Roadmap for Path Planning in Dynamic Environments}, 7 | author={Hüppi, Matthias and Bartolomei, Luca and Mascaro, Ruben and Chli, Margarita}, 8 | booktitle={2022 {IEEE/RSJ} International Conference on Intelligent Robots and Systems ({IROS})}, 9 | year={2022} 10 | } 11 | 12 | > **Abstract**: Sampling-based motion planners are widely used in robotics due to their simplicity, flexibility and computational efficiency. However, in their most basic form, these algorithms operate under the assumption of static scenes and lack the ability to avoid collisions with dynamic (i.e. moving) obstacles. This raises safety concerns, limiting the range of possible applications of mobile robots in the real world. Motivated by these challenges, in this work we present Temporal-PRM, a novel sampling-based path-planning algorithm that performs obstacle avoidance in dynamic environments. The proposed approach extends the original Probabilistic Roadmap (PRM) with the notion of time, generating an augmented graph-like structure that can be efficiently queried using a time-aware variant of the A* search algorithm, also introduced in this paper. Our design maintains all the properties of PRM, such as the ability to perform multiple queries and to find smooth paths, while circumventing its downside by enabling collision avoidance in highly dynamic scenes with a minor increase in the computational cost. Through a series of challenging experiments in highly cluttered and dynamic environments, we demonstrate that the proposed path planner outperforms other state-of-the-art sampling-based solvers. Moreover, we show that our algorithm can run onboard a flying robot, performing obstacle avoidance in real time. 13 | 14 | This project is released under a GPLv3 license. 15 | 16 | ## Dependencies 17 | The code has been tested under Ubuntu 20.04 and does not have any special dependency. 18 | 19 | However, in order to run the benchmarks against OMPL, this library needs to be installed: 20 | - If you have ROS installed, run `sudo apt install ros-noetic-ompl`; 21 | - Otherwise run `sudo apt install libompl-dev`. 22 | 23 | ## Compiling 24 | To compile the T-PRM library, execute the following steps: 25 | 1) Clone this repository: `git clone git@github.com:VIS4ROB-lab/t_prm.git` 26 | 2) Create a build folder: `mkdir build` 27 | 3) Navigate to the build folder: `cd build` 28 | 4) Execute cmake: `cmake ..` 29 | 5) Compile with make: `make` 30 | 31 | ## Example 32 | The [examples](examples) folder contains examples on how to use the library, but we strongly suggest to refer to the [documentation](https://vis4rob-lab.github.io/t_prm). 33 | Here, we report the main instructions to run T-PRM with one dynamic obstacle moving in the space: 34 | 1. Necessary includes: 35 | ``` 36 | #include 37 | #include 38 | ``` 39 | 2. Set the (holonomic) robot speed to a desired value: 40 | ``` 41 | tprm::HolonomicRobot::movement_speed = 0.1; // m/s 42 | ``` 43 | 3. Create the main T-PRM object: 44 | ``` 45 | tprm::TemporalPRM tprm; 46 | ``` 47 | 4. Add a dynamic obstacle. In this case, the obstacle with dimension `0.25 m` is spawned at `(1, 1, 1)` and moves towards `(0, 0, 0)` with a velocity vector `(-0.1, -0.1, -0.1)`: 48 | ``` 49 | tprm.addDynamicObstacle(std::make_shared(tprm::Vector3d::Constant(1.), tprm::Vector3d::Constant(-0.1), 0.25)); 50 | ``` 51 | 5. Sample the space (`150` samples) and create the roadmap with a maximum edge length of `0.25 m`: 52 | ``` 53 | tprm.placeSamples(150); 54 | tprm.buildPRM(0.25); 55 | ``` 56 | 6. Query the roadmap for a path from `(0, 0, 0)` to `(1, 1, 1)` starting at time `0.5 s`: 57 | ``` 58 | auto path = tprm.getShortestPath(tprm::Vector3d(0, 0, 0), tprm::Vector3d(1., 1., 1.), 0.5); 59 | ``` 60 | 7. Print the result: 61 | ``` 62 | if (path.empty()) { 63 | std::cout << "No path found" << std::endl; 64 | } else { 65 | for (auto& node : path) 66 | std::cout << "Node: " << node.position.transpose() << " at time " << node.time << std::endl; 67 | } 68 | ``` 69 | 70 | ## Running Benchmarks 71 | In order to run the benchmarks, check out the [instructions](benchmarking) in the `benchmarking` folder. To generate movies out of the benchmarks, check the folder [benchmarking/scripts](benchmarking/scripts). 72 | -------------------------------------------------------------------------------- /benchmarking/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11) 2 | project(benchmarking) 3 | 4 | find_package(ompl REQUIRED) 5 | 6 | macro(custom_exec file) 7 | add_executable(${file} study/${file}.cpp) 8 | target_link_libraries(${file} benchmarking) 9 | target_include_directories(${file} PUBLIC include ${OMPL_INCLUDE_DIR}) 10 | endmacro() 11 | 12 | set(CMAKE_CXX_STANDARD 17) 13 | 14 | file(GLOB LIBRARY_SOURCES "src/*.cpp") 15 | 16 | add_library(benchmarking ${LIBRARY_SOURCES}) 17 | target_link_libraries(benchmarking ${OMPL_LIBRARIES} Eigen3::Eigen TPRM) 18 | target_include_directories(benchmarking PUBLIC include ${OMPL_INCLUDE_DIRS} Eigen3::Eigen TPRM) 19 | 20 | custom_exec(study_static_obstacles) 21 | custom_exec(study_dynamic_obstacles) 22 | custom_exec(narrow_gap) -------------------------------------------------------------------------------- /benchmarking/README.md: -------------------------------------------------------------------------------- 1 | # T-PRM Benchmarking 2 | 3 | See folder `study` for some examples. 4 | 5 | Make sure that you compile against the OMPL version with which you are actually running the benchmarks. 6 | 7 | ## Run Instructions 8 | 1) Build the benchmarking library (`cmake -DBUILD_BENCHMARKING ..` and `make`) 9 | 2) Build the study executable (done together with the above command) 10 | 3) Run the study executable (`./benchmarking/study_dynamic_obstacles` or `./benchmarking/study_static_obstacles`) -------------------------------------------------------------------------------- /benchmarking/include/benchmarking/OMPLPlanner.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #pragma once 40 | 41 | #include 42 | 43 | namespace benchmarking { 44 | 45 | class OMPLPlannerBenchmark : public BasePlannerBenchmark { 46 | public: 47 | OMPLPlannerBenchmark(std::string planner = "RRTstar"); 48 | std::string getName() const override { 49 | return "OMPL " + m_planner; 50 | } 51 | 52 | BenchmarkResult runBenchmark(std::shared_ptr benchmark, int benchmark_idx, int run_idx) override; 53 | 54 | private: 55 | std::string m_planner; 56 | }; 57 | } // namespace benchmarking -------------------------------------------------------------------------------- /benchmarking/include/benchmarking/TPRMPlanner.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | 42 | namespace benchmarking { 43 | 44 | // Wrapper around T-PRM 45 | class TPRMPlannerBenchmark : public BasePlannerBenchmark { 46 | public: 47 | TPRMPlannerBenchmark() {} 48 | std::string getName() const override { 49 | return "T-PRM"; 50 | } 51 | 52 | BenchmarkResult runBenchmark(std::shared_ptr benchmark, int benchmark_idx, int run_idx) override; 53 | 54 | private: 55 | // path (pos x, pos y, pos z), time (t) 56 | std::pair, std::vector> computePath(const std::vector& path) const; 57 | }; 58 | 59 | } // namespace benchmarking -------------------------------------------------------------------------------- /benchmarking/include/benchmarking/base.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #pragma once 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | namespace benchmarking { 49 | 50 | // Individual time measurement. 51 | struct TimingResult { 52 | std::string description; 53 | double duration_micros; 54 | }; 55 | 56 | struct BenchmarkResult { 57 | std::string description = "unset"; 58 | double duration_micros = -1.; 59 | std::vector success = {}; 60 | std::vector> path = {}; 61 | std::vector> path_durations = {}; // for T-PRM 62 | std::vector timing_results = {}; 63 | }; 64 | 65 | class BasePlannerBenchmark { 66 | public: 67 | BasePlannerBenchmark(); 68 | 69 | virtual std::string getName() const = 0; 70 | 71 | virtual BenchmarkResult runBenchmark(std::shared_ptr benchmark, int benchmark_idx, int run_idx); 72 | 73 | void startBenchmark(); 74 | void stopBenchmark(); 75 | 76 | void startMeasurement(); 77 | TimingResult stopMeasurement(std::string what); 78 | 79 | // in micro seconds 80 | double getTotalDuration() const; 81 | 82 | private: 83 | std::chrono::time_point m_start_time; 84 | 85 | std::chrono::time_point m_total_start_time; 86 | double m_total_duration; 87 | }; 88 | 89 | } // namespace benchmarking -------------------------------------------------------------------------------- /benchmarking/include/benchmarking/benchmark.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #pragma once 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | namespace benchmarking { 46 | 47 | class Benchmark { 48 | public: 49 | Benchmark(const std::string& name, int num_runs); 50 | ~Benchmark(); 51 | 52 | int numRuns() const { 53 | return m_num_runs; 54 | } 55 | 56 | int numNodes = 500; 57 | 58 | // Domain size 59 | double domain_size = 10.; 60 | 61 | bool is_2d = false; 62 | 63 | std::vector start = {}; 64 | std::vector goal = {}; 65 | 66 | // Obstacles 67 | std::vector circles = {}; // static 68 | std::vector moving_circles = {}; // dynamic 69 | 70 | bool writeMovingIntermediatePaths = false; 71 | 72 | // Individual 73 | // ********** 74 | // RRT*-FND 75 | double ompl_edge_length = 1.; 76 | 77 | // T-PRM 78 | double tprm_cost_edge_threshold = 1.; 79 | 80 | // OMPL RRT* 81 | double ompl_time_limit = 1.0; 82 | double ompl_path_length_threshold = 1.0; 83 | 84 | std::string name; 85 | 86 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 87 | 88 | private: 89 | int m_num_runs; 90 | }; 91 | } // namespace benchmarking -------------------------------------------------------------------------------- /benchmarking/include/benchmarking/factory.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | #include 47 | 48 | namespace benchmarking { 49 | 50 | class BenchmarkFactory { 51 | public: 52 | BenchmarkFactory(); 53 | ~BenchmarkFactory(); 54 | 55 | void register_benchmark(std::shared_ptr benchmark); 56 | void register_benchmarks(std::vector> benchmarks); 57 | void register_planner(std::shared_ptr planner); 58 | void register_result_handler(std::shared_ptr result_handler); 59 | 60 | void run(); 61 | 62 | private: 63 | void run_benchmark(std::shared_ptr planner, std::shared_ptr benchmark); 64 | 65 | void writeStaticObstacles(size_t bm_idx) const; 66 | 67 | void log(std::string message) const; 68 | void log(std::string tag, std::string message) const; 69 | 70 | private: 71 | std::vector> m_planners; 72 | std::vector> m_benchmarks; 73 | std::vector> m_result_handlers; 74 | 75 | int current_benchmark_index = 0; 76 | }; 77 | 78 | } // namespace benchmarking 79 | -------------------------------------------------------------------------------- /benchmarking/include/benchmarking/obstacle.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #pragma once 40 | #include 41 | 42 | namespace benchmarking { 43 | 44 | struct Circle { 45 | Circle(const Eigen::Vector3d& _center, double _radius) : center(_center), radius(_radius) {} 46 | Eigen::Vector3d center; 47 | double radius; 48 | }; 49 | 50 | struct MovingCircle { 51 | MovingCircle(const Eigen::Vector3d& _center, double _radius, Eigen::Vector3d _velocity) : center(_center), radius(_radius), velocity(_velocity) {} 52 | Eigen::Vector3d center; 53 | double radius; 54 | Eigen::Vector3d velocity; 55 | }; 56 | 57 | }; // namespace benchmarking -------------------------------------------------------------------------------- /benchmarking/include/benchmarking/result_handler.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #pragma once 40 | 41 | #include 42 | 43 | #include 44 | 45 | namespace benchmarking { 46 | 47 | class ResultHandler { 48 | public: 49 | ResultHandler(std::string name) : m_name(name) {} 50 | virtual ~ResultHandler() = default; 51 | 52 | virtual void nextBenchmarkIs(std::shared_ptr benchmark) {} 53 | virtual void handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) = 0; 54 | std::string getName() const { 55 | return m_name; 56 | } 57 | 58 | virtual void printSummary(std::string planner, int benchmark_idx){}; // called after all runs for a single benchmark are done 59 | virtual void printSummary() {} // called after everything is done 60 | 61 | private: 62 | std::string m_name; 63 | }; 64 | 65 | class ResultHandlerPathLength : public ResultHandler { 66 | public: 67 | ResultHandlerPathLength() : ResultHandler("Path Length"), m_file("path_length.csv") {} 68 | void handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) override; 69 | 70 | void printSummary(std::string planner, int benchmark_idx) override; 71 | 72 | void nextBenchmarkIs(std::shared_ptr benchmark) override; 73 | 74 | private: 75 | std::vector m_path_lengths_for_idx; 76 | std::string last_description; 77 | std::ofstream m_file; 78 | 79 | std::shared_ptr m_current_benchmark; 80 | }; 81 | 82 | class ResultHandlerSuccessRate : public ResultHandler { 83 | public: 84 | ResultHandlerSuccessRate() : ResultHandler("Success Rate"), m_file("success_rate.csv") {} 85 | void handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) override; 86 | void printSummary(std::string planner, int benchmark_idx) override; 87 | 88 | private: 89 | std::vector m_results_for_idx; 90 | std::ofstream m_file; 91 | std::string last_description; 92 | }; 93 | 94 | class ResultHandlerComputingTime : public ResultHandler { 95 | public: 96 | ResultHandlerComputingTime() : ResultHandler("Computing Time"), m_file("computing_time.csv") {} 97 | void handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) override; 98 | void printSummary(std::string planner, int benchmark_idx) override; 99 | 100 | private: 101 | std::vector m_results_for_idx; 102 | std::ofstream m_file; 103 | std::string last_description; 104 | }; 105 | 106 | class ResultHandlerThetaChanges : public ResultHandler { 107 | public: 108 | ResultHandlerThetaChanges() : ResultHandler("Theta Changes"), m_file("theta_changes.csv") {} 109 | void handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) override; 110 | void printSummary(std::string planner, int benchmark_idx) override; 111 | 112 | private: 113 | std::vector m_results_for_idx; 114 | std::ofstream m_file; 115 | std::string last_description; 116 | }; 117 | 118 | class ResultHandlerTPRMWaiting : public ResultHandler { 119 | public: 120 | ResultHandlerTPRMWaiting() : ResultHandler("TPRM Waiting") {} 121 | void handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) override; 122 | void printSummary(std::string planner, int benchmark_idx) override; 123 | 124 | private: 125 | int m_waiting = 0; 126 | }; 127 | 128 | class ResultHandlerPathWriter : public ResultHandler { 129 | public: 130 | ResultHandlerPathWriter() : ResultHandler("Path Writer") {} 131 | void handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) override; 132 | }; 133 | 134 | class ResultHandlerAllWriter : public ResultHandler { 135 | public: 136 | ResultHandlerAllWriter(); 137 | void handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) override; 138 | 139 | private: 140 | std::ofstream m_file; 141 | }; 142 | 143 | } // namespace benchmarking -------------------------------------------------------------------------------- /benchmarking/scripts/README.md: -------------------------------------------------------------------------------- 1 | # Generating movies out of benchmarks 2 | 3 | As shown in the video of the publication, we create several movies of the planners in static and dynamic environments. 4 | 5 | After running either `study_dynamic_obstacles` or `study_static_obstacles`, several files will be generated. 6 | 7 | In the current workspace, there will be a file called `info_about_bm.txt` which contains the information about the benchmark. Furthermore, several `*.path` files will be generated (each containing a path from the start to the goal for a given planner and benchmark). Using the script found in this folder, we can create the movies (either using the 2D or 3D script). -------------------------------------------------------------------------------- /benchmarking/scripts/create_movie.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | ********************************************************************* 5 | * 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2022, 9 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 10 | * All rights reserved. 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the institute nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | * 39 | * Authors: Matthias Busenhart 40 | ********************************************************************* 41 | """ 42 | 43 | import matplotlib.pyplot as plt 44 | import numpy as np 45 | from matplotlib.animation import FuncAnimation 46 | import matplotlib.patches as patches 47 | import os 48 | from matplotlib.collections import PatchCollection 49 | 50 | FRAMES_PER_SECOND = 30 51 | DT = 1./FRAMES_PER_SECOND 52 | MAX_TIME = 22. 53 | ROBOT_SIZE = 0.25 54 | 55 | 56 | class Obstacle: 57 | def __init__(self, x, y, r, vx, vy): 58 | self.x = x 59 | self.y = y 60 | self.r = r 61 | self.vx = vx 62 | self.vy = vy 63 | 64 | 65 | class BM: 66 | def __init__(self): 67 | self.name = "" 68 | self.start = [] 69 | self.goal = [] 70 | self.obstacles = [] 71 | self.number_of_runs = 0 72 | 73 | 74 | def parse_info(): 75 | info_file = "info_about_bm.txt" 76 | # parse the file and create a vector of bms 77 | bms = [] 78 | load_obstacles = False 79 | load_queries = False 80 | with open(info_file) as f: 81 | bm = None 82 | for line in f: 83 | if len(line) == 0: 84 | continue 85 | if line[0] == '=': 86 | if bm is not None: 87 | bms.append(bm) 88 | bm = BM() 89 | elif line.startswith("Benchmark:"): 90 | bm.name = line.split(":")[1].strip() 91 | elif line.startswith("Num. of runs:"): 92 | bm.number_of_runs = int(line.split(":")[1].strip()) 93 | elif line.startswith("OBSTACLES"): 94 | load_obstacles = True 95 | load_queries = False 96 | elif line.startswith("QUERIES"): 97 | load_obstacles = False 98 | load_queries = True 99 | elif line.startswith("Num."): 100 | pass 101 | else: 102 | # should be either obstacles or queries 103 | if load_obstacles: 104 | x, y, z, r, vx, vy, vz = line.split(",") 105 | bm.obstacles.append(Obstacle(float(x), float( 106 | y), float(r) - ROBOT_SIZE, float(vx), float(vy))) 107 | elif load_queries: 108 | x, y, z, x2, y2, z2 = line.split(",") 109 | bm.start.append([float(x), float(y)]) 110 | bm.goal.append([float(x2), float(y2)]) 111 | bms.append(bm) 112 | return bms[0] # only parse first bm 113 | 114 | 115 | def get_path_for_time(path, time): 116 | """ 117 | the path is like [[x1,y1,z1, t1], [x2,y2,z2, t2], ...] 118 | we have to search for the time which is highger, then 119 | interpolate the last segment linearly and return the new path (new start + end of input path) 120 | """ 121 | if len(path) == 0: 122 | return [], [] 123 | for i, segment in enumerate(path): 124 | if len(segment) < 3: 125 | return [], [] 126 | if segment[3] > time: 127 | break 128 | 129 | # the segment is i-1 in which we currently are 130 | if i == 0: 131 | print("time is smaller than first segment") 132 | return path[0], path 133 | 134 | if i == len(path): 135 | print("time is bigger than last segment") 136 | return path, path[-1] 137 | 138 | # interpolate 139 | x1, y1, z1, t1 = path[i-1] 140 | x2, y2, z2, t2 = path[i] 141 | t = (time - t1) / (t2 - t1) 142 | x = x1 + t * (x2 - x1) 143 | y = y1 + t * (y2 - y1) 144 | z = z1 + t * (z2 - z1) 145 | ret_a = [] 146 | ret_b = [] 147 | ret_a.extend(path[:i]) 148 | ret_a.append([x, y, z, time]) 149 | 150 | ret_b.append([x, y, z, time]) 151 | ret_b.extend(path[i:]) 152 | 153 | return ret_a, ret_b 154 | 155 | 156 | def compute_time(x1, y1, z1, x2, y2, z2, speed=0.75): 157 | # compute the norm of the vector 158 | length = np.sqrt((x2-x1)**2 + (y2-y1)**2 + (z2-z1)**2) 159 | return length / speed 160 | 161 | 162 | def read_path(filename): 163 | data = open(filename).read() 164 | data = data.split("\n") 165 | data = [line.split(" ") for line in data] 166 | ret = [] 167 | total_time = 0. 168 | floats = [] 169 | for line in data: 170 | if len(line) == 0 or len(line) == 1: 171 | continue 172 | if len(line) < 6: 173 | print(f"Error parsing {filename} at line {line}") 174 | floats = [float(x) for x in line] 175 | time = compute_time(*floats[:6]) 176 | ret.append([*floats[:3], total_time]) 177 | 178 | total_time += time 179 | # add last three points 180 | ret.append([*floats[3:6], total_time]) 181 | return ret 182 | 183 | 184 | def find_all_intermediate_paths_for(planner): 185 | # get all files in the current directory 186 | files = os.listdir() 187 | # find all files with the format: 188 | # INTERMEDIATE_PATH_{planner}_AT_{time}.txt 189 | # and return the time 190 | time = [float(x.split("_")[-1].split(".txt")[0]) 191 | for x in files if x.startswith("INTERMEDIATE_PATH_" + planner)] 192 | time.sort() 193 | ret = [] 194 | for t in time: 195 | path = read_path(f"INTERMEDIATE_PATH_{planner}_AT_{t:.6f}.txt") 196 | ret.append([t, path]) 197 | 198 | ret.append([1000, []]) 199 | return ret 200 | 201 | 202 | full_tprm = read_path("path_T-PRM_0_0_0.path") 203 | full_ompl_rrt = read_path("path_OMPL RRTstar_0_0_0.path") 204 | full_ompl_prm = read_path("path_OMPL PRM_0_0_0.path") 205 | 206 | intermediate_tprm = find_all_intermediate_paths_for("T-PRM A-star") 207 | intermediate_ompl_rrt = find_all_intermediate_paths_for("OMPL RRTstar") 208 | intermediate_ompl_prm = find_all_intermediate_paths_for("OMPL PRM") 209 | 210 | bm = parse_info() 211 | 212 | fig, ax = plt.subplots(figsize=(8, 8)) 213 | (ln,) = plt.plot([], [], "r") 214 | 215 | colors = { 216 | "tprm": "g", 217 | "ompl_rrt": "y", 218 | "ompl_prm": "b" 219 | } 220 | 221 | 222 | def draw_path(path, ax, color, label, alpha=1., addLabel=True): 223 | if len(path) == 0 or len(path[0]) < 3: 224 | return 225 | # convert to a np array 226 | path = np.array(path) 227 | # draw the path 228 | if addLabel: 229 | ax.plot(path[:, 0], path[:, 1], color, label=label, alpha=alpha) 230 | else: 231 | ax.plot(path[:, 0], path[:, 1], color, alpha=alpha) 232 | 233 | 234 | def draw_robot(path, ax, color, zorder=1): 235 | if len(path) == 0 or len(path[0]) < 3: 236 | return 237 | ax.add_collection(PatchCollection([patches.Circle( 238 | (path[0][0], path[0][1]), ROBOT_SIZE, color=color)], match_original=True, zorder=zorder)) 239 | 240 | 241 | def draw_intermediate_paths(i_paths, time, ax, color): 242 | return # TODO: remove this line if you want to draw the paths yet to be driven 243 | if time == 0: 244 | draw_path(i_paths[0][1], ax, color, "", 0.5, False) 245 | return 246 | # search the entry in i_paths which is lower than time 247 | for i, (t, path) in enumerate(i_paths): 248 | if t >= time: 249 | break 250 | # draw the path 251 | _, path = get_path_for_time(i_paths[i-1][1], time - i_paths[i-1][0]) 252 | # draw_path(_, ax, color, f"intermediate path {i}", 1.0, False) 253 | draw_path(path, ax, color, f"intermediate path {i}", 0.5, False) 254 | 255 | 256 | def init_anim(): 257 | return (ln,) 258 | 259 | 260 | def update(frame): 261 | ax.clear() 262 | ax.set_xlim(0, 10) 263 | ax.set_ylim(0, 10) 264 | ax.set_aspect('equal') 265 | ax.set_xticks([]) 266 | ax.set_yticks([]) 267 | 268 | # draw obstacles 269 | for obstacle in bm.obstacles: 270 | pos_x = obstacle.x + frame * DT * obstacle.vx 271 | pos_y = obstacle.y + frame * DT * obstacle.vy 272 | ax.add_patch(plt.Circle((pos_x, pos_y), obstacle.r, color='gray')) 273 | 274 | # T-PRM 275 | tprm_path_a, tprm_path_b = get_path_for_time(full_tprm, frame * DT) 276 | draw_path(tprm_path_a, ax, colors["tprm"], "T-PRM") 277 | #draw_path(tprm_path_b, ax, colors["tprm"], "T-PRM", 0.5, False) 278 | 279 | # OMPL PRM 280 | omplprm_path_a, omplprm_path_b = get_path_for_time( 281 | full_ompl_prm, frame * DT) 282 | draw_path(omplprm_path_a, ax, colors["ompl_prm"], "OMPL PRM") 283 | draw_intermediate_paths(intermediate_ompl_prm, 284 | frame * DT, ax, colors["ompl_prm"]) 285 | 286 | # OMPL RRT 287 | omplrrt_path_a, omplrrt_path_b = get_path_for_time( 288 | full_ompl_rrt, frame * DT) 289 | draw_path(omplrrt_path_a, ax, colors["ompl_rrt"], "OMPL RRT*") 290 | draw_intermediate_paths(intermediate_ompl_rrt, 291 | frame * DT, ax, colors["ompl_rrt"]) 292 | 293 | draw_robot(omplrrt_path_b, ax, colors["ompl_rrt"],zorder=5) 294 | draw_robot(omplprm_path_b, ax, colors["ompl_prm"],zorder=6) 295 | draw_robot(tprm_path_b, ax, colors["tprm"],zorder=7) 296 | 297 | # show legend 298 | ax.legend(loc='upper left') 299 | 300 | ax.set_title(f"{frame * DT:.2f}s") 301 | 302 | plt.tight_layout() 303 | return (ln,) 304 | 305 | 306 | ani = FuncAnimation(fig, update, frames=range( 307 | int(MAX_TIME / DT)), init_func=init_anim, blit=False) 308 | 309 | plt.show() 310 | #ani.save("movie.mp4", writer="ffmpeg", fps=FRAMES_PER_SECOND) 311 | -------------------------------------------------------------------------------- /benchmarking/scripts/create_movie_3D.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | """ 4 | ********************************************************************* 5 | * 6 | * Software License Agreement (BSD License) 7 | * 8 | * Copyright (c) 2022, 9 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 10 | * All rights reserved. 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the institute nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | * 39 | * Authors: Matthias Busenhart 40 | ********************************************************************* 41 | """ 42 | 43 | import matplotlib.pyplot as plt 44 | import numpy as np 45 | from matplotlib.animation import FuncAnimation 46 | import matplotlib.patches as patches 47 | import os 48 | 49 | FRAMES_PER_SECOND = 15 50 | DT = 1./FRAMES_PER_SECOND 51 | MAX_TIME = 29.3 52 | ROBOT_SIZE = 0.25 53 | 54 | 55 | class Obstacle: 56 | def __init__(self, x, y, z, r, vx, vy, vz): 57 | self.x = x 58 | self.y = y 59 | self.z = z 60 | self.r = r 61 | self.vx = vx 62 | self.vy = vy 63 | self.vz = vz 64 | 65 | 66 | class BM: 67 | def __init__(self): 68 | self.name = "" 69 | self.start = [] 70 | self.goal = [] 71 | self.obstacles = [] 72 | self.number_of_runs = 0 73 | 74 | 75 | def parse_info(): 76 | info_file = "info_about_bm.txt" 77 | # parse the file and create a vector of bms 78 | bms = [] 79 | load_obstacles = False 80 | load_queries = False 81 | with open(info_file) as f: 82 | bm = None 83 | for line in f: 84 | if len(line) == 0: 85 | continue 86 | if line[0] == '=': 87 | if bm is not None: 88 | bms.append(bm) 89 | bm = BM() 90 | elif line.startswith("Benchmark:"): 91 | bm.name = line.split(":")[1].strip() 92 | elif line.startswith("Num. of runs:"): 93 | bm.number_of_runs = int(line.split(":")[1].strip()) 94 | elif line.startswith("OBSTACLES"): 95 | load_obstacles = True 96 | load_queries = False 97 | elif line.startswith("QUERIES"): 98 | load_obstacles = False 99 | load_queries = True 100 | elif line.startswith("Num."): 101 | pass 102 | else: 103 | # should be either obstacles or queries 104 | if load_obstacles: 105 | x, y, z, r, vx, vy, vz = line.split(",") 106 | bm.obstacles.append(Obstacle(float(x), float( 107 | y), float(z), float(r) - ROBOT_SIZE, float(vx), float(vy), float(vz))) 108 | elif load_queries: 109 | x, y, z, x2, y2, z2 = line.split(",") 110 | bm.start.append([float(x), float(y), float(z)]) 111 | bm.goal.append([float(x2), float(y2), float(z2)]) 112 | bms.append(bm) 113 | return bms[0] # only parse first bm 114 | 115 | 116 | def get_path_for_time(path, time): 117 | """ 118 | the path is like [[x1,y1,z1, t1], [x2,y2,z2, t2], ...] 119 | we have to search for the time which is highger, then 120 | interpolate the last segment linearly and return the new path (new start + end of input path) 121 | """ 122 | if len(path) == 0: 123 | return [], [] 124 | for i, segment in enumerate(path): 125 | if len(segment) < 3: 126 | return [], [] 127 | if segment[3] > time: 128 | break 129 | 130 | # the segment is i-1 in which we currently are 131 | if i == 0: 132 | print("time is smaller than first segment") 133 | return path[0], path 134 | 135 | if i == len(path)-1: 136 | print("time is bigger than last segment") 137 | return path, path[-1] 138 | 139 | # interpolate 140 | x1, y1, z1, t1 = path[i-1] 141 | x2, y2, z2, t2 = path[i] 142 | t = (time - t1) / (t2 - t1) 143 | x = x1 + t * (x2 - x1) 144 | y = y1 + t * (y2 - y1) 145 | z = z1 + t * (z2 - z1) 146 | ret_a = [] 147 | ret_b = [] 148 | ret_a.extend(path[:i]) 149 | ret_a.append([x, y, z, time]) 150 | 151 | ret_b.append([x, y, z, time]) 152 | ret_b.extend(path[i:]) 153 | 154 | return ret_a, ret_b 155 | 156 | 157 | def compute_time(x1, y1, z1, x2, y2, z2, speed=0.75): 158 | # compute the norm of the vector 159 | length = np.sqrt((x2-x1)**2 + (y2-y1)**2 + (z2-z1)**2) 160 | return length / speed 161 | 162 | 163 | def read_path(filename): 164 | data = open(filename).read() 165 | data = data.split("\n") 166 | data = [line.split(" ") for line in data] 167 | ret = [] 168 | total_time = 0. 169 | floats = [] 170 | for line in data: 171 | if len(line) == 0 or len(line) == 1: 172 | continue 173 | if len(line) < 6: 174 | print(f"Error parsing {filename} at line {line}") 175 | floats = [float(x) for x in line] 176 | time = compute_time(*floats[:6]) 177 | ret.append([*floats[:3], total_time]) 178 | 179 | total_time += time 180 | # add last three points 181 | ret.append([*floats[3:6], total_time]) 182 | return ret 183 | 184 | 185 | def find_all_intermediate_paths_for(planner): 186 | # get all files in the current directory 187 | files = os.listdir() 188 | # find all files with the format: 189 | # INTERMEDIATE_PATH_{planner}_AT_{time}.txt 190 | # and return the time 191 | time = [float(x.split("_")[-1].split(".txt")[0]) 192 | for x in files if x.startswith("INTERMEDIATE_PATH_" + planner)] 193 | time.sort() 194 | ret = [] 195 | for t in time: 196 | path = read_path(f"INTERMEDIATE_PATH_{planner}_AT_{t:.6f}.txt") 197 | ret.append([t, path]) 198 | ret.append([1000, []]) 199 | return ret 200 | 201 | 202 | full_tprm = read_path("path_T-PRM_0_0_0.path") 203 | full_ompl_rrt = read_path("path_OMPL RRTstar_0_0_0.path") 204 | full_ompl_prm = read_path("path_OMPL PRM_0_0_0.path") 205 | 206 | intermediate_tprm = find_all_intermediate_paths_for("T-PRM A-star") 207 | intermediate_ompl_rrt = find_all_intermediate_paths_for("OMPL RRTstar") 208 | intermediate_ompl_prm = find_all_intermediate_paths_for("OMPL PRM") 209 | 210 | bm = parse_info() 211 | 212 | # 3D 213 | fig, ax = plt.subplots(subplot_kw=dict(projection='3d'), figsize=(8, 8)) 214 | (ln,) = plt.plot([], [], "r") 215 | 216 | colors = { 217 | "tprm": "g", 218 | "ompl_rrt": "y", 219 | "ompl_prm": "b" 220 | } 221 | 222 | u = np.linspace(0, 2 * np.pi, 100) 223 | v = np.linspace(0, np.pi, 100) 224 | sphere_x = np.outer(np.cos(u), np.sin(v)) 225 | sphere_y = np.outer(np.sin(u), np.sin(v)) 226 | sphere_z = np.outer(np.ones(np.size(u)), np.cos(v)) 227 | 228 | def draw_sphere(x_, y_, z_, r, color): 229 | x = x_ + sphere_x * r 230 | y = y_ + sphere_y * r 231 | z = z_ + sphere_z * r 232 | elev = 10.0 233 | rot = 80.0 / 180 * np.pi 234 | ax.plot_surface(x, y, z, rstride=4, cstride=4, 235 | color=color, linewidth=0, alpha=0.5) 236 | 237 | 238 | def draw_path(path, ax, color, label, alpha=1., addLabel=True): 239 | try: 240 | if len(path) == 0 or len(path[0]) < 3: 241 | return 242 | # convert to a np array 243 | path = np.array(path) 244 | # draw the path 245 | if addLabel: 246 | ax.plot(path[:, 0], path[:, 1], path[:, 2], 247 | color, label=label, alpha=alpha) 248 | else: 249 | ax.plot(path[:, 0], path[:, 1], path[:, 2], color, alpha=alpha) 250 | except: 251 | pass 252 | 253 | 254 | def draw_robot(path, ax, color): 255 | try: 256 | if len(path) == 0 or len(path[0]) < 3: 257 | return 258 | 259 | # check if the path is outwards of [0,10] 260 | if path[0][0] > 10 or path[0][1] > 10 or path[0][2] > 10: 261 | pass 262 | 263 | draw_sphere(path[0][0], path[0][1], path[0][2], ROBOT_SIZE, color) 264 | except: 265 | pass 266 | 267 | def draw_intermediate_paths(i_paths, time, ax, color): 268 | return # TODO: remove this line if you want to draw the paths yet to be driven 269 | if time == 0: 270 | draw_path(i_paths[0][1], ax, color, "", 0.5, False) 271 | return 272 | 273 | # search the entry in i_paths which is lower than time 274 | for i, (t, path) in enumerate(i_paths): 275 | if t >= time: 276 | break 277 | 278 | if i == 0: 279 | return 280 | # draw the path 281 | _, path = get_path_for_time(i_paths[i-1][1], time - i_paths[i-1][0]) 282 | draw_path(path, ax, color, f"intermediate path {i}", 0.5, False) 283 | 284 | 285 | def init_anim(): 286 | return (ln,) 287 | 288 | 289 | def update(frame): 290 | ax.clear() 291 | ax.set_xlim(0, 10) 292 | ax.set_ylim(0, 10) 293 | ax.set_zlim(0, 10) 294 | ax.set_aspect('auto') 295 | ax.set_xticks([]) 296 | ax.set_yticks([]) 297 | ax.set_zticks([]) 298 | 299 | # draw obstacles 300 | for obstacle in bm.obstacles: 301 | pos_x = obstacle.x + frame * DT * obstacle.vx 302 | pos_y = obstacle.y + frame * DT * obstacle.vy 303 | pos_z = obstacle.z + frame * DT * obstacle.vz 304 | # check if any coordinate is out of [0,10], if so, don't draw it 305 | if pos_x < 0 or pos_x > 10 or pos_y < 0 or pos_y > 10 or pos_z < 0 or pos_z > 10: 306 | continue 307 | 308 | draw_sphere(pos_x, pos_y, pos_z, obstacle.r, "gray") 309 | 310 | # T-PRM 311 | path_a, path_b = get_path_for_time(full_tprm, frame * DT) 312 | draw_path(path_a, ax, colors["tprm"], "T-PRM") 313 | #draw_path(path_b, ax, colors["tprm"], "T-PRM", 0.5, False) 314 | draw_robot(path_b, ax, colors["tprm"]) 315 | 316 | # OMPL PRM 317 | path_a, path_b = get_path_for_time(full_ompl_prm, frame * DT) 318 | draw_path(path_a, ax, colors["ompl_prm"], "OMPL PRM") 319 | draw_robot(path_b, ax, colors["ompl_prm"]) 320 | draw_intermediate_paths(intermediate_ompl_prm, 321 | frame * DT, ax, colors["ompl_prm"]) 322 | 323 | # OMPL RRT 324 | path_a, path_b = get_path_for_time(full_ompl_rrt, frame * DT) 325 | draw_path(path_a, ax, colors["ompl_rrt"], "OMPL RRT*") 326 | draw_robot(path_b, ax, colors["ompl_rrt"]) 327 | draw_intermediate_paths(intermediate_ompl_rrt, 328 | frame * DT, ax, colors["ompl_rrt"]) 329 | 330 | # show legend 331 | ax.legend(loc='upper left') 332 | ax.set_title(f"{frame * DT:.2f}s") 333 | 334 | plt.tight_layout() 335 | print("Done Frame %d" % frame) 336 | return (ln,) 337 | 338 | 339 | ani = FuncAnimation(fig, update, frames=range( 340 | int(MAX_TIME / DT)), init_func=init_anim, blit=False) 341 | plt.show() 342 | #ani.save("movie_3D.mp4", writer="ffmpeg", fps=FRAMES_PER_SECOND) 343 | -------------------------------------------------------------------------------- /benchmarking/src/OMPLPlanner.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | 50 | //#define OMPL_USE_NUM_NODES 51 | #define OMPL_USE_EDGE_LENGTH 52 | 53 | namespace ompl::geometric { 54 | 55 | class MyPRM : public PRM { 56 | public: 57 | MyPRM(ompl::base::SpaceInformationPtr si, double edgeThreshold) : PRM(si), m_edgeThreshold(edgeThreshold) { 58 | setConnectionFilter(std::bind(&MyPRM::connectionFilter, this, std::placeholders::_1, std::placeholders::_2)); 59 | } 60 | 61 | private: 62 | bool connectionFilter(const Vertex& v1, const Vertex& v2) const { 63 | return PRM::distanceFunction(v1, v2) < m_edgeThreshold; 64 | } 65 | double m_edgeThreshold; 66 | }; 67 | } // namespace ompl::geometric 68 | 69 | namespace benchmarking { 70 | 71 | class ValidityChecker : public ompl::base::StateValidityChecker { 72 | public: 73 | ValidityChecker(const ompl::base::SpaceInformationPtr& si, std::shared_ptr bm) : ompl::base::StateValidityChecker(si), benchmark(bm) {} 74 | 75 | // Returns whether the given state's position overlaps the 76 | // circular obstacle 77 | bool isValid(const ompl::base::State* state) const { 78 | return this->clearance(state) > 0.0; 79 | } 80 | 81 | // Returns the distance from the given state's position to the 82 | // boundary of the circular obstacle. 83 | double clearance(const ompl::base::State* state) const { 84 | // We know we're working with a RealVectorStateSpace in this 85 | // example, so we downcast state into the specific type. 86 | const ompl::base::RealVectorStateSpace::StateType* state2D = state->as(); 87 | 88 | double min_dist = 1e4; 89 | if (benchmark->is_2d) { 90 | double x = state2D->values[0]; 91 | double y = state2D->values[1]; 92 | Eigen::Vector2d pos(x, y); 93 | double min_dist = 1e4; 94 | for (const benchmarking::Circle& c : benchmark->circles) { 95 | // get minimum distance to circle 96 | double dist = (pos - c.center.head(2)).norm() - c.radius; 97 | if (dist < min_dist) 98 | min_dist = dist; 99 | } 100 | } else { // 3D case 101 | // Extract the robot's (x,y) position from its state 102 | double x = state2D->values[0]; 103 | double y = state2D->values[1]; 104 | double z = state2D->values[2]; 105 | Eigen::Vector3d pos(x, y, z); 106 | 107 | // static obstacles: 108 | double min_dist = 1e4; 109 | for (const benchmarking::Circle& c : benchmark->circles) { 110 | // get minimum distance to circle 111 | double dist = (pos - c.center).norm() - c.radius; 112 | if (dist < min_dist) 113 | min_dist = dist; 114 | } 115 | } 116 | 117 | return min_dist; 118 | } 119 | 120 | private: 121 | std::shared_ptr benchmark; 122 | }; 123 | 124 | class ValidityCheckerDynObst : public ompl::base::StateValidityChecker { 125 | public: 126 | ValidityCheckerDynObst(const ompl::base::SpaceInformationPtr& si, std::shared_ptr bm, double time) 127 | : ompl::base::StateValidityChecker(si), benchmark(bm), m_time(time) {} 128 | 129 | // Returns whether the given state's position overlaps the 130 | // circular obstacle 131 | bool isValid(const ompl::base::State* state) const { 132 | return this->clearance(state) > 0.0; 133 | } 134 | 135 | void set_time(double time) { 136 | m_time = time; 137 | } 138 | 139 | // Returns the distance from the given state's position to the 140 | // boundary of the circular obstacle. 141 | double clearance(const ompl::base::State* state) const { 142 | // We know we're working with a RealVectorStateSpace in this 143 | // example, so we downcast state into the specific type. 144 | const ompl::base::RealVectorStateSpace::StateType* state2D = state->as(); 145 | 146 | double min_dist = 1e4; 147 | if (benchmark->is_2d) { 148 | // Extract the robot's (x,y) position from its state 149 | double x = state2D->values[0]; 150 | double y = state2D->values[1]; 151 | Eigen::Vector2d pos(x, y); 152 | 153 | // static obstacles: 154 | 155 | for (const benchmarking::Circle& c : benchmark->circles) { 156 | // get minimum distance to circle 157 | double dist = (pos - c.center.head(2)).norm() - c.radius; 158 | if (dist < min_dist) 159 | min_dist = dist; 160 | } 161 | 162 | for (const benchmarking::MovingCircle& mc : benchmark->moving_circles) { 163 | Eigen::Vector3d pos_mc = mc.center + mc.velocity * m_time; 164 | double dist = (pos - pos_mc.head(2)).norm() - mc.radius; 165 | if (dist < min_dist) 166 | min_dist = dist; 167 | } 168 | } else { // 3D 169 | // Extract the robot's (x,y) position from its state 170 | double x = state2D->values[0]; 171 | double y = state2D->values[1]; 172 | double z = state2D->values[2]; 173 | Eigen::Vector3d pos(x, y, z); 174 | 175 | // static obstacles: 176 | 177 | for (const benchmarking::Circle& c : benchmark->circles) { 178 | // get minimum distance to circle 179 | double dist = (pos - c.center).norm() - c.radius; 180 | if (dist < min_dist) 181 | min_dist = dist; 182 | } 183 | 184 | for (const benchmarking::MovingCircle& mc : benchmark->moving_circles) { 185 | Eigen::Vector3d pos_mc = mc.center + mc.velocity * m_time; 186 | double dist = (pos - pos_mc).norm() - mc.radius; 187 | if (dist < min_dist) 188 | min_dist = dist; 189 | } 190 | } 191 | 192 | return min_dist; 193 | } 194 | 195 | private: 196 | std::shared_ptr benchmark; 197 | double m_time; 198 | }; 199 | 200 | OMPLPlannerBenchmark::OMPLPlannerBenchmark(std::string planner) : m_planner(planner) { 201 | ompl::msg::setLogLevel(ompl::msg::LogLevel::LOG_NONE); 202 | } 203 | 204 | ompl::base::OptimizationObjectivePtr getThresholdPathLengthObj(const ompl::base::SpaceInformationPtr& si, double cost) { 205 | ompl::base::OptimizationObjectivePtr obj(new ompl::base::PathLengthOptimizationObjective(si)); 206 | obj->setCostThreshold(ompl::base::Cost(cost)); 207 | return obj; 208 | } 209 | 210 | void write_intermediate_path(std::shared_ptr benchmark, const std::vector& path, double time, std::string name) { 211 | if (!benchmark->writeMovingIntermediatePaths) 212 | return; 213 | 214 | std::ofstream of("INTERMEDIATE_PATH_" + name + "_AT_" + std::to_string(time) + ".txt"); 215 | if (path.size() == 0) { 216 | return; 217 | } 218 | for (int i = 0; i < path.size() - 1; i++) { 219 | of << path[i].x() << " " << path[i].y() << " " << path[i].z() << " " << path[i + 1].x() << " " << path[i + 1].y() << " " << path[i + 1].z() 220 | << std::endl; 221 | } 222 | } 223 | 224 | BenchmarkResult OMPLPlannerBenchmark::runBenchmark(std::shared_ptr benchmark, int benchmark_idx, int run_idx) { 225 | if (benchmark->moving_circles.size() == 0) { 226 | BasePlannerBenchmark::startBenchmark(); 227 | 228 | BenchmarkResult result; 229 | 230 | ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(2 + !benchmark->is_2d)); 231 | space->as()->setBounds(0, benchmark->domain_size); 232 | 233 | ompl::base::SpaceInformationPtr si(new ompl::base::SpaceInformation(space)); 234 | 235 | ValidityChecker* validityChecker = new ValidityChecker(si, benchmark); 236 | 237 | si->setStateValidityChecker(ompl::base::StateValidityCheckerPtr(validityChecker)); 238 | si->setup(); 239 | 240 | ompl::base::ProblemDefinitionPtr pdef(new ompl::base::ProblemDefinition(si)); 241 | pdef->setOptimizationObjective(getThresholdPathLengthObj(si, benchmark->ompl_path_length_threshold)); 242 | 243 | ompl::base::PlannerPtr planner; 244 | if (m_planner == "RRTstar") { 245 | planner = std::make_shared(si); 246 | dynamic_cast(planner.get())->setRange(benchmark->ompl_edge_length); 247 | } else if (m_planner == "PRM") { 248 | #ifdef OMPL_USE_EDGE_LENGTH 249 | planner = std::make_shared(si, benchmark->ompl_edge_length); 250 | #else 251 | planner = std::make_shared(si); 252 | #endif 253 | } else if (m_planner != "") { 254 | std::cerr << "Unknown planner: " << m_planner << std::endl; 255 | exit(1); 256 | } 257 | ompl::base::ScopedState<> start(space); 258 | ompl::base::ScopedState<> goal(space); 259 | for (size_t query_idx = 0; query_idx < benchmark->start.size(); query_idx++) { 260 | start->as()->values[0] = benchmark->start[query_idx].x(); 261 | start->as()->values[1] = benchmark->start[query_idx].y(); 262 | if (!benchmark->is_2d) 263 | start->as()->values[2] = benchmark->start[query_idx].z(); 264 | 265 | goal->as()->values[0] = benchmark->goal[query_idx].x(); 266 | goal->as()->values[1] = benchmark->goal[query_idx].y(); 267 | if (!benchmark->is_2d) 268 | goal->as()->values[2] = benchmark->goal[query_idx].z(); 269 | 270 | if (query_idx > 0) { 271 | pdef->clearSolutionPaths(); 272 | pdef->clearStartStates(); 273 | pdef->clearGoal(); 274 | 275 | if (m_planner == "PRM") { 276 | // cast planner 277 | static_cast(planner.get())->clearQuery(); 278 | } else if ( 279 | 280 | m_planner == "RRTstar") { 281 | static_cast(planner.get())->clear(); 282 | } 283 | } 284 | pdef->setStartAndGoalStates(start, goal); 285 | planner->setProblemDefinition(pdef); 286 | planner->setup(); 287 | 288 | startMeasurement(); 289 | #ifndef OMPL_USE_NUM_NODES 290 | ompl::base::PlannerStatus status = planner->solve(benchmark->ompl_time_limit); 291 | #else 292 | // if we want to limit the number of nodes: (hacky!) 293 | ompl::base::PlannerStatus status; 294 | if (m_planner == "PRM") { 295 | status = planner->solve(ompl::base::PlannerTerminationCondition([&]() -> bool { 296 | // return true if we should stop 297 | auto planner_ = static_cast(planner.get()); 298 | return planner_->milestoneCount() >= benchmark->numNodes; 299 | })); 300 | } else { 301 | status = planner->solve(ompl::base::PlannerTerminationCondition([&]() -> bool { 302 | // return true if we should stop 303 | auto planner_ = static_cast(planner.get()); 304 | return planner_->numIterations() >= benchmark->numNodes; 305 | })); 306 | } 307 | #endif 308 | result.success.push_back(status == ompl::base::PlannerStatus::EXACT_SOLUTION); 309 | 310 | result.timing_results.push_back(stopMeasurement("Solve query " + std::to_string(query_idx))); 311 | 312 | ompl::base::PathPtr path = pdef->getSolutionPath(); 313 | if (path != nullptr) { 314 | ompl::geometric::PathGeometric& pathGeometric = dynamic_cast(*path); 315 | auto states = pathGeometric.getStates(); 316 | result.path.push_back({}); 317 | for (auto state : states) { 318 | if (!benchmark->is_2d) { 319 | result.path.back().push_back(Eigen::Vector3d(state->as()->values[0], 320 | state->as()->values[1], 321 | state->as()->values[2])); 322 | } else { // pad with 0 323 | result.path.back().push_back(Eigen::Vector3d(state->as()->values[0], 324 | state->as()->values[1], 0.)); 325 | } 326 | } 327 | } 328 | } 329 | 330 | BasePlannerBenchmark::stopBenchmark(); 331 | 332 | result.description = "OMPL " + m_planner + " - " + benchmark->name; 333 | result.duration_micros = getTotalDuration(); 334 | 335 | return result; 336 | } else { 337 | // moving circles --> run OMPL iteratively 338 | BasePlannerBenchmark::startBenchmark(); 339 | 340 | BenchmarkResult result; 341 | 342 | ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(2 + !benchmark->is_2d)); 343 | space->as()->setBounds(0, benchmark->domain_size); 344 | 345 | ompl::base::SpaceInformationPtr si(new ompl::base::SpaceInformation(space)); 346 | 347 | ompl::base::StateValidityCheckerPtr validityChecker = std::make_shared(si, benchmark, 0.); 348 | si->setStateValidityChecker(validityChecker); 349 | 350 | si->setup(); 351 | ompl::base::ProblemDefinitionPtr pdef(new ompl::base::ProblemDefinition(si)); 352 | pdef->setOptimizationObjective(getThresholdPathLengthObj(si, benchmark->ompl_path_length_threshold)); 353 | 354 | ompl::base::PlannerPtr planner; 355 | if (m_planner == "RRTstar") { 356 | planner = std::make_shared(si); 357 | dynamic_cast(planner.get())->setRange(benchmark->ompl_edge_length); 358 | } else if (m_planner == "PRM") { 359 | #ifdef OMPL_USE_EDGE_LENGTH 360 | planner = std::make_shared(si, benchmark->ompl_edge_length); 361 | #else 362 | planner = std::make_shared(si); 363 | #endif 364 | } else if (m_planner != "") { 365 | std::cerr << "Unknown planner: " << m_planner << std::endl; 366 | exit(1); 367 | } 368 | 369 | ompl::base::ScopedState<> start(space); 370 | ompl::base::ScopedState<> goal(space); 371 | start->as()->values[0] = benchmark->start[0].x(); 372 | start->as()->values[1] = benchmark->start[0].y(); 373 | if (!benchmark->is_2d) 374 | start->as()->values[2] = benchmark->start[0].z(); 375 | 376 | goal->as()->values[0] = benchmark->goal[0].x(); 377 | goal->as()->values[1] = benchmark->goal[0].y(); 378 | if (!benchmark->is_2d) 379 | goal->as()->values[2] = benchmark->goal[0].z(); 380 | 381 | pdef->setStartAndGoalStates(start, goal); 382 | planner->setProblemDefinition(pdef); 383 | planner->setup(); 384 | 385 | startMeasurement(); 386 | 387 | ompl::base::PlannerStatus status = planner->solve(benchmark->ompl_time_limit); 388 | result.success.push_back(status == ompl::base::PlannerStatus::EXACT_SOLUTION); 389 | 390 | result.timing_results.push_back(stopMeasurement("Solve query " + std::to_string(0))); 391 | 392 | ompl::base::PathPtr path = pdef->getSolutionPath(); 393 | std::vector path_; 394 | if (path != nullptr) { 395 | ompl::geometric::PathGeometric& pathGeometric = dynamic_cast(*path); 396 | auto states = pathGeometric.getStates(); 397 | 398 | for (auto state : states) { 399 | if (!benchmark->is_2d) { 400 | path_.push_back(Eigen::Vector3d(state->as()->values[0], 401 | state->as()->values[1], 402 | state->as()->values[2])); 403 | } else { // pad with 0 404 | path_.push_back(Eigen::Vector3d(state->as()->values[0], 405 | state->as()->values[1], 0.)); 406 | } 407 | } 408 | } 409 | 410 | write_intermediate_path(benchmark, path_, 0., OMPLPlannerBenchmark::getName()); 411 | 412 | double total_time = 0.; 413 | bool has_reached_end = false; 414 | std::vector full_path; 415 | bool in_collision = false; 416 | full_path.push_back(benchmark->start[0]); 417 | while (!has_reached_end) { 418 | in_collision = false; // reset 419 | double time_on_current_path = 0.; 420 | 421 | if (path_.size() < 2) { 422 | /*std::cerr << "Path is too short (" << path_.size() << ")" << std::endl; 423 | for (auto& s : path_) { 424 | std::cerr << s.transpose() << std::endl; 425 | } 426 | std::cerr << "Current start: " << start->as()->values[0] << " " 427 | << start->as()->values[1] << " "; 428 | 429 | if (!benchmark->is_2d) 430 | std::cerr << start->as()->values[2] << std::endl; 431 | */ 432 | BasePlannerBenchmark::stopBenchmark(); 433 | result.description = "OMPL " + m_planner + " - " + benchmark->name; 434 | result.duration_micros = getTotalDuration(); 435 | result.success.back() = false; 436 | return result; 437 | } 438 | 439 | // search the current segment to use 440 | int current_segment = 0; 441 | //full_path.push_back(path_.at(current_segment)); 442 | //std::cout << "Full path push back A: " << full_path.back().transpose() << std::endl; 443 | Eigen::Vector3d start_; 444 | double MOVE_FORWARD_TIME = 0.25; 445 | double time_on_segment = 0.; 446 | while (!in_collision) { 447 | while (current_segment < path_.size() - 1) { 448 | double time_for_segment = (path_.at(current_segment + 1) - path_.at(current_segment)).norm() / 0.75; 449 | 450 | double move_time = std::min(MOVE_FORWARD_TIME, time_for_segment - time_on_segment); 451 | time_on_current_path += move_time; 452 | time_on_segment += move_time; 453 | 454 | if (time_on_segment >= time_for_segment) { 455 | // we have reached the end of the segment 456 | current_segment++; 457 | time_on_segment = 0.; 458 | if (current_segment == path_.size() - 1) { 459 | has_reached_end = true; 460 | break; 461 | } 462 | full_path.push_back(path_.at(current_segment)); 463 | start_ = path_.at(current_segment); // update start 464 | } else { 465 | // we are still on the same segment 466 | start_ = path_.at(current_segment) + (path_.at(current_segment + 1) - path_.at(current_segment)) * (time_on_segment / time_for_segment); 467 | full_path.push_back(start_); 468 | } 469 | 470 | // COLLISION CHECK 471 | auto path = pdef->getSolutionPath(); 472 | if (path != nullptr) { 473 | ompl::geometric::PathGeometric& pathGeometric = dynamic_cast(*path); 474 | auto states = pathGeometric.getStates(); 475 | ValidityCheckerDynObst val_checker(si, benchmark, total_time + time_on_current_path); 476 | for (auto state : states) { 477 | if (!val_checker.isValid(state)) { 478 | in_collision = true; 479 | // this means, we have to replan 480 | break; 481 | } 482 | } 483 | 484 | if (!in_collision) { 485 | //std::cout << "Can continue with time " << time_on_current_path << " on segment " << current_segment << " with time " 486 | // << time_for_segment << std::endl; 487 | } else { 488 | //std::cout << "Collision detected at time " << time_on_current_path << std::endl; 489 | break; 490 | } 491 | } 492 | } 493 | if (has_reached_end) { 494 | // propagate break 495 | break; 496 | } 497 | } 498 | if (has_reached_end) { 499 | // propagate break 500 | break; 501 | } 502 | 503 | //std::cout << "Replanning at time " << total_time + time_on_current_path << std::endl; 504 | 505 | static_cast(validityChecker.get())->set_time(total_time + time_on_current_path); 506 | 507 | start->as()->values[0] = start_.x(); 508 | start->as()->values[1] = start_.y(); 509 | if (!benchmark->is_2d) 510 | start->as()->values[2] = start_.z(); 511 | 512 | // goal is the same 513 | 514 | si = std::make_shared(space); 515 | si->setStateValidityChecker(validityChecker); 516 | si->setup(); 517 | pdef = std::make_shared(si); 518 | pdef->setOptimizationObjective(getThresholdPathLengthObj(si, benchmark->ompl_path_length_threshold)); 519 | if (m_planner == "RRTstar") { 520 | planner = std::make_shared(si); 521 | dynamic_cast(planner.get())->setRange(benchmark->ompl_edge_length); 522 | } else if (m_planner == "PRM") { 523 | #ifdef OMPL_USE_EDGE_LENGTH 524 | planner = std::make_shared(si, benchmark->ompl_edge_length); 525 | #else 526 | 527 | planner = std::make_shared(si); 528 | #endif 529 | } else if (m_planner != "") { 530 | std::cerr << "Unknown planner: " << m_planner << std::endl; 531 | exit(1); 532 | } 533 | pdef->setStartAndGoalStates(start, goal); 534 | planner->setProblemDefinition(pdef); 535 | planner->setup(); 536 | 537 | status = planner->solve(benchmark->ompl_time_limit); 538 | 539 | // and-ing success 540 | result.success.back() = result.success.back() & (status == ompl::base::PlannerStatus::EXACT_SOLUTION); 541 | path = pdef->getSolutionPath(); 542 | path_.clear(); 543 | if (path != nullptr) { 544 | ompl::geometric::PathGeometric& pathGeometric = dynamic_cast(*path); 545 | auto states = pathGeometric.getStates(); 546 | 547 | for (auto state : states) { 548 | if (!benchmark->is_2d) { 549 | path_.push_back(Eigen::Vector3d(state->as()->values[0], 550 | state->as()->values[1], 551 | state->as()->values[2])); 552 | } else { // pad with 0. 553 | path_.push_back(Eigen::Vector3d(state->as()->values[0], 554 | state->as()->values[1], 0.)); 555 | } 556 | } 557 | } 558 | 559 | total_time += time_on_current_path; 560 | write_intermediate_path(benchmark, path_, total_time, OMPLPlannerBenchmark::getName()); 561 | 562 | // reached end if start is close to goal 1e-3 563 | if (!benchmark->is_2d) { 564 | Eigen::Vector3d e_start(start->as()->values[0], 565 | start->as()->values[1], 566 | start->as()->values[2]); 567 | Eigen::Vector3d e_goal(goal->as()->values[0], 568 | goal->as()->values[1], 569 | goal->as()->values[2]); 570 | has_reached_end = (e_start - e_goal).norm() < 1e-3; 571 | } else { 572 | Eigen::Vector2d e_start(start->as()->values[0], 573 | start->as()->values[1]); 574 | Eigen::Vector2d e_goal(goal->as()->values[0], 575 | goal->as()->values[1]); 576 | has_reached_end = (e_start - e_goal).norm() < 1e-3; 577 | } 578 | } 579 | 580 | // push back goal 581 | full_path.push_back(benchmark->goal[0]); 582 | 583 | result.path.push_back(full_path); 584 | 585 | BasePlannerBenchmark::stopBenchmark(); 586 | 587 | result.description = "OMPL " + m_planner + " - " + benchmark->name; 588 | result.duration_micros = getTotalDuration(); 589 | 590 | return result; 591 | } 592 | } 593 | 594 | } // namespace benchmarking 595 | -------------------------------------------------------------------------------- /benchmarking/src/TPRMPlanner.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | namespace benchmarking { 47 | 48 | // path (pos x, pos y), time (t) 49 | std::pair, std::vector> TPRMPlannerBenchmark::computePath(const std::vector& path) const { 50 | std::vector path_; 51 | std::vector times; 52 | for (size_t i = 0; i < path.size(); ++i) { 53 | path_.push_back(path[i].position); 54 | times.push_back(path[i].time); 55 | } 56 | 57 | return {path_, times}; 58 | } 59 | 60 | BenchmarkResult TPRMPlannerBenchmark::runBenchmark(std::shared_ptr benchmark, int benchmark_idx, int run_idx) { 61 | BasePlannerBenchmark::startBenchmark(); 62 | 63 | using namespace tprm; 64 | 65 | BenchmarkResult result; 66 | 67 | TemporalPRM tprm; 68 | 69 | Vector3d roomMax; 70 | if (benchmark->is_2d) { 71 | roomMax = Vector3d(benchmark->domain_size, benchmark->domain_size, 0.); 72 | } else { 73 | roomMax = Vector3d::Constant(benchmark->domain_size); 74 | } 75 | 76 | tprm.setEnvironmentMin(Vector3d::Constant(0.)); 77 | tprm.setEnvironmentMax(roomMax); 78 | 79 | for (const auto& c : benchmark->circles) { 80 | tprm.addStaticObstacle(std::make_shared(c.center, c.radius)); 81 | } 82 | for (const auto& c : benchmark->moving_circles) { 83 | tprm.addDynamicObstacle(std::make_shared(c.center, c.velocity, c.radius)); 84 | } 85 | 86 | startMeasurement(); 87 | tprm.placeSamples(benchmark->numNodes); 88 | result.timing_results.push_back(stopMeasurement("Place Samples")); 89 | 90 | startMeasurement(); 91 | tprm.buildPRM(benchmark->tprm_cost_edge_threshold); 92 | result.timing_results.push_back(stopMeasurement("Build PRM")); 93 | 94 | for (size_t query_idx = 0; query_idx < benchmark->start.size(); query_idx++) { 95 | startMeasurement(); 96 | auto path = tprm.getShortestPath(benchmark->start[query_idx], benchmark->goal[query_idx]); 97 | result.timing_results.push_back(stopMeasurement("A* " + std::to_string(query_idx))); 98 | 99 | result.success.push_back(path.size() > 0); 100 | 101 | auto [path_pos, path_time] = computePath(path); 102 | result.path.push_back(path_pos); 103 | result.path_durations.push_back(path_time); 104 | } 105 | 106 | BasePlannerBenchmark::stopBenchmark(); 107 | result.description = "T-PRM - " + benchmark->name; 108 | result.duration_micros = getTotalDuration(); 109 | 110 | return result; 111 | } 112 | 113 | } /* namespace benchmarking */ -------------------------------------------------------------------------------- /benchmarking/src/base.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | namespace benchmarking { 42 | 43 | BasePlannerBenchmark::BasePlannerBenchmark() {} 44 | 45 | void BasePlannerBenchmark::startMeasurement() { 46 | m_start_time = std::chrono::high_resolution_clock::now(); 47 | } 48 | 49 | BenchmarkResult BasePlannerBenchmark::runBenchmark(std::shared_ptr, int, int) { 50 | return BenchmarkResult(); 51 | }; 52 | 53 | void BasePlannerBenchmark::startBenchmark() { 54 | m_total_start_time = std::chrono::high_resolution_clock::now(); 55 | } 56 | 57 | void BasePlannerBenchmark::stopBenchmark() { 58 | m_total_duration = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - m_total_start_time).count(); 59 | } 60 | 61 | TimingResult BasePlannerBenchmark::stopMeasurement(std::string what) { 62 | auto end_time = std::chrono::high_resolution_clock::now(); 63 | double duration_micros = std::chrono::duration_cast(end_time - m_start_time).count(); 64 | TimingResult tr; 65 | tr.description = what; 66 | tr.duration_micros = duration_micros; 67 | return tr; 68 | } 69 | 70 | double BasePlannerBenchmark::getTotalDuration() const { 71 | return m_total_duration; 72 | } 73 | 74 | } // namespace benchmarking 75 | -------------------------------------------------------------------------------- /benchmarking/src/benchmark.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | namespace benchmarking { 42 | 43 | Benchmark::Benchmark(const std::string& name, int num_runs) : name(name), m_num_runs(num_runs) {} 44 | Benchmark::~Benchmark() {} 45 | 46 | } // namespace benchmarking -------------------------------------------------------------------------------- /benchmarking/src/factory.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | #include 42 | 43 | namespace benchmarking { 44 | 45 | BenchmarkFactory::BenchmarkFactory() {} 46 | 47 | BenchmarkFactory::~BenchmarkFactory() {} 48 | 49 | void BenchmarkFactory::run() { 50 | // Run all registered benchmarks for all planners 51 | for (size_t i = 0; i < m_benchmarks.size(); ++i) { 52 | current_benchmark_index = i; 53 | log("********************************************************"); 54 | log("Running benchmark: " + m_benchmarks[i]->name); 55 | 56 | for (size_t j = 0; j < m_planners.size(); ++j) { 57 | log("Running planner: " + m_planners[j]->getName()); 58 | run_benchmark(m_planners[j], m_benchmarks[i]); 59 | } 60 | } 61 | for (std::shared_ptr handler : m_result_handlers) { 62 | handler->printSummary(); 63 | } 64 | } 65 | 66 | void BenchmarkFactory::run_benchmark(std::shared_ptr planner, std::shared_ptr benchmark) { 67 | // Run the benchmark for the planner n times 68 | int n = benchmark->numRuns(); 69 | std::vector durations(n); 70 | std::vector success(n); 71 | 72 | // Tell the result handler which benchmark we use. 73 | for (std::shared_ptr handler : m_result_handlers) { 74 | handler->nextBenchmarkIs(benchmark); 75 | } 76 | 77 | for (int i = 0; i < n; ++i) { 78 | BenchmarkResult result = planner->runBenchmark(benchmark, current_benchmark_index, i); 79 | for (std::shared_ptr handler : m_result_handlers) { 80 | handler->handleResult(result, planner->getName(), current_benchmark_index, i); 81 | } 82 | } 83 | 84 | for (std::shared_ptr handler : m_result_handlers) { 85 | handler->printSummary(planner->getName(), current_benchmark_index); 86 | } 87 | } 88 | 89 | void BenchmarkFactory::register_planner(std::shared_ptr planner) { 90 | m_planners.push_back(planner); 91 | log("Registered planner '" + planner->getName() + "'"); 92 | } 93 | 94 | void BenchmarkFactory::register_benchmark(std::shared_ptr benchmark) { 95 | m_benchmarks.push_back(benchmark); 96 | log("Registered benchmark '" + benchmark->name + "'"); 97 | 98 | if (benchmark->is_2d) { 99 | for (auto& q : benchmark->start) { 100 | q.z() = 0.; 101 | } 102 | for (auto& q : benchmark->goal) { 103 | q.z() = 0.; 104 | } 105 | } 106 | } 107 | 108 | void BenchmarkFactory::register_benchmarks(std::vector> benchmarks) { 109 | for (std::shared_ptr benchmark : benchmarks) { 110 | register_benchmark(benchmark); 111 | } 112 | } 113 | 114 | void BenchmarkFactory::register_result_handler(std::shared_ptr handler) { 115 | m_result_handlers.push_back(handler); 116 | log("Registered result handler '" + handler->getName() + "'"); 117 | } 118 | 119 | void BenchmarkFactory::log(std::string message) const { 120 | log("Factory", message); 121 | } 122 | 123 | void BenchmarkFactory::log(std::string tag, std::string message) const { 124 | std::cout << "[" << tag << "] " << message << std::endl; 125 | } 126 | 127 | } // namespace benchmarking -------------------------------------------------------------------------------- /benchmarking/src/result_handler.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace benchmarking { 48 | 49 | void ResultHandlerPathLength::nextBenchmarkIs(std::shared_ptr benchmark) { 50 | m_current_benchmark = benchmark; 51 | } 52 | 53 | double std_dev(double mean, const std::vector& data) { 54 | double sum = 0; 55 | for (double d : data) { 56 | sum += (d - mean) * (d - mean); 57 | } 58 | return std::sqrt(sum / data.size()); 59 | } 60 | 61 | // Path Length 62 | void ResultHandlerPathLength::handleResult(const BenchmarkResult& result, std::string, int, int) { 63 | last_description = result.description; 64 | 65 | for (size_t query_idx = 0; query_idx < result.success.size(); ++query_idx) { 66 | if (!result.success[query_idx] || result.path[query_idx].size() == 0) 67 | return; 68 | 69 | double path_length = 0.0; 70 | path_length += (m_current_benchmark->start[query_idx] - result.path[query_idx][0]).norm(); 71 | for (size_t i = 1; i < result.path[query_idx].size(); ++i) { 72 | path_length += (result.path[query_idx][i - 1] - result.path[query_idx][i]).norm(); 73 | } 74 | path_length += (m_current_benchmark->goal[query_idx] - result.path[query_idx].back()).norm(); 75 | 76 | m_path_lengths_for_idx.push_back(path_length); 77 | } 78 | } 79 | 80 | void ResultHandlerPathLength::printSummary(std::string planner, int benchmark_idx) { 81 | // Print the average, min and max path length 82 | if (m_path_lengths_for_idx.size() == 0) { 83 | std::cout << "Path length: not any valid measurements." << std::endl; 84 | return; 85 | } 86 | std::cout << "Path length: " << std::endl; 87 | double avg = std::accumulate(m_path_lengths_for_idx.begin(), m_path_lengths_for_idx.end(), 0.0) / m_path_lengths_for_idx.size(); 88 | std::cout << " Average: " << avg << std::endl; 89 | std::cout << " Std. Dev: " << std_dev(avg, m_path_lengths_for_idx) << std::endl; 90 | std::cout << " Min: " << *std::min_element(m_path_lengths_for_idx.begin(), m_path_lengths_for_idx.end()) << std::endl; 91 | std::cout << " Max: " << *std::max_element(m_path_lengths_for_idx.begin(), m_path_lengths_for_idx.end()) << std::endl; 92 | std::cout << " " << m_path_lengths_for_idx.size() << " measurements." << std::endl; 93 | 94 | m_file << last_description << ", " << benchmark_idx << ", "; 95 | for (size_t i = 0; i < m_path_lengths_for_idx.size(); ++i) { 96 | m_file << m_path_lengths_for_idx[i]; 97 | if (i < m_path_lengths_for_idx.size() - 1) { 98 | m_file << ", "; 99 | } 100 | } 101 | m_file << std::endl; 102 | 103 | m_path_lengths_for_idx.clear(); 104 | } 105 | 106 | // Success Rate 107 | void ResultHandlerSuccessRate::handleResult(const BenchmarkResult& result, std::string, int, int) { 108 | for (size_t query_idx = 0; query_idx < result.success.size(); ++query_idx) { 109 | m_results_for_idx.push_back(result.success[query_idx]); 110 | } 111 | last_description = result.description; 112 | } 113 | 114 | void ResultHandlerSuccessRate::printSummary(std::string planner, int benchmark_idx) { 115 | if (m_results_for_idx.size() == 0) { 116 | std::cout << "Success rate: not any valid measurements." << std::endl; 117 | return; 118 | } 119 | // print the average success rate 120 | std::cout << "Success rate: " << std::accumulate(m_results_for_idx.begin(), m_results_for_idx.end(), 0.0) / m_results_for_idx.size() << std::endl; 121 | std::cout << " " << m_results_for_idx.size() << " measurements." << std::endl; 122 | m_file << last_description << ", " << benchmark_idx << ", "; 123 | for (size_t i = 0; i < m_results_for_idx.size(); ++i) { 124 | m_file << m_results_for_idx[i]; 125 | if (i < m_results_for_idx.size() - 1) { 126 | m_file << ", "; 127 | } 128 | } 129 | m_file << std::endl; 130 | 131 | m_results_for_idx.clear(); 132 | } 133 | 134 | // Computation Time 135 | void ResultHandlerComputingTime::handleResult(const BenchmarkResult& result, std::string, int, int) { 136 | // if not all are true, return 137 | if (std::all_of(result.success.begin(), result.success.end(), [](bool b) { return !b; })) 138 | return; 139 | m_results_for_idx.push_back(result.duration_micros); 140 | last_description = result.description; 141 | } 142 | 143 | void ResultHandlerComputingTime::printSummary(std::string planner, int benchmark_idx) { 144 | if (m_results_for_idx.size() == 0) { 145 | std::cout << "Computation time: not any valid measurements." << std::endl; 146 | return; 147 | } 148 | // print the average computation time, min and max 149 | std::cout << "Computation time: " << std::endl; 150 | double avg = std::accumulate(m_results_for_idx.begin(), m_results_for_idx.end(), 0.0) / m_results_for_idx.size(); 151 | std::cout << " Average: " << avg << " microseconds" << std::endl; 152 | std::cout << " Std. Dev.: " << std_dev(avg, m_results_for_idx) << std::endl; 153 | std::cout << " Min: " << *std::min_element(m_results_for_idx.begin(), m_results_for_idx.end()) << " microseconds" << std::endl; 154 | std::cout << " Max: " << *std::max_element(m_results_for_idx.begin(), m_results_for_idx.end()) << " microseconds" << std::endl; 155 | std::cout << " " << m_results_for_idx.size() << " measurements." << std::endl; 156 | 157 | m_file << last_description << ", " << benchmark_idx << ", "; 158 | for (size_t i = 0; i < m_results_for_idx.size(); ++i) { 159 | m_file << m_results_for_idx[i]; 160 | if (i < m_results_for_idx.size() - 1) { 161 | m_file << ", "; 162 | } 163 | } 164 | m_file << std::endl; 165 | 166 | m_results_for_idx.clear(); 167 | } 168 | 169 | // Theta Changes 170 | void ResultHandlerThetaChanges::handleResult(const BenchmarkResult& result, std::string, int, int) { 171 | // iterate over result.path, compute the angle for each line 172 | // and add it to the vector 173 | 174 | last_description = result.description; 175 | for (size_t query_idx = 0; query_idx < result.success.size(); query_idx++) { 176 | if (!result.success[query_idx] || result.path[query_idx].size() < 2) 177 | return; 178 | 179 | double total_diff = 0.0; 180 | for (size_t i = 2; i < result.path[query_idx].size(); ++i) { 181 | double angle1 = std::atan2(result.path[query_idx][i - 1].y() - result.path[query_idx][i - 2].y(), 182 | result.path[query_idx][i - 1].x() - result.path[query_idx][i - 2].x()); 183 | double angle2 = std::atan2(result.path[query_idx][i].y() - result.path[query_idx][i - 1].y(), 184 | result.path[query_idx][i].x() - result.path[query_idx][i - 1].x()); 185 | total_diff += std::abs(angle1 - angle2); 186 | } 187 | 188 | m_results_for_idx.push_back(total_diff); 189 | } 190 | } 191 | 192 | void ResultHandlerThetaChanges::printSummary(std::string planner, int benchmark_idx) { 193 | if (m_results_for_idx.size() == 0) { 194 | std::cout << "Theta changes: not any valid measurements." << std::endl; 195 | return; 196 | } 197 | 198 | // print average, min and max theta changes 199 | std::cout << "Theta changes: " << std::endl; 200 | double avg = std::accumulate(m_results_for_idx.begin(), m_results_for_idx.end(), 0.0) / m_results_for_idx.size(); 201 | std::cout << " Average: " << avg << " rad" << std::endl; 202 | std::cout << " Std. Dev.: " << std_dev(avg, m_results_for_idx) << std::endl; 203 | std::cout << " Min: " << *std::min_element(m_results_for_idx.begin(), m_results_for_idx.end()) << " rad" << std::endl; 204 | std::cout << " Max: " << *std::max_element(m_results_for_idx.begin(), m_results_for_idx.end()) << " rad" << std::endl; 205 | std::cout << " " << m_results_for_idx.size() << " measurements." << std::endl; 206 | 207 | m_file << last_description << ", " << benchmark_idx << ", "; 208 | for (size_t i = 0; i < m_results_for_idx.size(); ++i) { 209 | m_file << m_results_for_idx[i]; 210 | if (i < m_results_for_idx.size() - 1) { 211 | m_file << ", "; 212 | } 213 | } 214 | m_file << std::endl; 215 | 216 | m_results_for_idx.clear(); 217 | } 218 | 219 | // Summary of waiting moves performed by T-PRM 220 | void ResultHandlerTPRMWaiting::handleResult(const BenchmarkResult& result, std::string planner, int, int) { 221 | for (size_t query_idx = 0; query_idx < result.success.size(); query_idx++) { 222 | if (!result.success[query_idx] || result.path[query_idx].size() == 0) 223 | return; 224 | if (planner == "T-PRM") { 225 | // iterate over the path, check if two successors are close to each other, if so, increment m_waiting 226 | for (size_t i = 0; i < result.path.size() - 1; ++i) { 227 | ; 228 | if ((result.path[query_idx][i] - result.path[query_idx][i + 1]).squaredNorm() < 0.01) { 229 | ++m_waiting; 230 | } 231 | } 232 | } 233 | } 234 | } 235 | 236 | void ResultHandlerTPRMWaiting::printSummary(std::string planner, int benchmark_idx) { 237 | if (planner == "T-PRM") { 238 | std::cout << "Waiting moves: " << m_waiting << std::endl; 239 | } 240 | m_waiting = 0; 241 | } 242 | 243 | // Path Writer 244 | void ResultHandlerPathWriter::handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) { 245 | for (int query_idx = 0; query_idx < result.success.size(); query_idx++) { 246 | std::ofstream path_file("path_" + planner + "_" + std::to_string(benchmark_idx) + "_" + std::to_string(run_idx) + "_" + std::to_string(query_idx) + 247 | ".path"); 248 | if (result.success[query_idx] && result.path[query_idx].size() > 1) 249 | for (size_t i = 1; i < result.path[query_idx].size(); i++) { 250 | path_file << result.path[query_idx][i - 1].x() << " " << result.path[query_idx][i - 1].y() << " " << result.path[query_idx][i - 1].z() << " " 251 | << result.path[query_idx][i].x() << " " << result.path[query_idx][i].y() << " " << result.path[query_idx][i].z(); 252 | if (result.path_durations.size() && result.timing_results.size() > 0) { 253 | path_file << " " << result.path_durations[query_idx][i]; 254 | } 255 | path_file << std::endl; 256 | } 257 | path_file.close(); 258 | } 259 | } 260 | 261 | // All writer 262 | ResultHandlerAllWriter::ResultHandlerAllWriter() : ResultHandler("All Writer"), m_file("all_data.csv") { 263 | m_file << "Planner,Benchmark Index,Run Index,Description,Duration\n"; 264 | } 265 | void ResultHandlerAllWriter::handleResult(const BenchmarkResult& result, std::string planner, int benchmark_idx, int run_idx) { 266 | for (const auto& r : result.timing_results) { 267 | m_file << planner << "," << benchmark_idx << "," << run_idx << ","; 268 | m_file << r.description << "," << r.duration_micros << "\n"; 269 | } 270 | m_file << planner << "," << benchmark_idx << "," << run_idx << ",FULL DURATION," << result.duration_micros << std::endl; 271 | } 272 | 273 | } // namespace benchmarking 274 | -------------------------------------------------------------------------------- /benchmarking/study/narrow_gap.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | void write_info_about_bm(std::shared_ptr bm) { 46 | std::ofstream file("info_about_bm.csv", std::ios::app); 47 | file << "==========" << std::endl; 48 | file << "Benchmark: " << bm->name << std::endl; 49 | file << "Num. of obstacles: " << bm->circles.size() << std::endl; 50 | file << "Num. of queries: " << bm->start.size() << std::endl; 51 | file << "Num. of runs: " << bm->numRuns() << std::endl; 52 | file << "OBSTACLES:" << std::endl; 53 | for (size_t i = 0; i < bm->circles.size(); ++i) { 54 | file << " " << bm->circles[i].center.x() << ", " << bm->circles[i].center.y() << ", " << bm->circles[i].radius << std::endl; 55 | } 56 | file << "QUERIES:" << std::endl; 57 | for (size_t i = 0; i < bm->start.size(); ++i) { 58 | file << " " << bm->start[i].x() << ", " << bm->start[i].y() << ", " << bm->start[i].z() << ", " << bm->goal[i].x() << ", " << bm->goal[i].y() << ", " 59 | << bm->goal[i].z() << std::endl; 60 | } 61 | } 62 | 63 | std::shared_ptr create_basic_bm(int iters) { 64 | using namespace benchmarking; 65 | 66 | // Register benchmarks 67 | auto b1 = std::make_shared("Narrow Gap", iters); 68 | b1->numNodes = 1000; // CHANGE HERE (number of nodes of T-PRM) 69 | 70 | b1->domain_size = 10.; 71 | 72 | // Specific 73 | 74 | b1->tprm_cost_edge_threshold = 1.0; // CHANGE HERE (edge connection radius of T-PRM) 75 | 76 | b1->ompl_path_length_threshold = std::numeric_limits::infinity(); 77 | b1->ompl_edge_length = 1.0; // CHANGE HERE (edge connection radius of PRM) 78 | b1->ompl_time_limit = 1.0; 79 | 80 | b1->start.push_back(tprm::Vector3d::Zero()); 81 | b1->goal.push_back(tprm::Vector3d::Constant(10.)); 82 | 83 | b1->circles.push_back(benchmarking::Circle(tprm::Vector3d(5., 1.5, 0.), 2.)); 84 | b1->circles.push_back(benchmarking::Circle(tprm::Vector3d(5., 3., 0.), 1.95)); 85 | 86 | // and other side 87 | b1->circles.push_back(benchmarking::Circle(tprm::Vector3d(5., 10. - 1.5, 0.), 2.)); 88 | b1->circles.push_back(benchmarking::Circle(tprm::Vector3d(5., 10. - 3., 0.), 1.95)); 89 | 90 | write_info_about_bm(b1); 91 | 92 | return b1; 93 | } 94 | 95 | int main(int argc, char const* argv[]) { 96 | const int NUM_ITERS = 1; // 100 for real runs, 1 for path print. 97 | 98 | std::ofstream file("info_about_bm.csv"); 99 | file.close(); // clear the file 100 | 101 | srand(42); 102 | using namespace benchmarking; 103 | 104 | BenchmarkFactory factory = BenchmarkFactory(); 105 | 106 | // Register planners 107 | factory.register_planner(std::make_shared()); 108 | factory.register_planner(std::make_shared("RRTstar")); 109 | factory.register_planner(std::make_shared("PRM")); 110 | 111 | // Register result handlers 112 | factory.register_result_handler(std::make_shared()); 113 | factory.register_result_handler(std::make_shared()); 114 | factory.register_result_handler(std::make_shared()); 115 | factory.register_result_handler(std::make_shared()); 116 | 117 | // Remove for benchmark runs because slow 118 | factory.register_result_handler(std::make_shared()); 119 | 120 | //factory.register_result_handler(std::make_shared()); 121 | 122 | // Register benchmarks 123 | factory.register_benchmark(create_basic_bm(NUM_ITERS)); 124 | 125 | // Run all benchmarks on all planners 126 | factory.run(); 127 | 128 | return 0; 129 | } 130 | -------------------------------------------------------------------------------- /benchmarking/study/study_dynamic_obstacles.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | void write_info_about_bm(std::shared_ptr bm) { 46 | std::ofstream file("info_about_bm.txt", std::ios::app); 47 | file << "==========" << std::endl; 48 | file << "Benchmark: " << bm->name << std::endl; 49 | file << "Num. of obstacles: " << bm->moving_circles.size() << std::endl; 50 | file << "Num. of queries: " << bm->start.size() << std::endl; 51 | file << "Num. of runs: " << bm->numRuns() << std::endl; 52 | file << "OBSTACLES:" << std::endl; 53 | for (size_t i = 0; i < bm->moving_circles.size(); ++i) { 54 | file << " " << bm->moving_circles[i].center.x() << ", " << bm->moving_circles[i].center.y() << ", " << bm->moving_circles[i].center.z() << ", " 55 | << bm->moving_circles[i].radius << ", " << bm->moving_circles[i].velocity.x() << ", " << bm->moving_circles[i].velocity.y() << ", " 56 | << bm->moving_circles[i].velocity.z() << std::endl; 57 | } 58 | file << "QUERIES:" << std::endl; 59 | for (size_t i = 0; i < bm->start.size(); ++i) { 60 | file << " " << bm->start[i].x() << ", " << bm->start[i].y() << ", " << bm->start[i].z() << ", " << bm->goal[i].x() << ", " << bm->goal[i].y() << ", " 61 | << bm->goal[i].z() << std::endl; 62 | } 63 | } 64 | 65 | std::shared_ptr create_basic_bm(int iters) { 66 | using namespace benchmarking; 67 | 68 | // Register benchmarks 69 | auto b1 = std::make_shared("Basic", iters); 70 | b1->numNodes = 3000; // TODO: CHANGE HERE (number of nodes of T-PRM) 71 | 72 | b1->start = {tprm::Vector3d::Zero()}; 73 | b1->goal = {tprm::Vector3d::Constant(10.)}; 74 | 75 | b1->domain_size = 10.; 76 | 77 | // Specific 78 | b1->tprm_cost_edge_threshold = 1.75; // TODO: CHANGE HERE (edge connection radius of T-PRM) 79 | 80 | b1->ompl_path_length_threshold = std::numeric_limits::infinity(); 81 | b1->ompl_edge_length = 1.75; // TODO: CHANGE HERE (edge connection radius of PRM) 82 | b1->ompl_time_limit = 1.0; 83 | 84 | b1->is_2d = true; // TODO: CHANGE HERE FOR 3D 85 | 86 | return b1; 87 | } 88 | 89 | std::vector> create_more_obstacles_bm(int iters) { 90 | using namespace benchmarking; 91 | 92 | std::vector> bms; 93 | 94 | for (int i : {15}) { // TODO: CHANGE HERE (number of obstacles) 95 | std::shared_ptr b1 = create_basic_bm(iters); 96 | 97 | b1->name = std::to_string(i); 98 | 99 | for (int j = 0; j < i; j++) { 100 | Eigen::Vector3d pos = Eigen::Vector3d::Random() * 4 + Eigen::Vector3d::Constant(5.); 101 | Eigen::Vector3d vel = Eigen::Vector3d::Random() * 0.5; 102 | b1->moving_circles.push_back(MovingCircle(pos, 0.75, vel)); 103 | } 104 | 105 | write_info_about_bm(b1); 106 | bms.push_back(b1); 107 | } 108 | 109 | return bms; 110 | } 111 | 112 | int main(int argc, char const* argv[]) { 113 | const int NUM_ITERS = 1; 114 | 115 | std::ofstream file("info_about_bm.txt"); 116 | file.close(); // clear the file 117 | 118 | srand(time(NULL)); 119 | using namespace benchmarking; 120 | 121 | BenchmarkFactory factory = BenchmarkFactory(); 122 | 123 | // Register planners 124 | factory.register_planner(std::make_shared()); 125 | factory.register_planner(std::make_shared("PRM")); 126 | factory.register_planner(std::make_shared("RRTstar")); 127 | 128 | // Register result handlers 129 | factory.register_result_handler(std::make_shared()); 130 | factory.register_result_handler(std::make_shared()); 131 | factory.register_result_handler(std::make_shared()); 132 | factory.register_result_handler(std::make_shared()); 133 | factory.register_result_handler(std::make_shared()); 134 | 135 | // Remove for benchmark runs because slow 136 | factory.register_result_handler(std::make_shared()); 137 | 138 | factory.register_result_handler(std::make_shared()); 139 | 140 | // Register benchmarks 141 | factory.register_benchmarks(create_more_obstacles_bm(NUM_ITERS)); 142 | 143 | // Run all benchmarks on all planners 144 | factory.run(); 145 | 146 | return 0; 147 | } 148 | -------------------------------------------------------------------------------- /benchmarking/study/study_static_obstacles.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | void write_info_about_bm(std::shared_ptr bm) { 46 | std::ofstream file("info_about_bm.txt", std::ios::app); 47 | file << "==========" << std::endl; 48 | file << "Benchmark: " << bm->name << std::endl; 49 | file << "Num. of obstacles: " << bm->circles.size() << std::endl; 50 | file << "Num. of queries: " << bm->start.size() << std::endl; 51 | file << "Num. of runs: " << bm->numRuns() << std::endl; 52 | file << "OBSTACLES:" << std::endl; 53 | for (size_t i = 0; i < bm->circles.size(); ++i) { 54 | file << " " << bm->circles[i].center.x() << ", " << bm->circles[i].center.y() << ", " << bm->circles[i].center.z() << ", " << bm->circles[i].radius 55 | << std::endl; 56 | } 57 | file << "QUERIES:" << std::endl; 58 | for (size_t i = 0; i < bm->start.size(); ++i) { 59 | file << " " << bm->start[i].x() << ", " << bm->start[i].y() << ", " << bm->start[i].z() << ", " << bm->goal[i].x() << ", " << bm->goal[i].y() << ", " 60 | << bm->goal[i].z() << std::endl; 61 | } 62 | } 63 | 64 | void create_starts(std::shared_ptr bm) { 65 | bm->start = {tprm::Vector3d::Zero()}; 66 | bm->goal = {tprm::Vector3d::Constant(10.)}; 67 | } 68 | 69 | std::shared_ptr create_basic_bm(int iters) { 70 | using namespace benchmarking; 71 | 72 | // Register benchmarks 73 | auto b1 = std::make_shared("Static Obstacles", iters); 74 | b1->numNodes = 3000; // CHANGE HERE (number of T-PRM nodes) 75 | 76 | b1->domain_size = 10.; 77 | 78 | // Specific 79 | b1->tprm_cost_edge_threshold = 1.75; // TODO: CHANGE HERE (edge connection radius of T-PRM) 80 | 81 | b1->ompl_path_length_threshold = std::numeric_limits::infinity(); 82 | b1->ompl_edge_length = 1.0; // TOOD: CHANGE HERE (edge connection radius of PRM) 83 | b1->ompl_time_limit = 1.0; 84 | 85 | b1->is_2d = true; // TODO: CHANGE HERE FOR 3D 86 | 87 | return b1; 88 | } 89 | 90 | std::vector> create_more_obstacles_bm(int iters) { 91 | using namespace benchmarking; 92 | 93 | std::vector> bms; 94 | 95 | for (int i : {20}) { // TODO: CHANGE HERE (number of obstacles) 96 | auto bm = create_basic_bm(iters); 97 | bm->name = std::to_string(i); 98 | for (int j = 0; j < i; j++) { 99 | bm->circles.push_back(benchmarking::Circle(Eigen::Vector3d::Random() * 4. + Eigen::Vector3d::Constant(5.), 0.75)); 100 | } 101 | 102 | create_starts(bm); 103 | write_info_about_bm(bm); 104 | 105 | bms.push_back(bm); 106 | } 107 | 108 | return bms; 109 | } 110 | 111 | int main(int argc, char const* argv[]) { 112 | const int NUM_ITERS = 1; // 100 for real runs, 1 for path print. 113 | 114 | std::ofstream file("info_about_bm.txt"); 115 | file.close(); // clear the file 116 | 117 | srand(42); 118 | using namespace benchmarking; 119 | 120 | BenchmarkFactory factory = BenchmarkFactory(); 121 | 122 | // Register planners 123 | factory.register_planner(std::make_shared()); 124 | factory.register_planner(std::make_shared("PRM")); 125 | factory.register_planner(std::make_shared("RRTstar")); 126 | 127 | // Register result handlers 128 | factory.register_result_handler(std::make_shared()); 129 | factory.register_result_handler(std::make_shared()); 130 | factory.register_result_handler(std::make_shared()); 131 | factory.register_result_handler(std::make_shared()); 132 | 133 | // Remove for benchmark runs because slow 134 | factory.register_result_handler(std::make_shared()); 135 | 136 | factory.register_result_handler(std::make_shared()); 137 | 138 | // Register benchmarks 139 | factory.register_benchmarks(create_more_obstacles_bm(NUM_ITERS)); 140 | 141 | // Run all benchmarks on all planners 142 | factory.run(); 143 | 144 | return 0; 145 | } 146 | -------------------------------------------------------------------------------- /cmake/FindOMPL.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find the OMPL library 2 | # Once done this will define: 3 | # 4 | # OMPL_FOUND - OMPL was found 5 | # OMPL_INCLUDE_DIRS - The OMPL include directory 6 | # OMPL_LIBRARIES - The OMPL library 7 | # OMPLAPP_LIBRARIES - The OMPL.app libraries 8 | # OMPL_VERSION - The OMPL version in the form .. 9 | # OMPL_MAJOR_VERSION - Major version 10 | # OMPL_MINOR_VERSION - Minor version 11 | # OMPL_PATCH_VERSION - Patch version 12 | 13 | include(FindPackageHandleStandardArgs) 14 | 15 | # user can set OMPL_PREFIX to specify the prefix path of the OMPL library 16 | # and include directory, either as an environment variable or as an 17 | # argument to cmake ("cmake -DOMPL_PREFIX=...") 18 | if (NOT OMPL_PREFIX) 19 | set(OMPL_PREFIX $ENV{OMPL_PREFIX}) 20 | endif() 21 | 22 | if (OMPL_FIND_VERSION) 23 | set(OMPL_SUFFIX "-${OMPL_VERSION}") 24 | else() 25 | set(OMPL_SUFFIX "") 26 | endif() 27 | 28 | # user can set OMPL_LIB_PATH to specify the path for the OMPL library 29 | # (analogous to OMPL_PREFIX) 30 | if (NOT OMPL_LIB_PATH) 31 | set(OMPL_LIB_PATH $ENV{OMPL_LIB_PATH}) 32 | if (NOT OMPL_LIB_PATH) 33 | set(OMPL_LIB_PATH ${OMPL_PREFIX}) 34 | endif() 35 | endif() 36 | 37 | # user can set OMPL_INCLUDE_PATH to specify the path for the OMPL include 38 | # directory (analogous to OMPL_PREFIX) 39 | if (NOT OMPL_INCLUDE_PATH) 40 | set(OMPL_INCLUDE_PATH $ENV{OMPL_INCLUDE_PATH}) 41 | if (NOT OMPL_INCLUDE_PATH) 42 | set(OMPL_INCLUDE_PATH ${OMPL_PREFIX}) 43 | endif() 44 | endif() 45 | 46 | # find the OMPL library 47 | find_library(OMPL_LIBRARY ompl 48 | PATHS ${OMPL_LIB_PATH} 49 | PATH_SUFFIXES lib build/lib) 50 | if (OMPL_LIBRARY) 51 | if (OMPL_FIND_VERSION) 52 | get_filename_component(libpath ${OMPL_LIBRARY} PATH) 53 | file(GLOB OMPL_LIBS "${libpath}/libompl.${OMPL_FIND_VERSION}.*") 54 | list(GET OMPL_LIBS -1 OMPL_LIBRARY) 55 | endif() 56 | set(OMPL_LIBRARIES "${OMPL_LIBRARY}" CACHE FILEPATH "Path to OMPL library") 57 | endif() 58 | # find the OMPL.app libraries 59 | find_library(OMPLAPPBASE_LIBRARY ompl_app_base 60 | PATHS ${OMPL_LIB_PATH} 61 | PATH_SUFFIXES lib build/lib) 62 | find_library(OMPLAPP_LIBRARY ompl_app 63 | PATHS ${OMPL_LIB_PATH} 64 | PATH_SUFFIXES lib build/lib) 65 | if (OMPLAPPBASE_LIBRARY AND OMPLAPP_LIBRARY) 66 | if (OMPL_FIND_VERSION) 67 | get_filename_component(libpath ${OMPLAPPBASE_LIBRARY} PATH) 68 | file(GLOB OMPLAPPBASE_LIBS "${libpath}/libompl_app_base.${OMPL_FIND_VERSION}.*") 69 | list(GET OMPLAPPBASE_LIBS -1 OMPLAPPBASE_LIBRARY) 70 | get_filename_component(libpath ${OMPLAPP_LIBRARY} PATH) 71 | file(GLOB OMPLAPP_LIBS "${libpath}/libompl_app.${OMPL_FIND_VERSION}.*") 72 | list(GET OMPLAPP_LIBS -1 OMPLAPP_LIBRARY) 73 | endif() 74 | set(OMPLAPP_LIBRARIES "${OMPLAPPBASE_LIBRARY};${OMPLAPP_LIBRARY}" CACHE STRING "Paths to OMPL.app libraries") 75 | endif() 76 | 77 | # find include path 78 | find_path(OMPL_INCLUDE_DIRS SpaceInformation.h 79 | PATHS ${OMPL_INCLUDE_PATH} 80 | PATH_SUFFIXES base "ompl${OMPL_SUFFIX}/base" "include/ompl${OMPL_SUFFIX}/base" ompl/base include/ompl/base src/ompl/base) 81 | if (OMPL_INCLUDE_DIRS) 82 | string(REGEX REPLACE "/ompl/base$" "" OMPL_INCLUDE_DIRS ${OMPL_INCLUDE_DIRS}) 83 | else() 84 | set(OMPL_INCLUDE_DIRS "") 85 | endif() 86 | 87 | find_path(EIGEN_INCLUDE_DIRS Eigen/Core 88 | PATHS ${OMPL_INCLUDE_PATH} 89 | PATH_SUFFIXES eigen3 include/eigen3 src/eigen3) 90 | if (EIGEN_INCLUDE_DIRS) 91 | string(REGEX REPLACE "Eigen/Core$" "" EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 92 | list(APPEND OMPL_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) 93 | unset(EIGEN_INCLUDE_DIRS) 94 | else() 95 | set(EIGEN_INCLUDE_DIRS "") 96 | endif() 97 | 98 | # find version 99 | find_file(OMPL_CONFIG config.h 100 | PATHS ${OMPL_INCLUDE_DIRS} 101 | PATH_SUFFIXES ompl 102 | NO_DEFAULT_PATH) 103 | if(OMPL_CONFIG) 104 | file(READ ${OMPL_CONFIG} OMPL_CONFIG_STR) 105 | string(REGEX REPLACE ".*OMPL_VERSION \"([0-9.]+)\".*" "\\1" 106 | OMPL_VERSION 107 | "${OMPL_CONFIG_STR}") 108 | string(REGEX REPLACE "([0-9]+).([0-9]+).([0-9]+)" "\\1" OMPL_MAJOR_VERSION "${OMPL_VERSION}") 109 | string(REGEX REPLACE "([0-9]+).([0-9]+).([0-9]+)" "\\2" OMPL_MINOR_VERSION "${OMPL_VERSION}") 110 | string(REGEX REPLACE "([0-9]+).([0-9]+).([0-9]+)" "\\3" OMPL_PATCH_VERSION "${OMPL_VERSION}") 111 | endif() 112 | 113 | find_package_handle_standard_args(OMPL DEFAULT_MSG OMPL_LIBRARIES OMPL_INCLUDE_DIRS) -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11) 2 | 3 | project(TPRM_Examples) 4 | set(CMAKE_BUILD_TYPE Release) 5 | 6 | add_executable(static_avoidance static_avoidance.cpp) 7 | add_executable(dynamic_avoidance dynamic_avoidance.cpp) 8 | 9 | target_link_libraries(static_avoidance TPRM) 10 | target_link_libraries(dynamic_avoidance TPRM) 11 | -------------------------------------------------------------------------------- /examples/dynamic_avoidance.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | int main(int argc, char const* argv[]) { 45 | tprm::HolonomicRobot::movement_speed = 0.1; // m/s 46 | 47 | std::srand(3); 48 | 49 | tprm::TemporalPRM tprm; 50 | 51 | // add a moving sphere, towards 0/0/0 52 | tprm.addDynamicObstacle(std::make_shared(tprm::Vector3d::Constant(1.), tprm::Vector3d::Constant(-0.1), 0.25)); 53 | 54 | tprm.placeSamples(150); 55 | 56 | tprm.buildPRM(0.25); 57 | 58 | std::cout << "Number of nodes: " << tprm.getTemporalGraph().getNumNodes() << std::endl; 59 | std::cout << "Number of edges: " << tprm.getTemporalGraph().getNumEdges() << std::endl; 60 | 61 | auto path = tprm.getShortestPath(tprm::Vector3d(0, 0, 0), tprm::Vector3d(1., 1., 1.), 0.5); 62 | 63 | if (path.empty()) { 64 | std::cout << "No path found" << std::endl; 65 | } else { 66 | for (auto& node : path) { 67 | std::cout << "Node: " << node.position.transpose() << " at time " << node.time << std::endl; 68 | } 69 | } 70 | 71 | return 0; 72 | } -------------------------------------------------------------------------------- /examples/static_avoidance.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | int main(int argc, char const* argv[]) { 45 | tprm::HolonomicRobot::movement_speed = 0.1; // m/s 46 | 47 | std::srand(1); 48 | 49 | tprm::TemporalPRM tprm; 50 | 51 | tprm.addStaticObstacle(std::make_shared(tprm::Vector3d::Constant(0.5), 0.25)); 52 | 53 | tprm.placeSamples(100); 54 | 55 | tprm.buildPRM(0.25); 56 | 57 | std::cout << "Number of nodes: " << tprm.getTemporalGraph().getNumNodes() << std::endl; 58 | std::cout << "Number of edges: " << tprm.getTemporalGraph().getNumEdges() << std::endl; 59 | 60 | auto path = tprm.getShortestPath(tprm::Vector3d(0, 0, 0), tprm::Vector3d(1., 1., 1.), 0.5); 61 | 62 | if (path.empty()) { 63 | std::cout << "No path found" << std::endl; 64 | } else { 65 | for (auto& node : path) { 66 | std::cout << "Node: " << node.position.transpose() << " at time " << node.time << std::endl; 67 | } 68 | } 69 | 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /include/tprm/config.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #ifndef __TPRM_CONFIG_H__ 40 | #define __TPRM_CONFIG_H__ 41 | 42 | #include 43 | 44 | namespace tprm { 45 | /** 46 | * @brief Type definition for a vector of Eigen::Vector3d. Used everywhere. 47 | */ 48 | using Vector3d = Eigen::Vector3d; 49 | 50 | /** 51 | * @brief Parameters for the Robot 52 | * 53 | * This class contains all the parameters for the holonomic robot assumed in the planning problem. 54 | */ 55 | class HolonomicRobot { 56 | public: 57 | /** 58 | * @brief Movement speed in all directions. [m/s] 59 | * 60 | * Default value: 0.5 m/s 61 | */ 62 | static double movement_speed; 63 | }; 64 | 65 | } /* namespace tprm */ 66 | 67 | #endif /* __TPRM_CONFIG_H__ */ -------------------------------------------------------------------------------- /include/tprm/obstacle_base.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #ifndef __TPRM_OBSTACLE_BASE_H__ 40 | #define __TPRM_OBSTACLE_BASE_H__ 41 | 42 | #include 43 | 44 | namespace tprm { 45 | 46 | /** 47 | * @brief Base class for all obstacles. 48 | */ 49 | class Obstacle { 50 | public: 51 | /** 52 | * @brief Constructor. 53 | */ 54 | Obstacle() = delete; 55 | 56 | /** 57 | * @brief Constructor. 58 | * @param COM Center of mass of the obstacle. 59 | */ 60 | Obstacle(const Vector3d& COM); 61 | 62 | /** 63 | * @brief Destructor. 64 | */ 65 | virtual ~Obstacle() = default; 66 | 67 | /** 68 | * @brief Returns the center of mass of the obstacle. 69 | * @return Center of mass of the obstacle. 70 | */ 71 | Vector3d getCOM() const; 72 | 73 | /** 74 | * @brief Sets the center of mass of the obstacle. 75 | * @param COM Center of mass of the obstacle. 76 | */ 77 | void setCOM(const Vector3d& COM); 78 | 79 | protected: 80 | Vector3d m_COM; ///< center of mass 81 | }; 82 | 83 | /** 84 | * @brief Class to represent a static obstacle. 85 | */ 86 | class StaticObstacle : public Obstacle { 87 | public: 88 | /** 89 | * @brief Constructor. 90 | */ 91 | StaticObstacle() = delete; 92 | 93 | /** 94 | * @brief Constructor. 95 | * @param COM Center of mass of the obstacle. 96 | */ 97 | StaticObstacle(const Vector3d& COM); 98 | 99 | /** 100 | * @brief Destructor. 101 | */ 102 | virtual ~StaticObstacle() = default; 103 | 104 | /** 105 | * @brief Checks if the obstacle collides with a given position. 106 | * @param position The position to check. 107 | * @return True if the obstacle collides with the position, false otherwise. 108 | */ 109 | virtual bool isColliding(const Vector3d& position) const = 0; 110 | 111 | /** 112 | * @brief Checks if the obstacle collides with a line segment. 113 | * @param segment_from Start of the line segment. 114 | * @param segment_to End of the line segment. 115 | * @return True if the obstacle collides with the line segment, false otherwise. 116 | */ 117 | virtual bool isColliding(const Vector3d& segment_from, const Vector3d& segment_to) const = 0; 118 | }; 119 | 120 | /** 121 | * @brief Class to represent a dynamic obstacle. 122 | */ 123 | class DynamicObstacle : public Obstacle { 124 | public: 125 | /** 126 | * @brief Constructor. 127 | */ 128 | DynamicObstacle() = delete; 129 | 130 | /** 131 | * @brief Constructor. 132 | * @param COM Center of mass of the obstacle. 133 | * @param velocity Velocity of the obstacle. 134 | */ 135 | DynamicObstacle(const Vector3d& COM, const Vector3d& velocity); 136 | 137 | /** 138 | * @brief Destructor. 139 | */ 140 | virtual ~DynamicObstacle() = default; 141 | 142 | /** 143 | * @brief Returns the velocity of the obstacle. 144 | * @return Velocity of the obstacle. 145 | */ 146 | Vector3d getVelocity() const; 147 | 148 | /** 149 | * @brief Sets the velocity of the obstacle. 150 | * @param velocity Velocity of the obstacle. 151 | */ 152 | void setVelocity(const Vector3d& velocity); 153 | 154 | Vector3d getCOM() = delete; ///< getting static COM is not allowed 155 | 156 | /** 157 | * @brief Get the center of mass of the dynamic obstacle for a given time. 158 | * @param time Time. 159 | * @return Center of mass of the dynamic obstacle for the given time. 160 | */ 161 | virtual Vector3d getCOM(double time) const; 162 | 163 | /** 164 | * @brief Checks if the obstacle collides with a given position. 165 | * @param position The position to check. 166 | * @param[out] hitTimeFrom Time when the obstacle starts colliding with the position. 167 | * @param[out] hitTimeTo Time when the obstacle stops colliding with the position. 168 | * @return True if the obstacle collides with the position, false otherwise. 169 | */ 170 | virtual bool isColliding(const Vector3d& position, double& hitTimeFrom, double& hitTimeTo) const = 0; 171 | 172 | private: 173 | Vector3d m_velocity; ///< velocity 174 | }; 175 | 176 | } /* namespace tprm */ 177 | 178 | #endif /* __TPRM_OBSTACLE_BASE_H__ */ -------------------------------------------------------------------------------- /include/tprm/obstacle_impl.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #ifndef __TPRM_OBSTACLE_IMPL_H__ 40 | #define __TPRM_OBSTACLE_IMPL_H__ 41 | 42 | #include 43 | 44 | namespace tprm { 45 | 46 | /** 47 | * @brief Static Sphere 48 | */ 49 | class StaticSphereObstacle : public StaticObstacle { 50 | public: 51 | /** 52 | * @brief Constructor. 53 | */ 54 | StaticSphereObstacle() = delete; 55 | 56 | /** 57 | * @brief Constructor. 58 | * @param COM Center of mass of the obstacle. 59 | * @param radius Radius of the sphere. 60 | */ 61 | StaticSphereObstacle(const Vector3d& COM, double radius); 62 | 63 | /** 64 | * @brief Destructor. 65 | */ 66 | virtual ~StaticSphereObstacle() = default; 67 | 68 | /** 69 | * @brief Checks if the obstacle collides with a given position. 70 | * @param position The position to check. 71 | * @return True if the obstacle collides with the position, false otherwise. 72 | */ 73 | virtual bool isColliding(const Vector3d& position) const override; 74 | 75 | /** 76 | * @brief Checks if the obstacle collides with a line segment. 77 | * @param segment_from Start of the line segment. 78 | * @param segment_to End of the line segment. 79 | * @return True if the obstacle collides with the line segment, false otherwise. 80 | */ 81 | virtual bool isColliding(const Vector3d& segment_from, const Vector3d& segment_to) const override; 82 | 83 | private: 84 | double m_radius; ///< Radius of the sphere. 85 | }; 86 | 87 | /** 88 | * @brief Dynamic Sphere 89 | */ 90 | class DynamicSphereObstacle : public DynamicObstacle { 91 | public: 92 | /** 93 | * @brief Constructor. 94 | */ 95 | DynamicSphereObstacle() = delete; 96 | 97 | /** 98 | * @brief Constructor. 99 | * @param COM Center of mass of the obstacle. 100 | * @param velocity Velocity of the obstacle. 101 | * @param radius Radius of the sphere. 102 | */ 103 | DynamicSphereObstacle(const Vector3d& COM, const Vector3d& velocity, double radius); 104 | 105 | /** 106 | * @brief Destructor. 107 | */ 108 | virtual ~DynamicSphereObstacle() = default; 109 | 110 | /** 111 | * @brief Checks if the obstacle collides with a given position. 112 | * @param position The position to check. 113 | * @param[out] hitTimeFrom Time when the obstacle starts colliding with the position. 114 | * @param[out] hitTimeTo Time when the obstacle stops colliding with the position. 115 | * @return True if the obstacle collides with the position, false otherwise. 116 | */ 117 | virtual bool isColliding(const Vector3d& point, double& hitFromTime, double& hitToTime) const override; 118 | 119 | private: 120 | double m_radius; ///< Radius of the sphere. 121 | }; 122 | 123 | } /* namespace tprm */ 124 | 125 | #endif /* __TPRM_OBSTACLE_IMPL_H__ */ -------------------------------------------------------------------------------- /include/tprm/temporal_graph.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #ifndef __TPRM_TEMPORAL_GRAPH_H__ 40 | #define __TPRM_TEMPORAL_GRAPH_H__ 41 | 42 | #include 43 | 44 | #include // for std::function 45 | #include // for std::vector 46 | 47 | namespace tprm { 48 | 49 | /** 50 | * @brief Struct to store time availability of a node. 51 | */ 52 | struct TimeAvailability { 53 | /** 54 | * @brief Constructor. 55 | * @param start_time The start time of the node. 56 | * @param end_time The end time of the node. 57 | * @throw std::invalid_argument if start_time > end_time or any is below 0. 58 | */ 59 | TimeAvailability(double start_time, double end_time); 60 | double start; ///< The start time of the interval. 61 | double end; ///< The end time of the interval. 62 | }; 63 | 64 | /** 65 | * @brief Class to represent a temporal graph node. 66 | */ 67 | struct TemporalGraphNode { 68 | /** 69 | * @brief Constructor. 70 | * @param position The position of the node. 71 | * @param time_availability The time availability of the node (a vector of intervals) 72 | */ 73 | TemporalGraphNode(const Vector3d& position, const std::vector& time_availabilities) 74 | : position(position), time_availabilities(time_availabilities) {} 75 | 76 | Vector3d position; ///< The position of the node. 77 | 78 | std::vector time_availabilities; ///< The time availability of the node. 79 | 80 | /** 81 | * @brief Checks if the node is available at a given time. 82 | * @param time The time to check. 83 | * @return True if the node is available at the given time. 84 | * @throw std::invalid_argument if time is below 0. 85 | */ 86 | bool isActiveAt(double time) const; 87 | }; 88 | 89 | /** 90 | * @brief Class to represent a temporal graph edge. 91 | * 92 | * The edge is undirected. 93 | */ 94 | struct TemporalGraphEdge { 95 | /** 96 | * @brief Constructor. 97 | * @param node_a The first node of the edge. 98 | * @param node_b The second node of the edge. 99 | */ 100 | TemporalGraphEdge(int node_a, int node_b) : node_a_id(node_a), node_b_id(node_b) {} 101 | int node_a_id = -1; ///< The id of the first node of the edge. 102 | int node_b_id = -1; ///< The id of the second node of the edge. 103 | 104 | /** 105 | * @brief Helper function to get the other end. 106 | * @param node_id The id of the node. 107 | * @return The id of the other end of the edge. 108 | * @throw std::runtime_error if the edge is not connected to the given node. 109 | */ 110 | int getOtherNodeId(int node_id) const; 111 | }; 112 | 113 | /** 114 | * @brief Class to represent a path entry for the found path. 115 | * 116 | * An entry consists of a node and the time at which the node is reached. 117 | */ 118 | struct GraphPathResultEntry { 119 | /** 120 | * @brief Constructor. 121 | * @param node_id The node of the entry. 122 | * @param time The time at which the node is reached. 123 | */ 124 | GraphPathResultEntry(int node_id, double time) : node_id(node_id), time(time){}; 125 | int node_id = -1; ///< The id of the node. 126 | double time = -1; ///< The time at which the node is reached. 127 | }; 128 | 129 | /** 130 | * @brief Class to represent a T-PRM graph. 131 | */ 132 | class TemporalGraph { 133 | public: 134 | /** 135 | * @brief Constructor. 136 | */ 137 | TemporalGraph(); 138 | 139 | /** 140 | * @brief Destructor. 141 | */ 142 | virtual ~TemporalGraph() = default; 143 | 144 | /** 145 | * @brief Clear the graph (deletes all nodes & edges). 146 | */ 147 | void clear(); 148 | 149 | /** 150 | * @brief Add a node to the graph. 151 | * @param node The node to add. 152 | * @return The id of the added node. 153 | */ 154 | int addNode(const TemporalGraphNode& node); 155 | 156 | /** 157 | * @brief Add an edge to the graph. 158 | * @param edge The edge to add. 159 | * @return The id of the added edge. 160 | */ 161 | int addEdge(const TemporalGraphEdge& edge); 162 | 163 | /** 164 | * @brief Get the node with the given id. 165 | * @param id The id of the node. 166 | * @return The node with the given id. 167 | * @throw std::runtime_error if no node with the given id exists. 168 | */ 169 | const TemporalGraphNode& getNode(int id) const; 170 | 171 | /** 172 | * @brief Get a mutable reference to the node with the given id. 173 | * @param id The id of the node. 174 | * @return The node with the given id. 175 | * @throw std::runtime_error if no node with the given id exists. 176 | */ 177 | TemporalGraphNode& getNode(int id); 178 | 179 | /** 180 | * @brief Get the edge with the given id. 181 | * @param id The id of the edge. 182 | * @return The edge with the given id. 183 | * @throw std::runtime_error if no edge with the given id exists. 184 | */ 185 | const TemporalGraphEdge& getEdge(int id) const; 186 | 187 | /** 188 | * @brief Get the number of nodes in the graph. 189 | * @return The number of nodes in the graph. 190 | */ 191 | int getNumNodes() const; 192 | 193 | /** 194 | * @brief Get the number of edges in the graph. 195 | * @return The number of edges in the graph. 196 | */ 197 | int getNumEdges() const; 198 | 199 | /** 200 | * @brief Get the id of the node which is closest to the given position. 201 | * @param position The position to get the closest node to. 202 | * @return The id of the node with the given position. 203 | * @throw std::runtime_error if no node with the given position exists. 204 | */ 205 | int getClosestNode(const Vector3d& position) const; 206 | 207 | /** 208 | * @brief Get the shortest path from the given start node to the given end node. 209 | * 210 | * The first node is visited with the given start time (used for dynamic obstacles). 211 | * @param start_node_id The id of the start node. 212 | * @param end_node_id The id of the end node. 213 | * 214 | * @throws std::runtime_error if either the start or end node does not exist. 215 | * 216 | * @return The shortest path from the given start node to the given end node (empty if no path exists). 217 | */ 218 | std::vector getShortestPath(int start_node_id, int end_node_id, double start_time = 0.) const; 219 | 220 | /** 221 | * @brief Set the edge cost function. 222 | * 223 | * The edge cost function is used to calculate the cost of an edge. 224 | * The default edge cost function is the Euclidean distance between the two nodes. 225 | * @param edge_cost_function The edge cost function. 226 | */ 227 | void setEdgeCostFunction(std::function cost_function); 228 | 229 | /** 230 | * @brief Get the edge cost for an edge. 231 | * @param edge The edge to get the cost for. 232 | * @return The edge cost for the given edge. 233 | */ 234 | double getEdgeCost(const TemporalGraphEdge& edge) const; 235 | 236 | private: 237 | /** 238 | * @brief Get the edge cost between two nodes. 239 | * 240 | * This is the default cost, which is the Euclidean distance between the two nodes. 241 | * @param edge The edge to get the cost for. 242 | * @return The cost of the edge. 243 | */ 244 | double defaultEdgeCostFunction(const TemporalGraphEdge& edge) const; 245 | 246 | private: 247 | /** 248 | * @brief Get the cost of the given edge. 249 | * default: distance between the two nodes. 250 | */ 251 | std::function m_edge_cost_function; 252 | 253 | /** 254 | * @brief The nodes of the graph. 255 | */ 256 | std::vector m_nodes; 257 | /** 258 | * @brief The edges of the graph. 259 | */ 260 | std::vector m_edges; 261 | }; 262 | 263 | } /* namespace tprm */ 264 | 265 | #endif /* __TPRM_TEMPORAL_GRAPH_H__ */ -------------------------------------------------------------------------------- /include/tprm/temporal_prm.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #ifndef __TPRM_TEMPORAL_PRM_H__ 40 | #define __TPRM_TEMPORAL_PRM_H__ 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | namespace tprm { 48 | 49 | /** 50 | * @brief Class to represent a path entry for the found path. 51 | * 52 | * An entry consists of a position and the time at which the position is reached. 53 | */ 54 | struct PathResultEntry { 55 | /** 56 | * @brief Constructor. 57 | * @param position The position of the entry. 58 | * @param time The time at which the position is reached. 59 | */ 60 | PathResultEntry(Vector3d position, double time) : position(position), time(time) {} 61 | Vector3d position; ///< The position of the entry. 62 | double time; ///< The time at which the position is reached. 63 | }; 64 | 65 | /** 66 | * @brief Class to represent the T-PRM PRM. 67 | */ 68 | class TemporalPRM { 69 | public: 70 | /** 71 | * @brief Constructor. 72 | */ 73 | TemporalPRM() = default; 74 | 75 | /** 76 | * @brief Constructor. 77 | * @param environmentMin The minimum coordinates of the environment. 78 | * @param environmentMax The maximum coordinates of the environment. 79 | */ 80 | TemporalPRM(const Vector3d& environmentMin, const Vector3d& environmentMax); 81 | 82 | /** 83 | * @brief Destructor. 84 | */ 85 | virtual ~TemporalPRM() = default; 86 | 87 | /** 88 | * @brief Set the minimum coordinates of the environment. 89 | * @param environmentMin The minimum coordinates of the environment. 90 | */ 91 | void setEnvironmentMin(const Vector3d& environmentMin); 92 | 93 | /** 94 | * @brief Set the maximum coordinates of the environment. 95 | * @param environmentMax The maximum coordinates of the environment. 96 | */ 97 | void setEnvironmentMax(const Vector3d& environmentMax); 98 | 99 | /** 100 | * @brief Add a static obstacle to the environment. 101 | * @param obstacle The obstacle to add. 102 | */ 103 | void addStaticObstacle(const std::shared_ptr& obstacle); 104 | 105 | /** 106 | * @brief Add a dynamic obstacle to the environment. 107 | * @param obstacle The obstacle to add. 108 | */ 109 | void addDynamicObstacle(const std::shared_ptr& obstacle); 110 | 111 | /** 112 | * @brief Place sample points on the environment. 113 | * 114 | * @attention Static obstacles must be added before calling this! 115 | * @param numSamples The number of sample points to place. 116 | */ 117 | void placeSamples(int numSamples); 118 | 119 | /** 120 | * @brief Connect the sample points. 121 | * 122 | * @attention Sample points must be placed before calling this! 123 | * @attention Dynamic obstacles must be added before calling this! (otherwise, call TemporalPRM::recomputeTimeAvailabilities) 124 | * @param costEdgeThreshold The cost threshold for the edges. 125 | */ 126 | void buildPRM(double costEdgeThreshold); 127 | 128 | /** 129 | * @brief Recompute the time availabilities of the nodes in the graph. 130 | * 131 | * @attention Dynamic obstacles must be added before calling this! 132 | */ 133 | void recomputeTimeAvailabilities(); 134 | 135 | /** 136 | * @brief Get the underlying graph. 137 | * @return The underlying graph. 138 | */ 139 | TemporalGraph& getTemporalGraph(); 140 | 141 | /** 142 | * @brief Find the shortest path between two points. 143 | * @param start The start position. 144 | * @param goal The goal position. 145 | * @param timeStart The time at which to start movement at the start position. 146 | * @return The found path (positions and respective arrival times). 147 | */ 148 | std::vector getShortestPath(const Vector3d& start, const Vector3d& goal, double timeStart = 0.0) const; 149 | 150 | private: 151 | /** 152 | * @brief Compute time availability for a position. 153 | * @param position The position. 154 | * @return The time availability (set of intervals). 155 | */ 156 | std::vector getTimeAvailability(const Vector3d& position) const; 157 | 158 | private: 159 | TemporalGraph m_graph; ///< The underlying graph. 160 | 161 | Vector3d m_environmentMin = Vector3d(0., 0., 0.); ///< The minimum coordinates of the environment. 162 | Vector3d m_environmentMax = Vector3d(1., 1., 1.); ///< The maximum coordinates of the environment. 163 | 164 | std::vector> m_staticObstacles; ///< The static obstacles. 165 | std::vector> m_dynamicObstacles; ///< The dynamic obstacles. 166 | }; 167 | 168 | } /* namespace tprm */ 169 | 170 | #endif /* __TPRM_TEMPORAL_PRM_H__ */ -------------------------------------------------------------------------------- /src/config.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | namespace tprm { 42 | double HolonomicRobot::movement_speed = 0.5; 43 | } /* namespace tprm */ -------------------------------------------------------------------------------- /src/obstacle_base.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | namespace tprm { 42 | 43 | Obstacle::Obstacle(const Vector3d& COM) : m_COM(COM) {} 44 | 45 | Vector3d Obstacle::getCOM() const { 46 | return m_COM; 47 | } 48 | void Obstacle::setCOM(const Vector3d& COM) { 49 | m_COM = COM; 50 | } 51 | 52 | StaticObstacle::StaticObstacle(const Vector3d& COM) : Obstacle(COM) {} 53 | 54 | DynamicObstacle::DynamicObstacle(const Vector3d& COM, const Vector3d& velocity) : Obstacle(COM), m_velocity(velocity) {} 55 | 56 | Vector3d DynamicObstacle::getVelocity() const { 57 | return m_velocity; 58 | } 59 | 60 | void DynamicObstacle::setVelocity(const Vector3d& velocity) { 61 | m_velocity = velocity; 62 | } 63 | 64 | Vector3d DynamicObstacle::getCOM(double time) const { 65 | return m_COM + m_velocity * time; 66 | } 67 | 68 | } /* namespace tprm */ -------------------------------------------------------------------------------- /src/obstacle_impl.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | namespace tprm { 42 | 43 | StaticSphereObstacle::StaticSphereObstacle(const Eigen::Vector3d& center, double radius) : StaticObstacle(center), m_radius(radius) {} 44 | 45 | bool StaticSphereObstacle::isColliding(const Vector3d& point) const { 46 | return (point - m_COM).squaredNorm() <= m_radius * m_radius; 47 | } 48 | 49 | bool StaticSphereObstacle::isColliding(const Vector3d& segment_from, const Vector3d& segment_to) const { 50 | if (isColliding(segment_from)) 51 | return true; 52 | if (isColliding(segment_to)) 53 | return true; 54 | 55 | const double r2 = m_radius * m_radius; 56 | 57 | // compute the closest point on the segment 58 | Vector3d p2m1 = segment_to - segment_from; 59 | Vector3d p1mc = segment_from - m_COM; 60 | 61 | double dot = p1mc.dot(p2m1); 62 | double squarednorm = p2m1.squaredNorm(); 63 | 64 | // clamp down t to the segment 65 | double t = -1. * (dot / squarednorm); 66 | 67 | if (t < 0.) 68 | t = 0.; 69 | if (t > 1.) 70 | t = 1.; 71 | Vector3d closest = segment_from + p2m1 * t; 72 | return ((m_COM - closest).squaredNorm() <= r2); 73 | } 74 | 75 | DynamicSphereObstacle::DynamicSphereObstacle(const Vector3d& COM, const Vector3d& velocity, double radius) : DynamicObstacle(COM, velocity), m_radius(radius) {} 76 | 77 | bool DynamicSphereObstacle::isColliding(const Vector3d& point, double& hitTimeFrom, double& hitTimeTo) const { 78 | if (getVelocity().squaredNorm() < std::numeric_limits::epsilon()) { 79 | if ((m_COM - point).squaredNorm() < m_radius * m_radius) { 80 | hitTimeFrom = 0.0; 81 | hitTimeTo = std::numeric_limits::infinity(); 82 | return true; 83 | } else { 84 | return false; 85 | } 86 | } 87 | 88 | const double maxTime = 10000; // max time to check for collision 89 | 90 | Vector3d d = -getVelocity() * maxTime; 91 | Vector3d f = point - m_COM; 92 | double a = d.squaredNorm(); 93 | double b = 2.0 * f.dot(d); 94 | double c = f.squaredNorm() - m_radius * m_radius; 95 | double discriminant = b * b - 4.0 * a * c; 96 | if (discriminant < 0) 97 | return false; 98 | double t1 = (-b - std::sqrt(discriminant)) / (2.0 * a); 99 | double t2 = (-b + std::sqrt(discriminant)) / (2.0 * a); 100 | if (t1 < 0 && t2 < 0) 101 | return false; 102 | if (t1 < 0) { 103 | hitTimeFrom = 0; 104 | hitTimeTo = t2 * maxTime; 105 | } else if (t2 < 0) { 106 | hitTimeFrom = t1 * maxTime; 107 | hitTimeTo = maxTime; 108 | } else { 109 | hitTimeFrom = t1 * maxTime; 110 | hitTimeTo = t2 * maxTime; 111 | } 112 | return true; 113 | } 114 | 115 | } /* namespace tprm */ -------------------------------------------------------------------------------- /src/temporal_graph.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | namespace tprm { 46 | 47 | TimeAvailability::TimeAvailability(double start_time, double end_time) : start(start_time), end(end_time) { 48 | if (start > end) { 49 | throw std::invalid_argument("start_time > end_time"); 50 | } 51 | if (start < 0.0) { 52 | throw std::invalid_argument("start_time < 0.0"); 53 | } 54 | if (end < 0.0) { 55 | throw std::invalid_argument("end_time < 0.0"); 56 | } 57 | } 58 | 59 | bool TemporalGraphNode::isActiveAt(double time) const { 60 | if (time < 0.0) { 61 | throw std::invalid_argument("time < 0.0"); 62 | } 63 | for (const TimeAvailability& time_availability : time_availabilities) { 64 | if (time >= time_availability.start && time <= time_availability.end) { 65 | return true; 66 | } 67 | } 68 | return false; 69 | } 70 | 71 | int TemporalGraphEdge::getOtherNodeId(int node_id) const { 72 | if (node_id == node_a_id) { 73 | return node_b_id; 74 | } else if (node_id == node_b_id) { 75 | return node_a_id; 76 | } else { 77 | throw std::runtime_error("Node id not found in edge"); 78 | } 79 | } 80 | 81 | TemporalGraph::TemporalGraph() { 82 | m_edge_cost_function = std::bind(&TemporalGraph::defaultEdgeCostFunction, this, std::placeholders::_1); 83 | } 84 | 85 | void TemporalGraph::clear() { 86 | m_nodes.clear(); 87 | m_edges.clear(); 88 | } 89 | 90 | int TemporalGraph::addNode(const TemporalGraphNode& node) { 91 | m_nodes.push_back(node); 92 | return m_nodes.size() - 1; 93 | } 94 | 95 | int TemporalGraph::addEdge(const TemporalGraphEdge& edge) { 96 | m_edges.push_back(edge); 97 | return m_edges.size() - 1; 98 | } 99 | 100 | const TemporalGraphNode& TemporalGraph::getNode(int node_id) const { 101 | if (node_id < 0 || node_id >= m_nodes.size()) { 102 | throw std::runtime_error("Node id not found"); 103 | } 104 | return m_nodes[node_id]; 105 | } 106 | 107 | TemporalGraphNode& TemporalGraph::getNode(int node_id) { 108 | if (node_id < 0 || node_id >= m_nodes.size()) { 109 | throw std::runtime_error("Node id not found"); 110 | } 111 | return m_nodes[node_id]; 112 | } 113 | 114 | const TemporalGraphEdge& TemporalGraph::getEdge(int edge_id) const { 115 | if (edge_id < 0 || edge_id >= m_edges.size()) { 116 | throw std::runtime_error("Edge id not found"); 117 | } 118 | return m_edges[edge_id]; 119 | } 120 | 121 | int TemporalGraph::getNumNodes() const { 122 | return m_nodes.size(); 123 | } 124 | 125 | int TemporalGraph::getNumEdges() const { 126 | return m_edges.size(); 127 | } 128 | 129 | int TemporalGraph::getClosestNode(const Vector3d& position) const { 130 | int closest_node_id = -1; 131 | double closest_node_distance = std::numeric_limits::max(); 132 | for (int node_id = 0; node_id < m_nodes.size(); node_id++) { 133 | double distance = (m_nodes[node_id].position - position).squaredNorm(); // use squared distance to avoid sqrt 134 | if (distance < closest_node_distance) { 135 | closest_node_distance = distance; 136 | closest_node_id = node_id; 137 | } 138 | } 139 | return closest_node_id; 140 | } 141 | 142 | /** 143 | * @brief Helper struct for special double. 144 | * 145 | * This struct allows to initialize an array / list / queue with infinite values. 146 | * Used in TemporalGraph::getShortestPath 147 | */ 148 | struct CustomDouble { 149 | double value = std::numeric_limits::infinity(); 150 | }; 151 | 152 | std::vector TemporalGraph::getShortestPath(int start_node_id, int end_node_id, double start_time) const { 153 | if (start_node_id < 0 || start_node_id >= m_nodes.size()) { 154 | throw std::runtime_error("Start node id not found"); 155 | } 156 | if (end_node_id < 0 || end_node_id >= m_nodes.size()) { 157 | throw std::runtime_error("End node id not found"); 158 | } 159 | 160 | std::vector> edge_buckets( 161 | m_nodes.size(), std::vector()); // bucket for each node. Each bucket contains all edges that start at the node 162 | for (const TemporalGraphEdge& edge : m_edges) { 163 | edge_buckets[edge.node_a_id].push_back(edge); 164 | edge_buckets[edge.node_b_id].push_back(edge); 165 | } 166 | 167 | std::unordered_map predecessor_map; // predecessor of each node 168 | std::unordered_map g_costs; // g-cost of each node 169 | std::unordered_map f_costs; // f-cost of each node 170 | 171 | const TemporalGraphNode& goal_node = m_nodes[end_node_id]; 172 | 173 | std::function heuristic_function = [&](int node_id) { return m_edge_cost_function(TemporalGraphEdge(node_id, end_node_id)); }; 174 | 175 | // sort function for the open queue 176 | auto compare = [&](int a, int b) { return f_costs[a].value < f_costs[b].value; }; 177 | 178 | std::deque open_queue; 179 | std::unordered_set open_queue_set; 180 | std::unordered_set closed_list_set; 181 | 182 | std::unordered_map arrival_time; 183 | 184 | g_costs[start_node_id].value = 0.; // init this node by access (--> inf value) and set 0 185 | f_costs[start_node_id].value = heuristic_function(start_node_id); 186 | 187 | open_queue.push_back(start_node_id); 188 | open_queue_set.insert(start_node_id); 189 | arrival_time[start_node_id].value = start_time; 190 | 191 | auto handle_successor = [&](int current_id, int successor_id, double current_time, const TemporalGraphEdge& edge, double edge_time) { 192 | if (closed_list_set.find(successor_id) != closed_list_set.end()) { 193 | return; 194 | } 195 | 196 | double tentative_g_cost = g_costs[current_id].value + getEdgeCost(edge); 197 | 198 | auto open_queue_set_it = open_queue_set.find(successor_id); 199 | 200 | if (open_queue_set_it != open_queue_set.end()) { 201 | if (tentative_g_cost >= g_costs[successor_id].value) { 202 | return; 203 | } 204 | } 205 | 206 | predecessor_map[successor_id] = current_id; 207 | g_costs[successor_id].value = tentative_g_cost; 208 | f_costs[successor_id].value = g_costs[successor_id].value + heuristic_function(successor_id); 209 | arrival_time[successor_id].value = current_time + edge_time; 210 | 211 | if (open_queue_set_it == open_queue_set.end()) { 212 | open_queue.push_back(successor_id); 213 | open_queue_set.insert(successor_id); 214 | } 215 | }; 216 | 217 | while (!open_queue.empty()) { 218 | int current_id = open_queue.front(); 219 | open_queue.pop_front(); 220 | open_queue_set.erase(current_id); 221 | 222 | if (current_id == end_node_id) { 223 | break; 224 | } 225 | 226 | closed_list_set.insert(current_id); 227 | for (const TemporalGraphEdge& edge : edge_buckets[current_id]) { 228 | int successor_id = edge.getOtherNodeId(current_id); 229 | 230 | double current_time = arrival_time[current_id].value; 231 | double edge_time = (getNode(current_id).position - getNode(successor_id).position).norm() / HolonomicRobot::movement_speed; 232 | 233 | // check TA 234 | if (m_nodes[successor_id].isActiveAt(current_time + edge_time)) { 235 | handle_successor(current_id, successor_id, current_time, edge, edge_time); 236 | } 237 | } 238 | 239 | std::sort(open_queue.begin(), open_queue.end(), compare); 240 | } 241 | 242 | // check if goal is reached (arrival time < INF) 243 | if (arrival_time[end_node_id].value == std::numeric_limits::infinity()) { 244 | return std::vector(); 245 | } 246 | 247 | // reconstruct path by backtracking 248 | std::vector path; 249 | int current_id = end_node_id; 250 | while (current_id != start_node_id) { 251 | path.push_back(GraphPathResultEntry(current_id, arrival_time[current_id].value)); 252 | current_id = predecessor_map[current_id]; 253 | } 254 | path.push_back(GraphPathResultEntry(current_id, arrival_time[current_id].value)); 255 | 256 | std::reverse(path.begin(), path.end()); 257 | return path; 258 | } 259 | 260 | void TemporalGraph::setEdgeCostFunction(std::function edge_cost_function) { 261 | m_edge_cost_function = edge_cost_function; 262 | } 263 | 264 | double TemporalGraph::defaultEdgeCostFunction(const TemporalGraphEdge& edge) const { 265 | return (getNode(edge.node_a_id).position - getNode(edge.node_b_id).position).norm(); 266 | }; 267 | 268 | double TemporalGraph::getEdgeCost(const TemporalGraphEdge& edge) const { 269 | return m_edge_cost_function(edge); 270 | } 271 | 272 | } /* namespace tprm */ -------------------------------------------------------------------------------- /src/temporal_prm.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * 3 | * Software License Agreement (BSD License) 4 | * 5 | * Copyright (c) 2022, 6 | * ETH Zurich - V4RL, Department of Mechanical and Process Engineering. 7 | * All rights reserved. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the institute nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | * Authors: Matthias Busenhart 37 | *********************************************************************/ 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | namespace tprm { 45 | 46 | TemporalPRM::TemporalPRM(const Vector3d& environmentMin, const Vector3d& environmentMax) : m_environmentMin(environmentMin), m_environmentMax(environmentMax) {} 47 | 48 | void TemporalPRM::setEnvironmentMin(const Vector3d& environmentMin) { 49 | m_environmentMin = environmentMin; 50 | } 51 | void TemporalPRM::setEnvironmentMax(const Vector3d& environmentMax) { 52 | m_environmentMax = environmentMax; 53 | } 54 | 55 | void TemporalPRM::addStaticObstacle(const std::shared_ptr& obstacle) { 56 | m_staticObstacles.push_back(obstacle); 57 | } 58 | 59 | void TemporalPRM::addDynamicObstacle(const std::shared_ptr& obstacle) { 60 | m_dynamicObstacles.push_back(obstacle); 61 | } 62 | 63 | void TemporalPRM::placeSamples(int numNodes) { 64 | const Vector3d half = Vector3d::Constant(0.5); 65 | const Vector3d envSize = m_environmentMax - m_environmentMin; 66 | for (int i = 0; i < numNodes; i++) { 67 | bool is_blocked = true; 68 | 69 | Vector3d sample = (Vector3d::Random() * 0.5 + half).cwiseProduct(envSize) + m_environmentMin; 70 | while (is_blocked) { 71 | is_blocked = false; 72 | for (const auto& obstacle : m_staticObstacles) { 73 | if (obstacle->isColliding(sample)) { 74 | is_blocked = true; 75 | sample = (Vector3d::Random() * 0.5 + half).cwiseProduct(envSize) + m_environmentMin; 76 | break; 77 | } 78 | } 79 | } 80 | m_graph.addNode(TemporalGraphNode(sample, getTimeAvailability(sample))); 81 | } 82 | } 83 | 84 | void TemporalPRM::buildPRM(double costEdgeThreshold) { 85 | const int loop_end = m_graph.getNumNodes(); 86 | #pragma omp parallel for schedule(dynamic, 1) shared(m_graph) firstprivate(costEdgeThreshold, m_staticObstacles, loop_end) default(none) 87 | for (int i = 0; i < loop_end; i++) { 88 | for (int j = i + 1; j < loop_end; j++) { 89 | const TemporalGraphNode& node_i = m_graph.getNode(i); 90 | const TemporalGraphNode& node_j = m_graph.getNode(j); 91 | 92 | double cost = m_graph.getEdgeCost(TemporalGraphEdge(i, j)); 93 | if (cost > costEdgeThreshold) { 94 | continue; 95 | } 96 | 97 | bool is_blocked = false; 98 | for (const auto& obstacle : m_staticObstacles) { 99 | if (obstacle->isColliding(node_i.position, node_j.position)) { 100 | is_blocked = true; 101 | break; 102 | } 103 | } 104 | if (is_blocked) { 105 | continue; 106 | } 107 | 108 | #pragma omp critical 109 | m_graph.addEdge(TemporalGraphEdge(i, j)); 110 | } 111 | } 112 | } 113 | 114 | void TemporalPRM::recomputeTimeAvailabilities() { 115 | for (int i = 0; i < m_graph.getNumNodes(); i++) { 116 | m_graph.getNode(i).time_availabilities = getTimeAvailability(m_graph.getNode(i).position); 117 | } 118 | } 119 | 120 | TemporalGraph& TemporalPRM::getTemporalGraph() { 121 | return m_graph; 122 | } 123 | 124 | std::vector TemporalPRM::getTimeAvailability(const Vector3d& position) const { 125 | if (m_dynamicObstacles.empty()) { 126 | return {TimeAvailability(0, std::numeric_limits::infinity())}; 127 | } 128 | 129 | std::vector time_availabilities; 130 | struct HitInfo { 131 | double t_start, t_end; 132 | }; 133 | 134 | std::vector hit_infos; 135 | for (const auto& obstacle : m_dynamicObstacles) { 136 | double t_start, t_end; 137 | if (obstacle->isColliding(position, t_start, t_end)) { 138 | hit_infos.push_back({t_start, t_end}); 139 | } 140 | } 141 | 142 | time_availabilities.push_back({0, std::numeric_limits::infinity()}); 143 | 144 | if (hit_infos.empty()) { 145 | return time_availabilities; 146 | } 147 | 148 | auto apply_obstacle_hit_to_interval = [&time_availabilities](const HitInfo& hi, TimeAvailability& interval) { 149 | if (hi.t_start > interval.start && hi.t_end < interval.end) { 150 | // create two intervals from this 151 | // this: slice.from_time, hi.t_start 152 | // new: hi.t_end, slice.to_time 153 | TimeAvailability new_interval(hi.t_end, interval.end); 154 | interval.end = hi.t_start; 155 | time_availabilities.push_back(new_interval); 156 | } else if (hi.t_start > interval.start && hi.t_start < interval.end) { 157 | // create an interval from this 158 | // this: slice.from_time, hi.t_start 159 | interval.end = hi.t_start; 160 | } else if (hi.t_end > interval.start && hi.t_end < interval.end) { 161 | // create an interval from this 162 | // this: hi.t_end, slice.to_time 163 | interval.start = hi.t_end; 164 | } else if (hi.t_start < interval.start && hi.t_end > interval.end) { 165 | // do nothing 166 | } else if (fabs(hi.t_start - interval.start) < 1e-6 && fabs(hi.t_end - interval.end) < 1e-6) { 167 | // this point is blocked any time by the moving obstacle. 168 | return false; 169 | } 170 | return true; 171 | }; 172 | 173 | for (size_t i = 0; i < time_availabilities.size(); i++) { // will be increased in this loop, therefore i indexed 174 | for (const auto& hi : hit_infos) { // does not change, therefore range loop 175 | if (!apply_obstacle_hit_to_interval(hi, time_availabilities[i])) { 176 | return {}; 177 | } 178 | } 179 | } 180 | return time_availabilities; 181 | } 182 | 183 | std::vector TemporalPRM::getShortestPath(const Vector3d& start, const Vector3d& goal, double timeStart) const { 184 | int closest_start_id = m_graph.getClosestNode(start); 185 | int closest_goal_id = m_graph.getClosestNode(goal); 186 | 187 | const TemporalGraphNode& start_node = m_graph.getNode(closest_start_id); 188 | const TemporalGraphNode& goal_node = m_graph.getNode(closest_goal_id); 189 | 190 | // try to connect start and goal to their closest nodes 191 | // check static obstacles 192 | bool is_blocked = false; 193 | for (const auto& obstacle : m_staticObstacles) { 194 | if (obstacle->isColliding(start, start_node.position)) { 195 | is_blocked = true; 196 | break; 197 | } 198 | } 199 | if (is_blocked) { 200 | return {}; 201 | } 202 | is_blocked = false; 203 | for (const auto& obstacle : m_staticObstacles) { 204 | if (obstacle->isColliding(goal, goal_node.position)) { 205 | is_blocked = true; 206 | break; 207 | } 208 | } 209 | if (is_blocked) { 210 | return {}; 211 | } 212 | 213 | // check dynamic obstacles 214 | TemporalGraphNode tmp_start(start, getTimeAvailability(start)); 215 | if (!tmp_start.isActiveAt(timeStart)) { 216 | return {}; 217 | } 218 | 219 | double time_to_closest_start = (start - start_node.position).norm() / HolonomicRobot::movement_speed; 220 | 221 | auto path = m_graph.getShortestPath(closest_start_id, closest_goal_id, timeStart + time_to_closest_start); 222 | if (path.empty()) { 223 | return {}; 224 | } 225 | 226 | TemporalGraphNode tmp_goal(goal, getTimeAvailability(goal)); 227 | double time_from_closest_goal = (goal - goal_node.position).norm() / HolonomicRobot::movement_speed; 228 | if (!tmp_goal.isActiveAt(path.back().time + time_from_closest_goal)) { 229 | return {}; 230 | } 231 | 232 | std::vector result; 233 | // push back custom start 234 | result.push_back(PathResultEntry(start, timeStart)); 235 | for (const auto& entry : path) { 236 | result.push_back(PathResultEntry(m_graph.getNode(entry.node_id).position, entry.time)); 237 | } 238 | result.push_back(PathResultEntry(goal, path.back().time + time_from_closest_goal)); 239 | return result; 240 | } 241 | 242 | } /* namespace tprm */ --------------------------------------------------------------------------------