├── .gitignore ├── LICENSE ├── README.md ├── demo ├── CMakeLists.txt ├── README.md ├── configure.sh ├── holonomic_2d_point_planning.cpp ├── holonomic_2d_point_scenario.hpp ├── link_demo.html ├── link_manipulator_planning.cpp ├── link_manipulator_scenario.hpp ├── nao_cup │ ├── README │ └── src │ │ ├── alloc.hpp │ │ ├── collide.hpp │ │ ├── hrtimer.hpp │ │ ├── kdtree.hpp │ │ ├── linear.hpp │ │ ├── naocup.hpp │ │ └── prrts.hpp ├── nao_cup_planning.cpp ├── png_2d_planning.cpp ├── png_2d_scenario.hpp ├── png_planning_input.png ├── proc_info.hpp ├── scenario_config.hpp ├── se3_rigid_body_planning.cpp ├── se3_rigid_body_scenario.hpp └── shape_hierarchy.hpp ├── src └── mpt │ ├── box_bounds.hpp │ ├── cartesian_bounds.hpp │ ├── cartesian_space.hpp │ ├── discrete_motion_validator.hpp │ ├── goal_sampler.hpp │ ├── goal_state.hpp │ ├── impl │ ├── always_false.hpp │ ├── atom.hpp │ ├── condition_timer.hpp │ ├── condition_timer_posix.hpp │ ├── constants.hpp │ ├── count_down_latch.hpp │ ├── djikstras.hpp │ ├── finally.hpp │ ├── goal_has_sampler.hpp │ ├── has_trajectory_callback.hpp │ ├── is_invocable.hpp │ ├── link.hpp │ ├── link_trajectory.hpp │ ├── metrics.hpp │ ├── nearest_strategy.hpp │ ├── object_pool.hpp │ ├── pack_nearest.hpp │ ├── packs.hpp │ ├── planner_base.hpp │ ├── pprm │ │ ├── component.hpp │ │ ├── edge.hpp │ │ ├── node.hpp │ │ └── pprm.hpp │ ├── pprm_irs │ │ ├── component.hpp │ │ ├── edge.hpp │ │ ├── node.hpp │ │ ├── pprm_irs.hpp │ │ └── shortest_path_check.hpp │ ├── prrt │ │ ├── edge.hpp │ │ ├── node.hpp │ │ └── prrt.hpp │ ├── prrt_star │ │ ├── edge.hpp │ │ ├── edge_data.hpp │ │ ├── node.hpp │ │ └── prrt_star.hpp │ ├── rrg_rewire_neighbors.hpp │ ├── scenario_bounds.hpp │ ├── scenario_goal.hpp │ ├── scenario_goal_sampler.hpp │ ├── scenario_link.hpp │ ├── scenario_rng.hpp │ ├── scenario_sampler.hpp │ ├── scenario_space.hpp │ ├── scenario_state.hpp │ ├── thread_pool.hpp │ ├── timer_stat.hpp │ ├── uniform_sampler_cartesian.hpp │ ├── uniform_sampler_lp.hpp │ ├── uniform_sampler_scaled.hpp │ ├── uniform_sampler_so2.hpp │ ├── uniform_sampler_so3.hpp │ ├── worker_pool.hpp │ ├── worker_pool_openmp.hpp │ └── worker_pool_std_thread.hpp │ ├── log.hpp │ ├── lp_space.hpp │ ├── mersenne_twister.hpp │ ├── planner.hpp │ ├── planner_tags.hpp │ ├── pprm.hpp │ ├── pprm_irs.hpp │ ├── prrt.hpp │ ├── prrt_star.hpp │ ├── random_device_seed.hpp │ ├── scaled_space.hpp │ ├── scenario.hpp │ ├── se2_space.hpp │ ├── se3_space.hpp │ ├── so2_space.hpp │ ├── so3_space.hpp │ ├── unbounded.hpp │ ├── uniform_box_sampler.hpp │ └── uniform_sampler.hpp └── test ├── CMakeLists.txt ├── README.md ├── bounds_test.cpp ├── count_down_latch_test.cpp ├── djikstras_test.cpp ├── goal_has_sampler_test.cpp ├── log_test.cpp ├── lp_space_test.cpp ├── mersenne_twister_test.cpp ├── pack_nearest_test.cpp ├── packs_test.cpp ├── planner_integration_test.hpp ├── pprm_integration_test.cpp ├── pprm_irs_integration_test.cpp ├── prrt_integration_test.cpp ├── prrt_star_integration_test.cpp ├── random_device_seed_test.cpp ├── scaled_space_test.cpp ├── scenario_sampler_test.cpp ├── se2_space_test.cpp ├── se3_space_test.cpp ├── so2_space_test.cpp ├── so3_space_test.cpp ├── test.hpp ├── thread_pool_test.cpp ├── uniform_sampler_lp_test.cpp ├── uniform_sampler_so3_test.cpp ├── worker_pool_openmp_test.cpp ├── worker_pool_std_thread_test.cpp └── worker_pool_test_common.hpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | build.ninja 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2018 The University of North Carolina at Chapel Hill 2 | 3 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 4 | 5 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 6 | 7 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 8 | 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | 11 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 12 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [Motion Planning Templates (MPT)](http://robotics.cs.unc.edu/mpt) 2 | =============================== 3 | 4 | [Motion Planning Templates (MPT)](http://robotics.cs.unc.edu/mpt) is a collection of C++ template classes for fast, parallel, robot-specific motion planning. MPT uses C++ templates to generate and optimize motion planners at compile-time. The result is that MPT planners are fast, have reduced memory requirements, predictable throughput, and reduced memory fragmentation. It aims to be as efficient as if one were to custom write a planner for a single robot. 5 | -------------------------------------------------------------------------------- /demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 3.10 introduces C++ 17 support 2 | cmake_minimum_required (VERSION 3.10) 3 | project (mpt_demos) 4 | 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | set(CMAKE_CXX_EXTENSIONS OFF) 8 | 9 | find_package(Eigen3 3.3 REQUIRED NO_MODULE) 10 | find_package(OpenMP REQUIRED) 11 | #find_package(FCL REQUIRED) 12 | find_package(Assimp REQUIRED) 13 | 14 | find_package(PNG REQUIRED) 15 | 16 | ######### Find CCD the hard way 17 | find_package(PkgConfig) 18 | if (PKGCONFIG_FOUND) 19 | pkg_check_modules(FCL fcl>=0.6) 20 | pkg_check_modules(CCD ccd>=2.0) 21 | else() 22 | find_package(FCL REQUIRED) 23 | find_package(CCD CONFIG) 24 | endif() 25 | ######### 26 | 27 | link_directories(${FCL_LIBRARY_DIRS} ${ASSIMP_LIBRARY_DIRS} ${CCD_LIBRARY_DIRS}) 28 | include_directories(../src ../../nigh/src ${FCL_INCLUDE_DIRS} ${CCD_LIBRARY_DIRS} ${ASSIMP_INCLUDE_DIRS} ${PNG_INCLUDE_DIR}) 29 | 30 | if (OPENMP_FOUND) 31 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 32 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 33 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 34 | endif() 35 | 36 | 37 | foreach (nn KDTreeBatch GNAT) 38 | foreach (scalar float double) 39 | foreach (mt 0 1) 40 | if (mt) 41 | set(mtname mt) 42 | else() 43 | set(mtname st) 44 | endif() 45 | 46 | set(trgt nao_cup_planning-${nn}-${scalar}-${mtname}) 47 | add_executable(${trgt} nao_cup_planning.cpp) 48 | target_compile_definitions(${trgt} PRIVATE NN_TYPE=${nn} SCALAR_TYPE=${scalar} MT=1) 49 | target_link_libraries(${trgt} Eigen3::Eigen) 50 | 51 | set(trgt se3_rigid_body_planning-${nn}-${scalar}-${mtname}) 52 | add_executable(${trgt} se3_rigid_body_planning.cpp) 53 | target_compile_definitions(${trgt} PRIVATE NN_TYPE=${nn} SCALAR_TYPE=${scalar} MT=1) 54 | target_link_libraries(${trgt} Eigen3::Eigen ${ASSIMP_LIBRARIES} ${FCL_LIBRARIES} ${CCD_LIBRARIES}) 55 | 56 | endforeach(mt) 57 | endforeach(scalar) 58 | endforeach(nn) 59 | 60 | 61 | set(trgt holonomic_2d_point_planning) 62 | add_executable(${trgt} holonomic_2d_point_planning.cpp) 63 | target_link_libraries(${trgt} Eigen3::Eigen) 64 | 65 | set(trgt png_2d_planning) 66 | add_executable(${trgt} png_2d_planning.cpp) 67 | target_link_libraries(${trgt} Eigen3::Eigen ${PNG_LIBRARY}) 68 | 69 | set(trgt link_maipulator_planning) 70 | add_executable(${trgt} link_manipulator_planning.cpp) 71 | target_link_libraries(${trgt} Eigen3::Eigen) 72 | -------------------------------------------------------------------------------- /demo/nao_cup/README: -------------------------------------------------------------------------------- 1 | This directory contains source files from 2 | 3 | https://github.com/jeffi/prrts-c 4 | 5 | 6 | -------------------------------------------------------------------------------- /demo/nao_cup/src/alloc.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifndef ALLOC_H 37 | #define ALLOC_H 38 | 39 | #include 40 | #include 41 | 42 | 43 | namespace nao_cup { 44 | /* 64 is a fairly standard cache-line size in x86, but this should be 45 | * parameterized elsewhere. */ 46 | #define CACHE_LINE_SIZE 64 47 | 48 | #define ALIGN_UP(x, b) (((size_t)(x) + (b-1)) & ~((size_t)(b-1))) 49 | #define CACHE_ALIGN(x) ALIGN_UP(x, CACHE_LINE_SIZE) 50 | 51 | 52 | static inline int 53 | aligned_offset(void *ptr, size_t b) 54 | { 55 | size_t address = (size_t)ptr; 56 | return (ALIGN_UP(address, b) - address); 57 | } 58 | 59 | 60 | 61 | 62 | typedef struct tl_mempool { 63 | pthread_key_t key; 64 | size_t chunk_size; 65 | } tl_mempool_t; 66 | 67 | #define struct_alloc(type) ((type *)malloc(sizeof(type))) 68 | #define array_alloc(type, count) ((type *)malloc(sizeof(type) * count)) 69 | #define array_grow(array, count) ((typeof(array))realloc(array, sizeof(array[0]) * count)); 70 | #define array_copy(array, count) memdup(array, sizeof(array[0]) * count) 71 | 72 | int tl_mempool_init(tl_mempool_t *pool, size_t chunk_size); 73 | int tl_mempool_destroy(tl_mempool_t *pool); 74 | void *tl_alloc(tl_mempool_t *pool, size_t size); 75 | void tl_free(tl_mempool_t *pool, void *ptr); 76 | 77 | void *memdup(const void *source, size_t bytes); 78 | 79 | } 80 | 81 | #endif /* ALLOC_H */ 82 | -------------------------------------------------------------------------------- /demo/nao_cup/src/hrtimer.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifndef HRTIMER_HPP 37 | #define HRTIMER_HPP 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | namespace nao_cup { 45 | inline hrtimer_t hrtimer_get() { 46 | #ifdef CLOCK_REALTIME 47 | struct timespec now; 48 | hrtimer_t t; 49 | 50 | if (clock_gettime(CLOCK_REALTIME, &now) != 0) { 51 | warn("clock_gettime failed"); 52 | return -1; 53 | } 54 | 55 | t = now.tv_sec; 56 | t *= 1000000000L; 57 | t += now.tv_nsec; 58 | return t; 59 | #else 60 | struct timeval now; 61 | hrtimer_t t; 62 | 63 | if (gettimeofday(&now, NULL) == -1) { 64 | warn("gettimeofday failed"); 65 | return -1; 66 | } 67 | 68 | t = now.tv_sec; 69 | t *= 1000000000L; 70 | t += now.tv_usec * 1000L; 71 | return t; 72 | #endif 73 | } 74 | 75 | } 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /demo/nao_cup/src/kdtree.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | 37 | #ifndef KD_TREE_H 38 | #define KD_TREE_H 39 | 40 | /* for size_t */ 41 | #include 42 | #include 43 | #include "alloc.hpp" 44 | 45 | #ifdef CHECK_CRCS 46 | #include "crc.h" 47 | #endif 48 | 49 | namespace nao_cup { 50 | 51 | typedef double (*kd_dist_func)(const double *a, const double *b); 52 | typedef void (*kd_near_callback)(void *data, int no, void *value, double dist); 53 | 54 | typedef struct kd_tree { 55 | size_t dimensions; 56 | const double *min; 57 | const double *max; 58 | struct kd_node *root; 59 | kd_dist_func dist_func; 60 | tl_mempool_t mempool; 61 | } kd_tree_t; 62 | 63 | kd_tree_t *kd_create_tree(size_t dimensions, const double *min, const double *max, 64 | kd_dist_func dist_func, 65 | const double *root_config, void *root_value); 66 | void kd_insert(kd_tree_t *t, const double *config, void *value); 67 | void *kd_nearest(kd_tree_t *t, const double *target, double *dist); 68 | int kd_near(kd_tree_t *t, const double *target, double radius, kd_near_callback callback, void *cb_data); 69 | 70 | } 71 | 72 | #endif 73 | -------------------------------------------------------------------------------- /demo/nao_cup/src/prrts.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifndef PRRTS_H 37 | #define PRRTS_H 38 | 39 | #include "kdtree.hpp" 40 | 41 | namespace nao_cup { 42 | #define REGION_SPLIT_AXIS 0 43 | #define INITIAL_NEAR_LIST_CAPACITY 1024 44 | 45 | template 46 | using prrts_in_goal_func = bool (*)(void *system, const S *config); 47 | template 48 | using prrts_clear_func = bool (*)(void *system, const S *config); 49 | template 50 | using prrts_link_func = bool (*)(void *system, const S *a, const S *b); 51 | template 52 | using prrts_system_data_alloc_func = void *(*)(int thread_no, const S *sample_min, const S *sample_max); 53 | 54 | typedef void (*prrts_system_data_free_func)(void *system); 55 | 56 | template 57 | struct prrts_system { 58 | size_t dimensions; 59 | 60 | const S *init; 61 | const S *min; 62 | const S *max; 63 | const S *target; 64 | 65 | prrts_system_data_alloc_func system_data_alloc_func; 66 | prrts_system_data_free_func system_data_free_func; 67 | 68 | kd_dist_func dist_func; 69 | prrts_in_goal_func in_goal_func; 70 | prrts_clear_func clear_func; 71 | prrts_link_func link_func; 72 | }; 73 | 74 | // typedef struct prrts_options { 75 | // double gamma; 76 | // bool regional_sampling; 77 | // int samples_per_step; 78 | // } prrts_options_t; 79 | 80 | // typedef struct prrts_solution { 81 | // double path_cost; 82 | // size_t path_length; 83 | // const double *configs[0]; 84 | // } prrts_solution_t; 85 | 86 | // prrts_solution_t* prrts_run_for_duration(prrts_system_t *system, prrts_options_t *options, int thread_count, long duration); 87 | // prrts_solution_t* prrts_run_for_samples(prrts_system_t *system, prrts_options_t *options, int thread_count, size_t sample_count); 88 | // prrts_solution_t* prrts_run_indefinitely(prrts_system_t *system, prrts_options_t *options, int thread_count); 89 | 90 | } 91 | 92 | #endif /* PRRTS_H */ 93 | -------------------------------------------------------------------------------- /demo/png_planning_input.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/UNC-Robotics/mpt/30360d64235b3aee36ce8739e42dfb344f6a032c/demo/png_planning_input.png -------------------------------------------------------------------------------- /demo/proc_info.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifndef MPT_DEMO_PROC_INFO_HPP 37 | #define MPT_DEMO_PROC_INFO_HPP 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #ifdef linux 45 | #include 46 | #endif 47 | 48 | namespace mpt_demo { 49 | class ProcInfo { 50 | public: 51 | template 52 | friend decltype(auto) operator<<( 53 | std::basic_ostream& out, const ProcInfo& p) 54 | { 55 | 56 | // TODO: need to find OSX equivalent 57 | #ifdef linux 58 | struct mallinfo mi = mallinfo(); 59 | 60 | // std::cout << std::flush; 61 | // std::cerr << "==== malloc_stats ===== " << std::endl; 62 | // malloc_stats(); 63 | // std::cerr << "==== malloc_stats end =====" << std::endl; 64 | 65 | out << 66 | // arena is also called "system bytes" 67 | // and is equivalent to mi.uordblks + mi.fordblks 68 | "Non-mmapped space allocated (bytes): " << mi.arena << "\n" 69 | "Space allocated in mmapped regions (bytes): " << mi.hblkhd << "\n" 70 | "Maximum total allocated space (bytes): " << mi.usmblks << "\n" 71 | "Total allocated space (bytes): " << mi.uordblks << "\n" 72 | "Total free space (bytes): " << mi.fordblks << "\n" 73 | "Top-most, releasable space (bytes): " << mi.keepcost << "\n"; 74 | #endif 75 | 76 | #if 0 77 | pid_t pid_{getpid()}; 78 | std::string fileName = 79 | "/proc/" + std::to_string(pid_) + "/status"; 80 | 81 | std::ifstream fin(fileName); 82 | 83 | if (!fin) 84 | throw std::runtime_error("failed to open: "+fileName); 85 | 86 | fin.unsetf(std::ios_base::skipws); 87 | 88 | std::copy( 89 | std::istreambuf_iterator(fin), 90 | std::istreambuf_iterator(), 91 | std::ostream_iterator(out)); 92 | #endif 93 | return out; 94 | } 95 | }; 96 | } 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /src/mpt/box_bounds.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_BOX_BOUNDS_HPP 38 | #define MPT_BOX_BOUNDS_HPP 39 | 40 | #include 41 | #include 42 | 43 | namespace unc::robotics::mpt { 44 | template 45 | class BoxBounds { 46 | Eigen::Matrix bounds_; 47 | 48 | void checkBounds() { 49 | if (!bounds_.allFinite()) 50 | throw std::domain_error("non-finite bounds"); 51 | 52 | if ((bounds_.col(0).array() > bounds_.col(1).array()).any()) 53 | throw std::invalid_argument("bounds have min > max"); 54 | } 55 | 56 | public: 57 | BoxBounds(const BoxBounds& bounds) noexcept 58 | : bounds_(bounds.bounds_) 59 | { 60 | } 61 | 62 | template 63 | BoxBounds(const Eigen::DenseBase& bounds) 64 | : bounds_(bounds) 65 | { 66 | checkBounds(); 67 | } 68 | 69 | template 70 | BoxBounds( 71 | const Eigen::DenseBase& min, 72 | const Eigen::DenseBase& max) 73 | { 74 | bounds_.col(0) = min; 75 | bounds_.col(1) = max; 76 | 77 | checkBounds(); 78 | } 79 | 80 | decltype(auto) min() const { 81 | return bounds_.col(0); 82 | } 83 | 84 | decltype(auto) max() const { 85 | return bounds_.col(1); 86 | } 87 | 88 | S measure() const { 89 | return (max() - min()).prod(); 90 | } 91 | 92 | unsigned size() const { 93 | return bounds_.rows(); 94 | } 95 | }; 96 | } 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /src/mpt/cartesian_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_CARTESIAN_SPACE_HPP 38 | #define MPT_CARTESIAN_SPACE_HPP 39 | 40 | #include 41 | #include "impl/metrics.hpp" 42 | 43 | namespace unc::robotics::mpt::impl { 44 | template 45 | T cartesian_interpolate( 46 | const Space& space, const T& a, const T& b, 47 | typename Space::Distance t, 48 | std::index_sequence) 49 | { 50 | T q; 51 | ((cartesian_state_element::get(q) = interpolate( 52 | std::get(space), 53 | cartesian_state_element::get(a), 54 | cartesian_state_element::get(b), 55 | t)), ...); 56 | return q; 57 | } 58 | } 59 | 60 | namespace unc::robotics::mpt { 61 | template 62 | T interpolate( 63 | const Space>& space, const T& a, const T& b, 64 | typename Space>::Distance t) 65 | { 66 | return impl::cartesian_interpolate(space, a, b, t, std::index_sequence_for{}); 67 | } 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /src/mpt/goal_sampler.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_GOAL_SAMPLER_HPP 38 | #define MPT_GOAL_SAMPLER_HPP 39 | 40 | namespace unc::robotics::mpt { 41 | template 42 | class GoalSampler; 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/mpt/goal_state.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_GOAL_STATE_HPP 38 | #define MPT_GOAL_STATE_HPP 39 | 40 | #include "goal_sampler.hpp" 41 | #include 42 | 43 | namespace unc::robotics::mpt { 44 | template 45 | class GoalState { 46 | using State = typename Space::Type; 47 | using Distance = typename Space::Distance; 48 | 49 | State goal_; 50 | Distance radius_; 51 | 52 | public: 53 | template 54 | GoalState(Distance radius, Args&& ... args) 55 | : goal_(std::forward(args)...) 56 | , radius_(radius) 57 | { 58 | } 59 | 60 | const State& state() const { 61 | return goal_; 62 | } 63 | 64 | std::pair operator() (const Space& space, const State& q) const { 65 | Distance d = space.distance(q, goal_); 66 | return (d <= radius_) 67 | ? std::make_pair(true, Distance(0)) 68 | : std::make_pair(false, d - radius_); 69 | } 70 | }; 71 | 72 | template 73 | class GoalSampler> { 74 | const GoalState& goal_; 75 | 76 | public: 77 | using Type = typename Space::Type; 78 | 79 | GoalSampler(const GoalState& goal) 80 | : goal_(goal) 81 | { 82 | } 83 | 84 | template 85 | Type operator() (RNG&) const { 86 | // TODO: sample within radius 87 | return goal_.state(); 88 | } 89 | }; 90 | } 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /src/mpt/impl/always_false.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_ALWAYS_FALSE_HPP 38 | #define MPT_IMPL_ALWAYS_FALSE_HPP 39 | 40 | namespace unc::robotics::mpt::impl { 41 | // helper for static assertions when template specialization (or 42 | // lack of) determines that something is wrong. This allows the 43 | // code to include static_assert(always_false, "informative 44 | // message") and trigger the compiler to generate an informative 45 | // message. (static_assert(false, "...") always triggers, 46 | // regardless of specialization since the assertion value is not 47 | // based upon a parameter) 48 | 49 | template 50 | static constexpr bool always_false = false; 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/mpt/impl/constants.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_CONSTANTS_HPP 38 | #define MPT_IMPL_CONSTANTS_HPP 39 | 40 | namespace unc::robotics::mpt::impl { 41 | template constexpr T PI = T( 42 | 3.14159265358979323846264338327950288419716939937510582097494459230781640628620L); 43 | template constexpr T PI_2 = T( 44 | 1.57079632679489661923132169163975144209858469968755291048747229615390820314310L); 45 | template constexpr T E = T( 46 | 2.71828182845904523536028747135266249775724709369995957496696762772407663035355L); 47 | template constexpr T SQRT2 = T( 48 | 1.41421356237309504880168872420969807856967187537694807317667973799073247846211L); 49 | template constexpr T SQRT1_2 = T( 50 | 0.70710678118654752440084436210484903928483593768847403658833986899536623923105L); 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/mpt/impl/count_down_latch.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_COUNT_DOWN_LATCH_HPP 38 | #define MPT_IMPL_COUNT_DOWN_LATCH_HPP 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace unc::robotics::mpt::impl { 46 | 47 | // Synchronization primitive which blocks the wait() method until 48 | // a pre-specified number of countDown() calls are made. 49 | class CountDownLatch { 50 | std::atomic count_; 51 | 52 | // unfortunately this seems to be the method to have one 53 | // thread sleep and be awaken by the other. We don't really 54 | // need a mutex since our state fits in an atomic, but 55 | // condition variables require a mutex. 56 | mutable std::mutex mutex_; 57 | mutable std::condition_variable cv_; 58 | 59 | public: 60 | inline CountDownLatch(int n) 61 | : count_(n) 62 | { 63 | assert(n >= 0); 64 | } 65 | 66 | inline void countDown() { 67 | int before = count_.fetch_sub(1, std::memory_order_relaxed); 68 | 69 | // fetch_sub returns the value before subtraction, if we 70 | // already reached 0, then this is an extra call to 71 | // countDown(). 72 | assert(before != 0); 73 | 74 | if (before == 1) 75 | cv_.notify_all(); 76 | } 77 | 78 | inline void wait() const { 79 | std::unique_lock lock(mutex_); 80 | cv_.wait(lock, [&] { return count_.load(std::memory_order_relaxed) == 0; }); 81 | } 82 | }; 83 | } 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /src/mpt/impl/finally.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_FINALLY_HPP 38 | #define MPT_IMPL_FINALLY_HPP 39 | 40 | namespace unc::robotics::mpt::impl { 41 | // Finally and finally implement a lambda-based version of the 42 | // 'finally' block in other languages. In general this class and 43 | // method should be avoided in favor of RAII, but it is here for 44 | // quick one-offs. 45 | template 46 | class Finally : T { 47 | public: 48 | Finally(T&& t) : T(std::move(t)) {} 49 | Finally(const T& t) : T(t) {} 50 | ~Finally() { T::operator()(); } 51 | }; 52 | 53 | template 54 | Finally finally(T t) { 55 | return Finally(t); 56 | } 57 | } 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /src/mpt/impl/goal_has_sampler.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_GOAL_HAS_SAMPLER_HPP 38 | #define MPT_IMPL_GOAL_HAS_SAMPLER_HPP 39 | 40 | #include "../goal_sampler.hpp" 41 | #include 42 | 43 | namespace unc::robotics::mpt::impl { 44 | 45 | template 46 | struct goal_has_sampler : std::false_type {}; 47 | 48 | template 49 | struct goal_has_sampler::Type>> : std::true_type {}; 50 | 51 | template 52 | constexpr bool goal_has_sampler_v = goal_has_sampler::value; 53 | } 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /src/mpt/impl/has_trajectory_callback.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_HAS_SOLUTION_CALLBACK_HPP 38 | #define MPT_IMPL_HAS_SOLUTION_CALLBACK_HPP 39 | 40 | #include 41 | #include 42 | 43 | namespace unc::robotics::mpt::impl { 44 | template 45 | struct has_trajectory_callback : std::false_type {}; 46 | 47 | // If link() returns bool, then we do not consider it a 48 | // trajectory, and nothing is stored in the edge of graphs. 49 | template 50 | struct has_trajectory_callback : std::false_type {}; 51 | 52 | template 53 | struct has_trajectory_callback< 54 | Fn, State, Traj, 55 | std::void_t().operator() ( 56 | std::declval(), 57 | std::declval(), 58 | std::declval() ) )> > 59 | : std::true_type {}; 60 | 61 | template 62 | constexpr bool has_trajectory_callback_v = has_trajectory_callback::value; 63 | } 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /src/mpt/impl/is_invocable.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_IS_INVOCABLE_HPP 38 | #define MPT_IMPL_IS_INVOCABLE_HPP 39 | 40 | #include 41 | 42 | // clang 5 ships with libc++ that has is_callable instead of is_invocable 43 | 44 | #if defined(_LIBCPP_VERSION) && _LIBCPP_VERSION == 5000 45 | namespace std { 46 | template 47 | using is_invocable = is_callable; 48 | 49 | template 50 | constexpr bool is_invocable_v = is_invocable::value; 51 | } 52 | #endif 53 | 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /src/mpt/impl/link.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_LINK_HPP 38 | #define MPT_IMPL_LINK_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::mpt::impl { 43 | 44 | template 45 | class Link { 46 | T link_; 47 | public: 48 | Link(T&& link) : link_(std::move(link)) {} 49 | 50 | const T& link() const { 51 | return link_; 52 | } 53 | 54 | void setLink(T&& link) const { 55 | link_ = std::move(link); 56 | } 57 | }; 58 | 59 | template <> 60 | class Link { 61 | public: 62 | Link(std::monostate) {} 63 | 64 | const std::monostate link() const { 65 | return {}; 66 | } 67 | 68 | void setLink(std::monostate) const { 69 | } 70 | }; 71 | } 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /src/mpt/impl/metrics.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_METRICS_HPP 38 | #define MPT_IMPL_METRICS_HPP 39 | 40 | namespace unc::robotics::mpt { 41 | using namespace nigh::metric; 42 | } 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /src/mpt/impl/nearest_strategy.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_NEAREST_STRATEGY_HPP 38 | #define MPT_IMPL_NEAREST_STRATEGY_HPP 39 | 40 | #include "scenario_space.hpp" 41 | #include 42 | #include 43 | 44 | namespace unc::robotics::mpt::impl { 45 | template 46 | struct nearest_strategy_impl { 47 | // TODO: check that space and concurrency is supported by the 48 | // specified NN. 49 | using type = NN; 50 | }; 51 | 52 | template 53 | struct nearest_strategy_impl { 54 | using type = nigh::auto_strategy_t; 55 | }; 56 | 57 | // nearest_strategy<...> selects the nearest neighbor strategy 58 | // based upon the arguments. The Scenario defines the space, 59 | // maxThreads is the maximum number of threads, NN is the 60 | // configured strategy (void = auto detect), and ST is the 61 | // concurrency model when running single threaded. 62 | 63 | template 64 | struct nearest_strategy : nearest_strategy_impl< 65 | scenario_space_t, 66 | std::conditional_t, 67 | NN> 68 | { 69 | }; 70 | 71 | template 72 | using nearest_strategy_t = typename nearest_strategy::type; 73 | 74 | } 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /src/mpt/impl/pack_nearest.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_PACK_NEAREST_HPP 38 | #define MPT_IMPL_PACK_NEAREST_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::mpt::impl { 43 | template 44 | struct pack_nearest { 45 | using type = void; 46 | }; 47 | 48 | template 49 | struct pack_nearest : pack_nearest {}; 50 | 51 | template 52 | using pack_nearest_t = typename pack_nearest::type; 53 | 54 | template 55 | struct pack_nearest { 56 | using type = nigh::Linear; 57 | static_assert( 58 | std::is_void_v>, 59 | "multiple nearest neighbor strategies"); 60 | }; 61 | 62 | template 63 | struct pack_nearest, Rest...> { 64 | using type = nigh::KDTreeBatch; 65 | static_assert( 66 | std::is_void_v>, 67 | "multiple nearest neighbor strategies"); 68 | }; 69 | 70 | template < 71 | unsigned degree, unsigned minDegree, unsigned maxDegree, 72 | unsigned maxNumPtsPerLeaf, unsigned removedCacheSize, 73 | bool rebalancing, 74 | typename ... Rest> 75 | struct pack_nearest< 76 | nigh::GNAT, 77 | Rest...> 78 | { 79 | using type = nigh::GNAT; 80 | static_assert( 81 | std::is_void_v>, 82 | "multiple nearest neighbor strategies"); 83 | }; 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /src/mpt/impl/planner_base.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_PLANNER_BASE_HPP 38 | #define MPT_IMPL_PLANNER_BASE_HPP 39 | 40 | #include 41 | #include "condition_timer.hpp" 42 | 43 | namespace unc::robotics::mpt::impl { 44 | template 45 | class PlannerBase { 46 | public: 47 | // The time based solve methods use ConditionTimer instead of 48 | // SleepTimer, since it is more well-behaved in the case when 49 | // solve() terminates for a reason other than the doneFn 50 | // predicate returning true (e.g., an exception) 51 | 52 | // TODO: when solve(fn) is only going to call the done 53 | // predicate a few times, it may be better to just call 54 | // Clock::now() directly in the done predicate, instead of 55 | // incurring the overhead of starting another thread just for 56 | // the conditioned timed wait. 57 | 58 | template 59 | void solveFor(const std::chrono::duration& duration) { 60 | ConditionTimer timer(duration); 61 | static_cast(this)->solve(timer.doneFn()); 62 | } 63 | 64 | template 65 | void solveUntil(const std::chrono::time_point& endTime) { 66 | ConditionTimer timer(endTime); 67 | static_cast(this)->solve(timer.doneFn()); 68 | } 69 | 70 | template 71 | void solveFor(DoneFn doneFn, const std::chrono::duration& duration) { 72 | ConditionTimer timer(duration); 73 | static_cast(this)->solve(timer.doneFn(doneFn)); 74 | } 75 | 76 | template 77 | void solveUntil(DoneFn doneFn, const std::chrono::time_point& endTime) { 78 | ConditionTimer timer(endTime); 79 | static_cast(this)->solve(timer.doneFn(doneFn)); 80 | } 81 | }; 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /src/mpt/impl/pprm/component.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_PPRM_COMPONENT_HPP 38 | #define MPT_IMPL_PPRM_COMPONENT_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::mpt::impl::pprm { 43 | struct Component { 44 | enum Flags : unsigned char { 45 | kNone = 0, 46 | kStart = 1, 47 | kGoal = 2, 48 | kSolution = kStart | kGoal 49 | }; 50 | 51 | std::size_t size_; 52 | Flags flags_; 53 | std::atomic next_{nullptr}; 54 | 55 | public: 56 | inline Component(std::size_t size, Flags flags) 57 | : size_(size), flags_(flags) 58 | { 59 | } 60 | 61 | inline Component(Component *a, Component *b) { 62 | update(a, b); 63 | } 64 | 65 | inline std::size_t size() const { 66 | return size_; 67 | } 68 | 69 | inline void update(Component *a, Component *b) { 70 | size_ = a->size_ + b->size_; 71 | flags_ = static_cast(a->flags_ | b->flags_); 72 | } 73 | 74 | inline Component *next() { 75 | Component *next = next_.load(std::memory_order_acquire); 76 | if (next != nullptr) { 77 | if (Component *n = next->next()) { 78 | next_.compare_exchange_strong(next, n, std::memory_order_relaxed); 79 | return n; 80 | } 81 | } 82 | return next; 83 | } 84 | 85 | inline bool casNext(Component*& expect, Component *value, std::memory_order order) { 86 | return next_.compare_exchange_strong(expect, value, order); 87 | } 88 | 89 | inline bool isGoal() { 90 | return (flags_ & kGoal) != 0; 91 | } 92 | 93 | inline bool isStart() { 94 | return (flags_ & kStart) != 0; 95 | } 96 | 97 | inline bool isSolution() { 98 | return flags_ == kSolution; 99 | } 100 | }; 101 | } 102 | 103 | #endif 104 | -------------------------------------------------------------------------------- /src/mpt/impl/pprm/node.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_PPRM_NODE_HPP 38 | #define MPT_IMPL_PPRM_NODE_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::mpt::impl::pprm { 43 | template 44 | class Edge; 45 | 46 | template 47 | class Node { 48 | State state_; 49 | std::atomic component_; 50 | std::atomic*> edges_; 51 | public: 52 | template 53 | Node(Component *component, Args&& ... args) 54 | : state_(std::forward(args)...) 55 | , component_(component) 56 | , edges_(nullptr) 57 | { 58 | } 59 | 60 | const State& state() const { 61 | return state_; 62 | } 63 | 64 | const Edge* edges() const { 65 | return edges_.load(std::memory_order_acquire); 66 | } 67 | 68 | Component* component() { 69 | Component *c = component_.load(std::memory_order_relaxed); 70 | if (Component *n = c->next()) { 71 | component_.compare_exchange_strong(c, n, std::memory_order_relaxed); 72 | return n; 73 | } 74 | return c; 75 | } 76 | 77 | Component* addEdge(Edge *edge) { 78 | Edge *head = edges_.load(std::memory_order_relaxed); 79 | do { 80 | edge->setNext(head, std::memory_order_relaxed); 81 | } while (edges_.compare_exchange_weak( 82 | head, edge, 83 | std::memory_order_release, 84 | std::memory_order_relaxed)); 85 | return component(); 86 | } 87 | }; 88 | 89 | struct NodeKey { 90 | template 91 | const State& operator() (const Node* n) const { 92 | return n->state(); 93 | } 94 | }; 95 | } 96 | 97 | #endif 98 | 99 | -------------------------------------------------------------------------------- /src/mpt/impl/pprm_irs/component.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_PPRM_IRS_COMPONENT_HPP 38 | #define MPT_IMPL_PPRM_IRS_COMPONENT_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::mpt::impl::pprm_irs { 43 | class Component { 44 | public: 45 | enum Flags : unsigned char { 46 | kNone = 0, 47 | kStart = 1, 48 | kGoal = 2, 49 | kSolution = kStart | kGoal 50 | }; 51 | 52 | private: 53 | std::size_t size_; 54 | Flags flags_; 55 | std::atomic next_{nullptr}; 56 | 57 | public: 58 | Component(std::size_t size, Flags flags) 59 | : size_(size), flags_(flags) 60 | { 61 | } 62 | 63 | inline Component(Component *a, Component *b) { 64 | update(a, b); 65 | } 66 | 67 | inline void update(Component *a, Component *b) { 68 | size_ = a->size_ + b->size_; 69 | flags_ = static_cast(a->flags_ | b->flags_); 70 | } 71 | 72 | inline std::size_t size() const { 73 | return size_; 74 | } 75 | 76 | inline Component *next() { 77 | Component *next = next_.load(std::memory_order_acquire); 78 | if (next != nullptr) { 79 | if (Component *n = next->next()) { 80 | next_.compare_exchange_strong(next, n, std::memory_order_relaxed); 81 | return n; 82 | } 83 | } 84 | return next; 85 | } 86 | 87 | inline bool casNext(Component*& expect, Component *value, std::memory_order order) { 88 | return next_.compare_exchange_strong(expect, value, order); 89 | } 90 | 91 | inline bool isGoal() { 92 | return (flags_ & kGoal) != 0; 93 | } 94 | 95 | inline bool isStart() { 96 | return (flags_ & kStart) != 0; 97 | } 98 | 99 | inline bool isSolution() { 100 | return flags_ == kSolution; 101 | } 102 | }; 103 | } 104 | 105 | #endif 106 | -------------------------------------------------------------------------------- /src/mpt/impl/prrt/edge.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_PRRT_EDGE_HPP 38 | #define MPT_IMPL_PRRT_EDGE_HPP 39 | 40 | #include "../link.hpp" 41 | #include 42 | 43 | namespace unc::robotics::mpt::impl::prrt { 44 | template 45 | class Node; 46 | 47 | template 48 | class Edge : public Link { 49 | using Node = prrt::Node; 50 | 51 | Node *to_; 52 | 53 | public: 54 | Edge(Traj&& traj, Node* to) 55 | : Link(std::move(traj)) 56 | , to_(to) 57 | { 58 | } 59 | 60 | operator Node* () { 61 | return to_; 62 | } 63 | 64 | operator const Node* () const { 65 | return to_; 66 | } 67 | }; 68 | } 69 | 70 | #endif 71 | -------------------------------------------------------------------------------- /src/mpt/impl/prrt/node.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_PRRT_NODE_HPP 38 | #define MPT_IMPL_PRRT_NODE_HPP 39 | 40 | #include "edge.hpp" 41 | #include 42 | 43 | namespace unc::robotics::mpt::impl::prrt { 44 | template 45 | class Node { 46 | State state_; 47 | Edge parent_; 48 | 49 | public: 50 | template 51 | Node(Traj&& traj, Node *parent, Args&& ... args) 52 | : state_(std::forward(args)...) 53 | , parent_(std::move(traj), parent) 54 | { 55 | } 56 | 57 | const State& state() const { 58 | return state_; 59 | } 60 | 61 | const Edge& edge() const { 62 | return parent_; 63 | } 64 | 65 | const Node* parent() const { 66 | return parent_; 67 | } 68 | }; 69 | 70 | struct NodeKey { 71 | template 72 | const State& operator() (const Node* node) const { 73 | return node->state(); 74 | } 75 | }; 76 | } 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /src/mpt/impl/scenario_bounds.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_SCENARIO_BOUNDS_HPP 38 | #define MPT_IMPL_SCENARIO_BOUNDS_HPP 39 | 40 | #include "../unbounded.hpp" 41 | #include 42 | 43 | namespace unc::robotics::mpt::impl { 44 | 45 | // Extracts the bounds type of the scenario. The bounds type is 46 | // determined by the return type of the bounds() method on the 47 | // scenario. If no such method exists, then the default is Unbounded. 48 | template 49 | struct scenario_bounds { 50 | using type = Unbounded; 51 | }; 52 | 53 | template 54 | struct scenario_bounds().bounds() )>> { 55 | using type = std::decay_t().bounds() )>; 56 | }; 57 | 58 | template 59 | using scenario_bounds_t = typename scenario_bounds::type; 60 | 61 | template 62 | constexpr bool scenario_has_bounds_v = !std::is_same_v>; 63 | } 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /src/mpt/impl/scenario_goal.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_SCENARIO_GOAL_HPP 38 | #define MPT_IMPL_SCENARIO_GOAL_HPP 39 | 40 | #include 41 | #include "scenario_state.hpp" 42 | #include "always_false.hpp" 43 | 44 | namespace unc::robotics::mpt::impl { 45 | 46 | template 47 | struct scenario_has_goal_class : std::false_type {}; 48 | 49 | template 50 | struct scenario_has_goal_class().goal() )>> : std::true_type {}; 51 | 52 | template 53 | struct scenario_has_goal_method : std::false_type {}; 54 | 55 | template 56 | struct scenario_has_goal_method().isGoal( std::declval>() ))>> 57 | : std::true_type {}; 58 | 59 | template 60 | struct scenario_goal_selector { 61 | static_assert(always_false, "scenario must have a goal() or isGoal(State) method, but not both"); 62 | }; 63 | 64 | // Template specialiation for scenarios that have a no-argument 65 | // goal method that returns a goal object. 66 | template 67 | struct scenario_goal_selector { 68 | // using type = std::decay_t().goal() )>; 69 | 70 | static decltype(auto) check(const T& scenario, const scenario_state_t& q) { 71 | return scenario.goal()(scenario.space(), q); 72 | } 73 | }; 74 | 75 | template 76 | struct scenario_goal_selector { 77 | static decltype(auto) check(const T& scenario, const scenario_state_t& q) { 78 | return scenario.isGoal(q); 79 | } 80 | }; 81 | 82 | template 83 | struct scenario_goal : scenario_goal_selector< 84 | T, 85 | scenario_has_goal_class::value, 86 | scenario_has_goal_method::value> 87 | { 88 | }; 89 | } 90 | 91 | #endif 92 | -------------------------------------------------------------------------------- /src/mpt/impl/scenario_rng.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_SCENARIO_RNG_HPP 38 | #define MPT_IMPL_SCENARIO_RNG_HPP 39 | 40 | #include "../mersenne_twister.hpp" 41 | #include 42 | 43 | namespace unc::robotics::mpt::impl { 44 | // Resolves the random number generator to use. Scenarios may 45 | // override the default by defining a type RNG. 46 | template > 47 | struct scenario_rng { 48 | using type = mersenne_twister_select; 49 | }; 50 | 51 | template 52 | struct scenario_rng> { 53 | using type = typename Scenario::RNG; 54 | }; 55 | 56 | template 57 | using scenario_rng_t = typename scenario_rng::type; 58 | } 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /src/mpt/impl/scenario_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_SCENARIO_SPACE_HPP 38 | #define MPT_IMPL_SCENARIO_SPACE_HPP 39 | 40 | namespace unc::robotics::mpt::impl { 41 | 42 | template 43 | struct scenario_space { 44 | using type = typename Scenario::Space; 45 | }; 46 | 47 | template 48 | using scenario_space_t = typename scenario_space::type; 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /src/mpt/impl/scenario_state.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_SCENARIO_STATE_HPP 38 | #define MPT_IMPL_SCENARIO_STATE_HPP 39 | 40 | namespace unc::robotics::mpt::impl { 41 | 42 | template 43 | struct scenario_state { 44 | using type = typename Scenario::Space::Type; 45 | }; 46 | 47 | template 48 | using scenario_state_t = typename scenario_state::type; 49 | } 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /src/mpt/impl/uniform_sampler_cartesian.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_UNIFORM_SAMPLER_CARTESIAN_HPP 38 | #define MPT_IMPL_UNIFORM_SAMPLER_CARTESIAN_HPP 39 | 40 | #include 41 | #include 42 | #include "constants.hpp" 43 | #include "../cartesian_space.hpp" 44 | 45 | namespace unc::robotics::mpt::impl { 46 | template 47 | using cartesian_uniform_sampler_t = UniformSampler< 48 | Space, 49 | std::tuple_element_t>, 50 | std::tuple_element_t>; 51 | 52 | template 53 | struct CartesianUniformSampler; 54 | 55 | template 56 | struct CartesianUniformSampler> 57 | : std::tuple...> 58 | { 59 | using Space = nigh::metric::Space; 60 | 61 | private: 62 | using Base = std::tuple...>; 63 | 64 | const Base& tuple() const { return *this; } 65 | 66 | public: 67 | CartesianUniformSampler(const Space& space, const Bounds& bounds) 68 | : Base(cartesian_uniform_sampler_t( 69 | std::get(space), 70 | std::get(bounds))...) 71 | { 72 | } 73 | 74 | template 75 | T operator() (RNG& rng) const { 76 | T q; 77 | ((std::get(q) = std::get(tuple())(rng)), ...); 78 | return q; 79 | } 80 | }; 81 | 82 | } 83 | 84 | namespace unc::robotics::mpt { 85 | template 86 | struct UniformSampler>, Bounds> 87 | : impl::CartesianUniformSampler, Bounds, std::index_sequence_for> 88 | { 89 | using impl::CartesianUniformSampler, Bounds, std::index_sequence_for>::CartesianUniformSampler; 90 | }; 91 | 92 | template 93 | auto measure( 94 | const impl::CartesianUniformSampler>& sampler, 95 | const Space& space) 96 | { 97 | return (measure(std::get(sampler), std::get(space)) * ...); 98 | } 99 | } 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /src/mpt/impl/uniform_sampler_lp.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_UNIFORM_SAMPLER_BOX_HPP 38 | #define MPT_IMPL_UNIFORM_SAMPLER_BOX_HPP 39 | 40 | #include 41 | #include 42 | #include "constants.hpp" 43 | #include "../lp_space.hpp" 44 | #include "../uniform_box_sampler.hpp" 45 | 46 | namespace unc::robotics::mpt { 47 | template 48 | struct UniformSampler>, BoxBounds> 49 | : UniformBoxSampler>> 50 | { 51 | using Base = UniformBoxSampler>>; 52 | 53 | UniformSampler(const Space>& space, const BoxBounds& bounds) 54 | : Base(bounds) 55 | { 56 | } 57 | }; 58 | } 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /src/mpt/impl/uniform_sampler_scaled.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_UNIFORM_SAMPLER_SCALED_HPP 38 | #define MPT_IMPL_UNIFORM_SAMPLER_SCALED_HPP 39 | 40 | #include 41 | #include 42 | #include "constants.hpp" 43 | #include "../scaled_space.hpp" 44 | 45 | namespace unc::robotics::mpt { 46 | template 47 | struct UniformSampler>, Bounds> 48 | : UniformSampler, Bounds> 49 | { 50 | using Base = UniformSampler, Bounds>; 51 | 52 | UniformSampler(const Space>& space, const Bounds& bounds) 53 | : Base(space.space(), bounds) 54 | { 55 | } 56 | }; 57 | 58 | template 59 | auto measure( 60 | const UniformSampler>, Bounds>& sampler, 61 | const Space>& space) 62 | { 63 | return measure( 64 | static_cast, Bounds>&>(sampler), 65 | space.space()) * space.weight(); 66 | } 67 | } 68 | 69 | #endif 70 | -------------------------------------------------------------------------------- /src/mpt/impl/uniform_sampler_so3.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_UNIFORM_SAMPLER_SO3_HPP 38 | #define MPT_IMPL_UNIFORM_SAMPLER_SO3_HPP 39 | 40 | #include 41 | #include 42 | #include "constants.hpp" 43 | #include "../unbounded.hpp" 44 | #include "../so3_space.hpp" 45 | 46 | namespace unc::robotics::mpt::impl { 47 | template 48 | struct SO3UniformSampler { 49 | using Space = nigh::metric::Space; 50 | using Distance = typename Space::Distance; 51 | 52 | SO3UniformSampler(const Space&, Unbounded = Unbounded{}) { 53 | } 54 | 55 | template 56 | T operator() (RNG& rng) const { 57 | std::uniform_real_distribution dist01(0, 1); 58 | std::uniform_real_distribution dist2pi(0, 2*PI); 59 | Distance a = dist01(rng); 60 | Distance b = dist2pi(rng); 61 | Distance c = dist2pi(rng); 62 | 63 | return T( 64 | std::sqrt(1-a)*std::sin(b), 65 | std::sqrt(1-a)*std::cos(b), 66 | std::sqrt(a)*std::sin(c), 67 | std::sqrt(a)*std::cos(c)); 68 | } 69 | }; 70 | } 71 | 72 | namespace unc::robotics::mpt { 73 | template 74 | struct UniformSampler, Unbounded> : impl::SO3UniformSampler { 75 | using impl::SO3UniformSampler::SO3UniformSampler; 76 | }; 77 | 78 | template 79 | auto measure(const UniformSampler, Unbounded>&, const Space&) { 80 | using Distance = typename Space::Distance; 81 | // half of the surface area of a unit 3-sphere. 82 | return impl::PI * impl::PI; 83 | } 84 | } 85 | 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /src/mpt/impl/worker_pool.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_IMPL_WORKER_POOL_HPP 38 | #define MPT_IMPL_WORKER_POOL_HPP 39 | 40 | #include "../log.hpp" 41 | #include 42 | 43 | namespace unc::robotics::mpt::impl { 44 | template > 45 | class WorkerPool; 46 | 47 | // specialization for 1 thread 48 | template 49 | class WorkerPool { 50 | T worker_; 51 | public: 52 | using value_type = T; 53 | using iterator = T*; 54 | using const_iterator = const T*; 55 | 56 | WorkerPool(WorkerPool&& other) 57 | : worker_(std::move(other)) 58 | { 59 | } 60 | 61 | template 62 | WorkerPool(const Args& ... args) 63 | : worker_(0, args...) 64 | { 65 | } 66 | 67 | unsigned size() const { 68 | return 1; 69 | } 70 | 71 | T& operator[] (std::size_t i) { 72 | assert(i == 0); 73 | return worker_; 74 | } 75 | 76 | const T& operator[] (std::size_t i) const { 77 | assert(i == 0); 78 | return worker_; 79 | } 80 | 81 | template 82 | void solve(Context& context, const DoneFn& doneFn) { 83 | // TODO exceptions 84 | MPT_LOG(INFO) << "solving with 1 thread"; 85 | worker_.solve(context, doneFn); 86 | } 87 | 88 | auto begin() { 89 | return &worker_; 90 | } 91 | 92 | auto end() { 93 | return &worker_ + 1; 94 | } 95 | 96 | auto begin() const { 97 | return &worker_; 98 | } 99 | 100 | auto end() const { 101 | return &worker_ + 1; 102 | } 103 | }; 104 | } 105 | 106 | #if MPT_USE_OPENMP && MPT_USE_STD_THREAD 107 | # error "MPT_USE_OPENMP and MPT_USE_STD_THREAD are mutually exclusive" 108 | #elif MPT_USE_OPENMP || (defined(_OPENMP) && !MPT_USE_STD_THREAD) 109 | # include "worker_pool_openmp.hpp" 110 | #else 111 | # include "worker_pool_std_thread.hpp" 112 | #endif 113 | 114 | #endif 115 | -------------------------------------------------------------------------------- /src/mpt/lp_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_LP_SPACE_HPP 38 | #define MPT_LP_SPACE_HPP 39 | 40 | #include 41 | #include "impl/metrics.hpp" 42 | 43 | namespace unc::robotics::mpt { 44 | template 45 | T interpolate( 46 | const Space>& space, const T& a, const T& b, 47 | typename Space>::Distance d) 48 | { 49 | using S = Space>; 50 | T q; 51 | for (unsigned i=0 ; i 43 | struct PlannerResolver; 44 | } 45 | 46 | template 47 | using Planner = typename impl::PlannerResolver::type; 48 | } 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /src/mpt/planner_tags.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_PLANNER_TAGS_HPP 38 | #define MPT_PLANNER_TAGS_HPP 39 | 40 | #include 41 | 42 | namespace unc::robotics::mpt { 43 | // option for most planners to optionally enable recording and 44 | // reporting of statistics. 45 | template 46 | struct report_stats : std::bool_constant {}; 47 | 48 | // For RRT*-type planners, this selects the nearest neighbor 49 | // strategy to use: k-nearest or radius-based nearest. 50 | struct rewire_k_nearest {}; 51 | struct rewire_r_nearest {}; 52 | 53 | template 54 | struct max_threads { 55 | // note: we're leaving threadCount as a signed integer since 56 | // we may later give negative numbers special meaning. 57 | // Currently only '0' has special meaning. 58 | static_assert(threadCount >= 0, "thread count must be non-negative"); 59 | }; 60 | using single_threaded = max_threads<1>; 61 | using hardware_concurrency = max_threads<0>; 62 | 63 | template 64 | struct keep_dense_edges : std::bool_constant {}; 65 | } 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /src/mpt/pprm.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_PPRM_HPP 38 | #define MPT_PPRM_HPP 39 | 40 | #include "planner.hpp" 41 | #include "planner_tags.hpp" 42 | #include "impl/packs.hpp" 43 | #include "impl/pack_nearest.hpp" 44 | #include "impl/nearest_strategy.hpp" 45 | #include "impl/pprm/pprm.hpp" 46 | 47 | namespace unc::robotics::mpt { 48 | 49 | namespace impl { 50 | // this is the actual strategy type for a PPRM planner 51 | template 52 | struct PPRMStrategy {}; 53 | 54 | // Option parser to generate a PPRMStrategy from a 55 | // collection of unordered options. 56 | template 57 | struct PPRMOptions { 58 | static constexpr bool reportStats = pack_bool_tag_v; 59 | static constexpr int maxThreads = pack_int_tag_v; 60 | 61 | using NNStrategy = pack_nearest_t; 62 | using type = PPRMStrategy; 63 | }; 64 | 65 | template 66 | struct PlannerResolver> { 67 | using type = impl::pprm::PPRM< 68 | Scenario, maxThreads, reportStats, 69 | nearest_strategy_t>; 70 | }; 71 | } 72 | 73 | // Type alias for a PPRM-based planner. The options supported are: 74 | // - stats reporting 75 | // - tag::report_stats - Reports stats as it plans, where R is false (default) or true. 76 | // - a nearest neighbor strategy 77 | // - nigh::KDTreeBatch<...> - fastest, supports concurrent operation, but does not support arbitrary metrics 78 | // - nigh::Linear - slowest, supports concurrent operations, supports arbitrary metrics 79 | // - nigh::GNAT<...> - fast, does NOT support concurrent operations, supports metrics for which triangle property holds 80 | template 81 | using PPRM = typename impl::PPRMOptions::type; 82 | } 83 | 84 | #endif 85 | -------------------------------------------------------------------------------- /src/mpt/prrt.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_PRRT_HPP 38 | #define MPT_PRRT_HPP 39 | 40 | #include "planner.hpp" 41 | #include "planner_tags.hpp" 42 | #include "impl/packs.hpp" 43 | #include "impl/pack_nearest.hpp" 44 | #include "impl/nearest_strategy.hpp" 45 | #include "impl/prrt/prrt.hpp" 46 | 47 | namespace unc::robotics::mpt { 48 | 49 | namespace impl { 50 | // this is the actual strategy type for a PRRT planner 51 | template 52 | struct PRRTStrategy {}; 53 | 54 | // Option parser to generate a PRRTStrategy from a 55 | // collection of unordered options. 56 | template 57 | struct PRRTOptions { 58 | static constexpr int maxThreads = pack_int_tag_v; 59 | static constexpr bool reportStats = pack_bool_tag_v; 60 | 61 | using NNStrategy = pack_nearest_t; 62 | 63 | using type = PRRTStrategy; 64 | }; 65 | 66 | template 67 | struct PlannerResolver> { 68 | using type = impl::prrt::PRRT< 69 | Scenario, maxThreads, reportStats, 70 | nearest_strategy_t>; 71 | }; 72 | } 73 | 74 | // Type alias for a PRRT*-based planner. The options supported are: 75 | // - stats reporting 76 | // - tag::report_stats - Reports stats as it plans, where R is false (default) or true. 77 | // - a nearest neighbor strategy 78 | // - nigh::KDTreeBatch<...> - fastest, supports concurrent operation, but does not support arbitrary metrics 79 | // - nigh::Linear - slowest, supports concurrent operations, supports arbitrary metrics 80 | // - nigh::GNAT<...> - fast, does NOT support concurrent operations, supports metrics for which triangle property holds 81 | template 82 | using PRRT = typename impl::PRRTOptions::type; 83 | } 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /src/mpt/random_device_seed.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #ifndef MPT_RANDOM_DEVICE_SEED_HPP_ 37 | #define MPT_RANDOM_DEVICE_SEED_HPP_ 38 | 39 | #include 40 | #include 41 | #include 42 | 43 | namespace unc::robotics::mpt { 44 | 45 | /** 46 | * An object that nominally conforms to the SeedSequence concept, 47 | * and can be used to seed pseudo-random number generators from a 48 | * different of randomness. It further uses std::seed_seq to help 49 | * distribute the data in case the underlying source of randomness 50 | * is poorly distributed. 51 | * 52 | * This does not implement the SeedSequence constructors that take 53 | * sequence arguments, nor does it implement the param() method, 54 | * as its result would be useless. 55 | * 56 | * @param _size is the maximum amount of entropy to use from the 57 | * random device. By default the size it reports is 624 which is 58 | * exactly enough for a MersenneTwister19937. 59 | * 60 | * @param _RDev is the underlying source of randomness to use. 61 | */ 62 | template 63 | class RandomDeviceSeed { 64 | mutable _RDev rdev_; 65 | 66 | public: 67 | typedef std::uint32_t result_type; 68 | 69 | template 70 | RandomDeviceSeed(_Args&& ... args) 71 | : rdev_(std::forward<_Args>(args)...) 72 | { 73 | } 74 | 75 | template 76 | void generate(_RandomIt begin, _RandomIt end) const { 77 | std::size_t n = std::min(_size, std::size_t(std::distance(begin, end))); 78 | std::array data; 79 | std::uniform_int_distribution dist32; 80 | std::generate_n(data.begin(), n, [&] { return dist32(rdev_); }); 81 | std::seed_seq seq(data.begin(), data.begin() + n); 82 | seq.generate(begin, end); 83 | } 84 | 85 | std::size_t size() const { 86 | return _size; 87 | } 88 | }; 89 | 90 | // template 91 | // void initRNG() { 92 | // static constexpr std::uint32_t nStates = _RNG::state_size * _RNG::word_size / 32; 93 | // std::random_device rdev; 94 | // std::array seedData; 95 | // std::uniform_int_distribution dist32; 96 | // std::generate_n(seedData.begin(), nStates, [&] { return dist32(rdev); }); 97 | // std::seed_seq seedSeq(seedData.begin(), seedData.end()); 98 | // _RNG rng(seedSeq); 99 | // } 100 | 101 | } 102 | 103 | #endif // MPT_RANDOM_DEVICE_SEED_HPP_ 104 | -------------------------------------------------------------------------------- /src/mpt/scaled_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_SCALED_SPACE_HPP 38 | #define MPT_SCALED_SPACE_HPP 39 | 40 | #include 41 | #include "impl/metrics.hpp" 42 | 43 | namespace unc::robotics::mpt { 44 | template 45 | T interpolate( 46 | const Space>& space, 47 | const T& a, const T& b, 48 | typename Space>::Distance t) 49 | { 50 | return interpolate(space.space(), a, b, t); 51 | } 52 | } 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /src/mpt/se2_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_SE2_SPACE_HPP 38 | #define MPT_SE2_SPACE_HPP 39 | 40 | #include "lp_space.hpp" 41 | #include "so2_space.hpp" 42 | #include "scaled_space.hpp" 43 | #include "cartesian_space.hpp" 44 | 45 | namespace unc::robotics::mpt { 46 | template 47 | class SE2State 48 | : public std::tuple, Scalar> 49 | { 50 | using Base = std::tuple, Scalar>; 51 | 52 | public: 53 | using Base::Base; 54 | 55 | Scalar& rotation() { return std::get<1>(*this); } 56 | const Scalar& rotation() const { return std::get<1>(*this); } 57 | 58 | Eigen::Matrix& translation() { return std::get<0>(*this); } 59 | const Eigen::Matrix& translation() const { return std::get<0>(*this); } 60 | }; 61 | 62 | namespace impl { 63 | template 64 | struct se2_space_selector { 65 | using type = Space, Cartesian>, Scaled, std::ratio>>>; 66 | }; 67 | 68 | template 69 | struct se2_space_selector { 70 | using type = Space, Cartesian, std::ratio>>>; 71 | }; 72 | 73 | template 74 | struct se2_space_selector { 75 | using type = Space, Cartesian>, SO2<>>>; 76 | }; 77 | 78 | template 79 | struct se2_space_selector { 80 | using type = Space, Cartesian>>; 81 | }; 82 | } 83 | 84 | template 85 | using SE2Space = typename impl::se2_space_selector::type; 86 | } 87 | 88 | namespace std { 89 | template 90 | struct tuple_element<0, unc::robotics::mpt::SE2State> { 91 | using type = Eigen::Matrix; 92 | }; 93 | 94 | template 95 | struct tuple_element<1, unc::robotics::mpt::SE2State> { 96 | using type = Scalar; 97 | }; 98 | } 99 | 100 | #endif 101 | -------------------------------------------------------------------------------- /src/mpt/so2_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_SO2_SPACE_HPP 38 | #define MPT_SO2_SPACE_HPP 39 | 40 | #include 41 | #include "impl/metrics.hpp" 42 | #include 43 | namespace unc::robotics::mpt { 44 | template 45 | T interpolate( 46 | const Space>& space, const T& a, const T& b, 47 | typename Space>::Distance d) 48 | { 49 | using namespace unc::robotics::nigh::impl; 50 | using S = Space>; 51 | using Scalar = typename S::Distance; 52 | T q; 53 | for (unsigned i=0; i < space.dimensions(); ++i) { 54 | Scalar ccwDist = so2::ccwDist(S::coeff(a, i), S::coeff(b, i)); 55 | if (ccwDist < PI) { 56 | // angular distance is in ccw direction from a to b 57 | S::coeff(q, i) = so2::bound(S::coeff(a, i) + ccwDist * d); 58 | } else { 59 | // angular distance is in cw direction from a to b 60 | Scalar cwDist = 2*PI - ccwDist; 61 | S::coeff(q, i) = so2::bound(S::coeff(a, i) - cwDist * d); 62 | } 63 | } 64 | return q; 65 | } 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /src/mpt/so3_space.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_SO3_SPACE_HPP 38 | #define MPT_SO3_SPACE_HPP 39 | 40 | #include 41 | #include "impl/metrics.hpp" 42 | 43 | namespace unc::robotics::mpt { 44 | template 45 | T interpolate( 46 | const Space& space, const T& a, const T& b, 47 | typename Space::Distance t) 48 | { 49 | using S = Space; 50 | using Scalar = typename S::Distance; 51 | // TODO: for Eigen types, we can do this: 52 | // return a.slerp(t, b); 53 | 54 | Scalar d = S::coeff(a, 0) * S::coeff(b, 0) 55 | + S::coeff(a, 1) * S::coeff(b, 1) 56 | + S::coeff(a, 2) * S::coeff(b, 2) 57 | + S::coeff(a, 3) * S::coeff(b, 3); 58 | Scalar ad = std::abs(d); 59 | Scalar s0, s1; 60 | 61 | if (d >= Scalar(1) - std::numeric_limits::epsilon()) { 62 | s0 = Scalar(1) - t; 63 | s1 = t; 64 | } else { 65 | Scalar theta = std::acos(ad); 66 | Scalar sinTheta = std::sin(theta); 67 | 68 | s0 = std::sin( (Scalar(1) - t) * theta) / sinTheta; 69 | s1 = std::sin( (t * theta) ) / sinTheta; 70 | } 71 | 72 | if (d < 0) 73 | s1 = -s1; 74 | 75 | T q; 76 | 77 | S::coeff(q, 0) = s0 * S::coeff(a, 0) + s1 * S::coeff(b, 0); 78 | S::coeff(q, 1) = s0 * S::coeff(a, 1) + s1 * S::coeff(b, 1); 79 | S::coeff(q, 2) = s0 * S::coeff(a, 2) + s1 * S::coeff(b, 2); 80 | S::coeff(q, 3) = s0 * S::coeff(a, 3) + s1 * S::coeff(b, 3); 81 | 82 | return q; 83 | } 84 | } 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /src/mpt/unbounded.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_UNBOUNDED_HPP 38 | #define MPT_UNBOUNDED_HPP 39 | 40 | namespace unc::robotics::mpt { 41 | struct Unbounded { 42 | }; 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /src/mpt/uniform_box_sampler.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_UNIFORM_BOX_SAMPLER_HPP 38 | #define MPT_UNIFORM_BOX_SAMPLER_HPP 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace unc::robotics::mpt { 45 | template 46 | class UniformBoxSampler { 47 | using Scalar = typename Space::Distance; 48 | using Type = typename Space::Type; 49 | static constexpr int dimensions = Space::kDimensions; 50 | 51 | BoxBounds bounds_; 52 | 53 | public: 54 | template 55 | UniformBoxSampler(Args&& ... args) 56 | : bounds_(std::forward(args)...) 57 | { 58 | } 59 | 60 | template 61 | Type operator() (RNG& rng) const { 62 | Type q; 63 | for (unsigned i=0 ; i < bounds_.size() ; ++i) { 64 | std::uniform_real_distribution dist(bounds_.min()[i], bounds_.max()[i]); 65 | Space::coeff(q, i) = dist(rng); 66 | } 67 | return q; 68 | } 69 | 70 | const BoxBounds& bounds() const { 71 | return bounds_; 72 | } 73 | }; 74 | 75 | template 76 | auto measure(const UniformBoxSampler& sampler, const Space&) { 77 | return sampler.bounds().measure(); 78 | } 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /src/mpt/uniform_sampler.hpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #pragma once 37 | #ifndef MPT_UNIFORM_SAMPLER_HPP 38 | #define MPT_UNIFORM_SAMPLER_HPP 39 | 40 | namespace unc::robotics::mpt { 41 | template 42 | struct UniformSampler; 43 | } 44 | 45 | #include "impl/uniform_sampler_so3.hpp" 46 | #include "impl/uniform_sampler_so2.hpp" 47 | #include "impl/uniform_sampler_scaled.hpp" 48 | #include "impl/uniform_sampler_cartesian.hpp" 49 | #include "impl/uniform_sampler_lp.hpp" 50 | 51 | // template 52 | // struct UniformSampler> 53 | // : impl::SO3UniformSampler 54 | // { 55 | // using impl::SO3UniformSampler::SO3UniformSampler; 56 | // }; 57 | 58 | // template 59 | // struct UniformSampler>> 60 | // : impl::SO2UniformSampler>::kDimensions> 61 | // { 62 | // using SO2UniformSampler>::kDimensions>::SO2UniformSampler; 63 | // }; 64 | 65 | // template 66 | // struct UniformSampler>> 67 | // : UniformSampler 68 | // { 69 | // using UniformSampler::UniformSampler; 70 | // }; 71 | 72 | // template 73 | // struct UniformSampler, BoxBounds>> 74 | // : BoxUniformSampler 75 | // { 76 | // using BoxUniformSampler::BoxUniformSampler; 77 | // }; 78 | 79 | // template 80 | // struct UniformSampler>> 81 | // : impl::CartesianUniformSampler, std::index_sequence_for> 82 | // { 83 | // using impl::CartesianUniformSampler, std::index_sequence_for>::CartesianUniformSampler; 84 | // }; 85 | // } 86 | 87 | #endif 88 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 3.10 introduces C++ 17 support 2 | cmake_minimum_required (VERSION 3.10) 3 | project (mpt_test) 4 | 5 | # Enable C++17 (required for MPT) 6 | set(CMAKE_CXX_STANDARD 17) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | 9 | # Disable extensions to keep compatible with standards 10 | set(CMAKE_CXX_EXTENSIONS OFF) 11 | 12 | # See https://cmake.org/cmake/help/v3.2/prop_gbl/JOB_POOLS.html#prop_gbl:JOB_POOLS 13 | # (TODO: it would be nice to be able to run concurrent tests on a pool 14 | # that only allows 1 job. It does not appear that CMake supports pool 15 | # selection yet.) 16 | # set_property(GLOBAL PROPERTY JOB_POOLS concurrent_pool=1) 17 | 18 | find_package(Eigen3 3.3 REQUIRED NO_MODULE) 19 | find_package(OPENMP REQUIRED) 20 | 21 | if (OPENMP_FOUND) 22 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}") 23 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") 24 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}") 25 | endif() 26 | 27 | # Currently we assume that Nigh is checked out at the same level as 28 | # MPT. 29 | include_directories(../src ../../nigh/src) 30 | 31 | # Create a test for each "*_test.cpp" file in this directory. 32 | file(GLOB files "*_test.cpp") 33 | foreach(file ${files}) 34 | get_filename_component(test_name ${file} NAME_WE) 35 | # Add the executable based upon the test source 36 | add_executable(${test_name} ${file}) 37 | target_link_libraries(${test_name} Eigen3::Eigen) 38 | 39 | # Add a custom command for the test's output (.log), making sure 40 | # that it depends on the test executable. 41 | add_custom_command( 42 | OUTPUT ${test_name}.log 43 | COMMAND ${test_name} | tee ${test_name}.tmp && mv ${test_name}.tmp ${test_name}.log 44 | DEPENDS ${test_name} 45 | COMMENT "TESTING ${test_name}") 46 | 47 | # Add the test to the top-level 'all' target 48 | add_custom_target(run_${test_name} ALL DEPENDS ${test_name}.log) 49 | endforeach() 50 | -------------------------------------------------------------------------------- /test/README.md: -------------------------------------------------------------------------------- 1 | # Unit Tests for MPT 2 | 3 | Running the unit tests requires [CMake](https://cmake.org/). The build assumes that [Nigh](https://github.com/UNC-Robotics/nigh) is checked out at the same directory level as `mpt`. Use the following commands to build (we assume the ninja build system is available): 4 | 5 | % mkdir build 6 | % cmake -G Ninja .. 7 | % ninja 8 | 9 | ## Troubleshooting 10 | 11 | ### Xcode + macOS + C++17 12 | 13 | The compiler included with macOS's Xcode does not appear to support all of C++17 without first upgrading to Mohave (macOS 10.14). Because there are also issues with getting OpenMP to work with this compiler, we recommend using a different compiler. See next tip. 14 | 15 | ### Xcode + macOS + OpenMP 16 | 17 | The compiler included with macOS's Xcode supports OpenMP, but requires extra flags that CMake doesn't appear to be able to add. We recommend using a custom install of `clang` (version 5+) or `gcc` (version 7+) and `libomp` using either MacPorts or Homebrew. Then make sure that cmake uses the right compiler: 18 | 19 | % mkdir build 20 | % cd build 21 | % CXX=/path/to/clang++ CC=/path/to/clang cmake -G Ninja .. 22 | % ninja 23 | 24 | -------------------------------------------------------------------------------- /test/bounds_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | #include 39 | #include "test.hpp" 40 | 41 | TEST(box_bounds) { 42 | using namespace unc::robotics::mpt; 43 | BoxBounds bounds(Eigen::Vector3d(0,1,2), Eigen::Vector3d(10, 12, 14)); 44 | 45 | EXPECT(bounds.min()[1]) == 1.0; 46 | EXPECT(bounds.max()[2]) == 14.0; 47 | } 48 | 49 | TEST(unbounded) { 50 | using namespace unc::robotics::mpt; 51 | Unbounded unbounded; 52 | 53 | EXPECT(sizeof(unbounded)) <= 1; 54 | } 55 | 56 | TEST(cartesian_bounds_2_boxes) { 57 | using namespace unc::robotics::mpt; 58 | using Vec2 = Eigen::Vector2d; 59 | using Vec3 = Eigen::Vector3d; 60 | CartesianBounds, BoxBounds> bounds( 61 | BoxBounds(Vec3(1,2,3), Vec3(2,4,6)), 62 | BoxBounds(Vec2(4,5), Vec2(8,7))); 63 | 64 | EXPECT(std::get<0>(bounds).min()[2]) == 3.0; 65 | EXPECT(std::get<1>(bounds).max()[0]) == 8.0; 66 | } 67 | 68 | TEST(cartesian_bounds_se3) { 69 | using namespace unc::robotics::mpt; 70 | using Vec3 = Eigen::Vector3d; 71 | 72 | // CartesianBound's constructor makes Unbounded arguments 73 | // optional. 74 | CartesianBounds> bounds( 75 | BoxBounds(Vec3(1,2,3), Vec3(2,4,6))); 76 | 77 | CartesianBounds> bounds2( 78 | Unbounded{}, 79 | BoxBounds(Vec3(1,2,3), Vec3(2,4,6))); 80 | 81 | CartesianBounds> bounds3( 82 | Vec3(1,2,3), Vec3(2,4,6)); 83 | 84 | EXPECT(sizeof(bounds)) == sizeof(BoxBounds); 85 | EXPECT(std::get<1>(bounds).max()[2]) == 6.0; 86 | EXPECT(std::get<1>(bounds2).max()[2]) == 6.0; 87 | EXPECT(std::get<1>(bounds3).max()[2]) == 6.0; 88 | } 89 | 90 | TEST(cartesian_bounds_se3r) { 91 | using namespace unc::robotics::mpt; 92 | using Vec3 = Eigen::Vector3d; 93 | 94 | // CartesianBound's constructor makes Unbounded arguments 95 | // optional. 96 | CartesianBounds, Unbounded> bounds( 97 | BoxBounds(Vec3(1,2,3), Vec3(2,4,6))); 98 | 99 | CartesianBounds, Unbounded> bounds2( 100 | BoxBounds(Vec3(1,2,3), Vec3(2,4,6)), 101 | Unbounded{}); 102 | 103 | // CartesianBounds, Unbounded> bounds3( 104 | // Vec3(1,2,3), Vec3(2,4,6)); 105 | 106 | EXPECT(sizeof(bounds)) == sizeof(BoxBounds); 107 | EXPECT(std::get<0>(bounds).max()[2]) == 6.0; 108 | EXPECT(std::get<0>(bounds2).max()[2]) == 6.0; 109 | } 110 | 111 | -------------------------------------------------------------------------------- /test/count_down_latch_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "test.hpp" 3 | #include 4 | #include 5 | 6 | TEST(count_down) { 7 | using namespace unc::robotics::mpt::impl; 8 | int n = 111; 9 | CountDownLatch latch(n); 10 | std::atomic_bool started{false}; 11 | std::atomic_bool finished{false}; 12 | 13 | std::thread waiter([&] { started = true; latch.wait(); finished = true; }); 14 | 15 | while (!started.load()) 16 | ; 17 | 18 | std::list threads; 19 | for (int i=0 ; i 37 | #include 38 | #include "test.hpp" 39 | 40 | namespace mpt_test { 41 | struct Vertex { 42 | int id_; 43 | std::forward_list> edges_; 44 | 45 | explicit Vertex(int id) : id_(id) {} 46 | 47 | void addEdge(double w, Vertex& v) { 48 | edges_.emplace_front(w, &v); 49 | v.edges_.emplace_front(w, this); 50 | } 51 | }; 52 | } 53 | 54 | TEST(find_shortest_path) { 55 | std::size_t N = 7; 56 | std::vector V; 57 | V.reserve(N); 58 | for (std::size_t i=0 ; i result = unc::robotics::mpt::impl::djikstras( 73 | &V[0], 74 | // Goal check, V[4] is our goal 75 | [&] (const auto& v) { return v == &V[4]; }, 76 | // Edge callback 77 | [&] (const auto& v, auto callback) { 78 | for (auto [w, u] : v->edges_) 79 | callback(w, u); 80 | }, 81 | // results callback 82 | [&] (std::size_t n, auto first, auto last) { 83 | EXPECT(n) == 5; 84 | auto it = first; 85 | EXPECT((*it++)->id_) == 0; 86 | EXPECT((*it++)->id_) == 1; 87 | EXPECT((*it++)->id_) == 3; 88 | EXPECT((*it++)->id_) == 2; 89 | EXPECT((*it++)->id_) == 4; 90 | std::vector result; 91 | result.reserve(n); 92 | for (auto it = first ; it != last ; ++it) 93 | result.push_back((*it)->id_); 94 | return result; 95 | }); 96 | 97 | EXPECT(result.size()) == 5; 98 | EXPECT(result[0]) == 0; 99 | EXPECT(result[1]) == 1; 100 | EXPECT(result[2]) == 3; 101 | EXPECT(result[3]) == 2; 102 | EXPECT(result[4]) == 4; 103 | } 104 | 105 | -------------------------------------------------------------------------------- /test/goal_has_sampler_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | #include 39 | #include "test.hpp" 40 | 41 | using namespace unc::robotics::mpt; 42 | 43 | TEST(positive) { 44 | using Space = L2Space; 45 | EXPECT(impl::goal_has_sampler_v>) == true; 46 | } 47 | 48 | struct MyGoal { 49 | bool operator() (double) const { 50 | return false; 51 | } 52 | }; 53 | 54 | TEST(negative) { 55 | EXPECT(impl::goal_has_sampler_v) == false; 56 | } 57 | -------------------------------------------------------------------------------- /test/log_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | 39 | // #include 40 | 41 | // #define _SELECT(PREFIX,_5,_4,_3,_2,_1,SUFFIX, ...) PREFIX ## _ ## SUFFIX 42 | 43 | // // #define _LOG_1(L) printf("%s:%d: [" #L "] (no message)\n", __FILE__, __LINE__) 44 | // #define _LOG_1(L) printf("[%s] (no message)\n", #L) 45 | // #define _LOG_2(L,fmt) printf("[%s] %s\n", #L, fmt) 46 | // #define _LOG_N(L,fmt, ...) printf("[%s] " fmt "\n", #L, __VA_ARGS__) 47 | // #define LOG(...) _SELECT(_LOG,__VA_ARGS__,N,N,N,2,1)(__VA_ARGS__) 48 | 49 | // #define _BAR_1(fmt) printf(fmt "\n") 50 | // #define _BAR_N(fmt, ...) printf(fmt "\n", __VA_ARGS__); 51 | // #define BAR(...) _SELECT(_BAR,__VA_ARGS__,N,N,N,N,1)(__VA_ARGS__) 52 | 53 | int main(int argc, char *argv[]) { 54 | std::thread thread([&] { 55 | MPT_LOG(INFO) << "this is not the main thread"; }); 56 | 57 | thread.join(); 58 | MPT_LOG(TRACE) << "a trace message with number " << 42.0; 59 | MPT_LOG(DEBUG) << "hello"; 60 | MPT_LOG(INFO) << "this is info"; 61 | MPT_LOG(WARN) << "achtung"; 62 | MPT_LOG(ERROR) << "an error"; 63 | MPT_LOG(FATAL) << "fatal error"; 64 | MPT_LOG(INFO, "%s world %f %d %d", "three", 4.0, 5, 6); 65 | MPT_LOG(WARN, "message without arguments (%s and %d, etc... are ignored)"); 66 | 67 | if (true) 68 | MPT_LOG(TRACE) << "dangling else warning?"; 69 | 70 | // //LOG(); 71 | // // PREFIX,_5,_4,_3,_2,_1,SUFFIX 72 | // // expands to _SELECT(_LOG , , N, N, N, N, 1)() 73 | // // (note, empty parameter _5 is OK in macros) 74 | // // expands to LOG ## _ ## 1 () 75 | // // _LOG_1 () 76 | 77 | // // LOG(); // this does NOT work, because it expands to _LOG_1() => printf("%s:%d: %s\n, __FILE__, __LINE__, ); 78 | // LOG(INFO); 79 | // LOG(DEBUG, "here is a log message"); 80 | // LOG(WARN, "here is a log message with param: %d", 42); 81 | 82 | // BAR(); // this works, because it expands to _SELECT(_BAR,,N,N,N,N,1)() => _BAR_1() => printf("\n") 83 | // BAR("here is a log message"); 84 | // BAR("here is one with a param: %d", 42); 85 | 86 | // BAR("here is a log message"); 87 | // BAR("here is a log message with a param: %d", 42); 88 | 89 | return 0; 90 | } 91 | -------------------------------------------------------------------------------- /test/lp_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(distance) { 40 | using Space = unc::robotics::mpt::LPSpace; 41 | 42 | static_assert(std::is_same_v); 43 | 44 | typename Space::Type a(1,2,3); 45 | typename Space::Type b(1,0,-1); 46 | 47 | Space space; 48 | 49 | EXPECT(space.distance(a, b)) == std::sqrt(0.0 + 2.0*2.0 + 4.0*4.0); 50 | } 51 | 52 | 53 | TEST(interpolate) { 54 | using namespace unc::robotics::mpt; 55 | using Space = LPSpace; 56 | 57 | Space space; 58 | 59 | typename Space::Type a(1,2,3); 60 | typename Space::Type b(1,0,-1); 61 | 62 | typename Space::Type c = interpolate(space, a, b, 0.1); 63 | 64 | EXPECT(c[0]) == 1.0; 65 | EXPECT(c[1]) == 1.8; 66 | EXPECT(c[2]) == 2.6; 67 | } 68 | -------------------------------------------------------------------------------- /test/pack_nearest_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(nearest_linear_match) { 40 | using namespace unc::robotics::nigh; 41 | using namespace unc::robotics::mpt::impl; 42 | 43 | EXPECT((std::is_same_v>)) == true; 44 | EXPECT((std::is_same_v>)) == true; 45 | } 46 | 47 | TEST(nearest_kdtree_batch_match) { 48 | using namespace unc::robotics::nigh; 49 | using namespace unc::robotics::mpt::impl; 50 | 51 | EXPECT((std::is_same_v, pack_nearest_t>>)) == true; 52 | EXPECT((std::is_same_v, pack_nearest_t, int>>)) == true; 53 | } 54 | 55 | TEST(nearest_gnat_match) { 56 | using namespace unc::robotics::nigh; 57 | using namespace unc::robotics::mpt::impl; 58 | 59 | EXPECT((std::is_same_v, pack_nearest_t>>)) == true; 60 | EXPECT((std::is_same_v, pack_nearest_t, int>>)) == true; 61 | } 62 | 63 | TEST(nearest_default) { 64 | using namespace unc::robotics::nigh; 65 | using namespace unc::robotics::mpt::impl; 66 | 67 | EXPECT((std::is_void_v>)) == true; 68 | EXPECT((std::is_void_v>)) == true; 69 | } 70 | 71 | 72 | -------------------------------------------------------------------------------- /test/packs_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(pack_find_v) { 40 | using namespace unc::robotics::mpt::impl; 41 | 42 | EXPECT((pack_find_v)) == true; 43 | } 44 | 45 | TEST(pack_find_t) { 46 | using namespace unc::robotics::mpt::impl; 47 | 48 | EXPECT((std::is_same_v>)) == true; 49 | } 50 | 51 | TEST(pack_default) { 52 | using namespace unc::robotics::mpt::impl; 53 | 54 | EXPECT((std::is_same_v>)) == true; 55 | } 56 | 57 | TEST(pack_contains_v) { 58 | using namespace unc::robotics::mpt::impl; 59 | EXPECT((pack_contains_v)) == false; 60 | EXPECT((pack_contains_v)) == true; 61 | EXPECT((pack_contains_v)) == true; 62 | } 63 | 64 | template 65 | struct test_bool {}; 66 | 67 | TEST(pack_bool_tag_v) { 68 | using namespace unc::robotics::mpt::impl; 69 | EXPECT((pack_bool_tag_v)) == false; 70 | EXPECT((pack_bool_tag_v, bool>)) == true; 71 | EXPECT((pack_bool_tag_v, bool>)) == false; 72 | } 73 | -------------------------------------------------------------------------------- /test/pprm_integration_test.cpp: -------------------------------------------------------------------------------- 1 | #define MPT_LOG_LEVEL WARN 2 | #include "planner_integration_test.hpp" 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace unc::robotics; 8 | using namespace mpt; 9 | using namespace mpt_test; 10 | 11 | TEST(pprm_until_solved_with_goal_class) { 12 | testSolvingBasicScenario, TEST_GOAL_KIND_CLASS>(); 13 | } 14 | 15 | TEST(pprm_until_solved_with_stats) { 16 | testSolvingBasicScenario>>(); 17 | } 18 | 19 | TEST(pprm_until_solved_single_threaded) { 20 | testSolvingBasicScenario>(); 21 | } 22 | 23 | TEST(pprm_until_solved_with_gnat) { 24 | testSolvingBasicScenario>>(); 25 | } 26 | 27 | TEST(pprm_until_solved_with_linear) { 28 | testSolvingBasicScenario>(); 29 | } 30 | 31 | TEST(pprm_options_parser) { 32 | // The options parser should resolve the following two planners to 33 | // the same concrete type. 34 | using Scenario = BasicScenario<>; 35 | using A = mpt::Planner>>; 36 | using B = mpt::Planner, nigh::Linear>>; 37 | EXPECT((std::is_same_v)) == true; 38 | } 39 | 40 | TEST(pprm_with_trajectory) { 41 | testSolvingTrajectoryScenario>(); 42 | } 43 | 44 | TEST(pprm_with_shared_trajectory) { 45 | testSolvingSharedTrajectoryScenario>(); 46 | } 47 | 48 | -------------------------------------------------------------------------------- /test/pprm_irs_integration_test.cpp: -------------------------------------------------------------------------------- 1 | #define MPT_LOG_LEVEL WARN 2 | #include "planner_integration_test.hpp" 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace unc::robotics; 8 | using namespace mpt; 9 | using namespace mpt_test; 10 | 11 | TEST(pprm_irs_until_solved) { 12 | testSolvingBasicScenario>(); 13 | } 14 | 15 | TEST(pprm_irs_until_solved_with_stats) { 16 | testSolvingBasicScenario>>(); 17 | } 18 | 19 | TEST(pprm_irs_until_solved_single_threaded) { 20 | testSolvingBasicScenario>(); 21 | } 22 | 23 | TEST(pprm_irs_until_solved_with_gnat) { 24 | testSolvingBasicScenario>>(); 25 | } 26 | 27 | TEST(pprm_irs_until_solved_with_linear) { 28 | testSolvingBasicScenario>(); 29 | } 30 | 31 | TEST(pprm_irs_options_parser) { 32 | // The options parser should resolve the following two planners to 33 | // the same concrete type. 34 | using Scenario = BasicScenario<>; 35 | using A = mpt::Planner>>; 36 | using B = mpt::Planner, nigh::Linear>>; 37 | EXPECT((std::is_same_v)) == true; 38 | } 39 | 40 | TEST(pprm_irs_with_trajectory) { 41 | testSolvingTrajectoryScenario>(); 42 | } 43 | #if 0 44 | TEST(pprm_irs_with_shared_trajectory) { 45 | testSolvingSharedTrajectoryScenario>(); 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /test/prrt_integration_test.cpp: -------------------------------------------------------------------------------- 1 | #define MPT_LOG_LEVEL WARN 2 | #include "planner_integration_test.hpp" 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace unc::robotics::mpt; 8 | using namespace mpt_test; 9 | namespace nigh = unc::robotics::nigh; 10 | 11 | TEST(prrt_until_solved_with_goal_class) { 12 | testSolvingBasicScenario, TEST_GOAL_KIND_CLASS>(); 13 | } 14 | 15 | TEST(prrt_until_solved_with_goal_method) { 16 | testSolvingBasicScenario, TEST_GOAL_KIND_METHOD>(); 17 | } 18 | 19 | TEST(prrt_until_solved_with_stats) { 20 | testSolvingBasicScenario>>(); 21 | } 22 | 23 | TEST(prrt_until_solved_single_threaded) { 24 | testSolvingBasicScenario>(); 25 | } 26 | 27 | TEST(prrt_until_solved_with_gnat) { 28 | testSolvingBasicScenario>>(); 29 | } 30 | 31 | TEST(prrt_until_solved_with_linear) { 32 | testSolvingBasicScenario>(); 33 | } 34 | 35 | TEST(prrt_options_parser) { 36 | // The options parser should resolve the following two planners to 37 | // the same concrete type. 38 | using Scenario = BasicScenario<>; 39 | using A = Planner>>; 40 | using B = Planner, nigh::Linear>>; 41 | EXPECT((std::is_same_v)) == true; 42 | } 43 | 44 | TEST(prrt_with_trajectory) { 45 | testSolvingTrajectoryScenario>(); 46 | } 47 | 48 | TEST(prrt_with_shared_trajectory) { 49 | testSolvingSharedTrajectoryScenario>(); 50 | } 51 | -------------------------------------------------------------------------------- /test/prrt_star_integration_test.cpp: -------------------------------------------------------------------------------- 1 | #define MPT_LOG_LEVEL WARN 2 | #include "planner_integration_test.hpp" 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace unc::robotics; 8 | using namespace mpt; 9 | using namespace mpt_test; 10 | 11 | TEST(prrt_star_until_solved_with_goal_class) { 12 | testSolvingBasicScenario, TEST_GOAL_KIND_CLASS>(); 13 | } 14 | 15 | TEST(prrt_star_until_solved_with_goal_method) { 16 | testSolvingBasicScenario, TEST_GOAL_KIND_METHOD>(); 17 | } 18 | 19 | TEST(prrt_star_until_solved_with_stats) { 20 | testSolvingBasicScenario>>(); 21 | } 22 | 23 | TEST(prrt_star_until_solved_single_threaded) { 24 | testSolvingBasicScenario>(); 25 | } 26 | 27 | TEST(prrt_star_until_solved_with_gnat) { 28 | testSolvingBasicScenario>>(); 29 | } 30 | 31 | TEST(prrt_star_until_solved_with_linear) { 32 | testSolvingBasicScenario>(); 33 | } 34 | 35 | TEST(prrt_star_until_solved_r_nearest) { 36 | testSolvingBasicScenario>(); 37 | } 38 | 39 | TEST(prrt_star_until_solved_k_nearest) { 40 | testSolvingBasicScenario>(); 41 | } 42 | 43 | TEST(prrt_star_options_parser) { 44 | // The options parser should resolve the following two planners to 45 | // the same concrete type. 46 | using Scenario = BasicScenario<>; 47 | using A = mpt::Planner>>; 48 | using B = mpt::Planner, nigh::Linear, rewire_r_nearest>>; 49 | EXPECT((std::is_same_v)) == true; 50 | } 51 | 52 | TEST(prrt_star_options_default) { 53 | using Scenario = BasicScenario<>; 54 | using A = mpt::Planner, max_threads<0>>>; 55 | using B = mpt::Planner>; 56 | EXPECT((std::is_same_v)) == true; 57 | } 58 | 59 | TEST(prrt_star_with_trajectory) { 60 | testSolvingTrajectoryScenario>(); 61 | } 62 | 63 | TEST(prrt_star_with_shared_trajectory) { 64 | testSolvingSharedTrajectoryScenario>(); 65 | } 66 | -------------------------------------------------------------------------------- /test/random_device_seed_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | #include "test.hpp" 39 | 40 | TEST(different) { 41 | using namespace unc::robotics::mpt; 42 | 43 | RandomDeviceSeed<> seed; 44 | 45 | std::array a, b; 46 | seed.generate(a.begin(), a.end()); 47 | seed.generate(b.begin(), b.end()); 48 | 49 | EXPECT(std::equal(a.begin(), a.end(), b.begin(), b.end())) == false; 50 | } 51 | 52 | TEST(seed_random) { 53 | using namespace unc::robotics::mpt; 54 | 55 | RandomDeviceSeed<> seed; 56 | std::mt19937_64 rngSeeded(seed); 57 | std::mt19937_64 rngDefault; 58 | 59 | bool allMatch = true; 60 | for (int i=0 ; i < 128 ; ++i) 61 | allMatch &= (rngSeeded() == rngDefault()); 62 | 63 | EXPECT(allMatch) == false; 64 | } 65 | -------------------------------------------------------------------------------- /test/scaled_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | #include "test.hpp" 39 | 40 | TEST(distance) { 41 | using namespace unc::robotics::mpt; 42 | using Space = ScaledSpace, std::ratio<5, 2>>; 43 | 44 | static_assert(std::is_same_v); 45 | 46 | typename Space::Type a(1,2,3); 47 | typename Space::Type b(1,0,-1); 48 | 49 | Space space; 50 | 51 | EXPECT(space.distance(a, b)) == std::sqrt(0.0 + 2.0*2.0 + 4.0*4.0) * 5 / 2; 52 | } 53 | -------------------------------------------------------------------------------- /test/se2_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | // template 40 | // constexpr std::string_view type_name() { 41 | // std::string_view p = __PRETTY_FUNCTION__; 42 | // return std::string_view(p.data() + 34, p.size() - 34 - 1); 43 | // } 44 | 45 | template 46 | void testSE2dist() { 47 | using namespace unc::robotics::mpt; 48 | using Space = SE2Space; 49 | using Rot2 = Scalar; 50 | using Vec2 = Eigen::Matrix; 51 | 52 | // std::cout << type_name() << std::endl; 53 | 54 | static_assert(std::is_same_v, typename Space::Type>); 55 | 56 | Space space; 57 | 58 | typename Space::Type a, b; 59 | 60 | std::get(a) = -1.0; 61 | std::get<1>(b) = 2.0; 62 | 63 | std::get<0>(a) << 2, 3; 64 | std::get(b) << 0, -1; 65 | 66 | EXPECT(space.distance(a, b)) == 3.0 * so2w + std::sqrt(0.0 + 2.0*2.0 + 4.0*4.0) * l2w; 67 | } 68 | 69 | 70 | TEST(distance_1_1) { 71 | testSE2dist(); 72 | } 73 | 74 | TEST(distance_5_2) { 75 | testSE2dist(); 76 | } 77 | 78 | TEST(distance_11_1) { 79 | testSE2dist(); 80 | } 81 | 82 | TEST(distance_1_13) { 83 | testSE2dist(); 84 | } 85 | -------------------------------------------------------------------------------- /test/se3_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | // template 40 | // constexpr std::string_view type_name() { 41 | // std::string_view p = __PRETTY_FUNCTION__; 42 | // return std::string_view(p.data() + 34, p.size() - 34 - 1); 43 | // } 44 | 45 | template 46 | void testSE3dist() { 47 | using namespace unc::robotics::mpt; 48 | using Space = SE3Space; 49 | using Quat = Eigen::Quaternion; 50 | using Vec3 = Eigen::Matrix; 51 | using AAxs = Eigen::AngleAxis; 52 | 53 | // std::cout << type_name() << std::endl; 54 | 55 | static_assert(std::is_same_v, typename Space::Type>); 56 | 57 | Space space; 58 | 59 | typename Space::Type a, b; 60 | 61 | Vec3 axis{-1,2,3}; 62 | axis.normalize(); 63 | 64 | std::get(a) = AAxs(-1.0, axis); 65 | std::get<0>(b) = AAxs(2.0, axis); 66 | 67 | std::get<1>(a) << 1, 2, 3; 68 | std::get(b) << 1, 0, -1; 69 | 70 | Scalar expected = 3.0/2 * so3w + std::sqrt(0.0 + 2.0*2.0 + 4.0*4.0) * l2w; 71 | EXPECT(std::abs(space.distance(a, b) - expected)) < Scalar(1e-9); 72 | } 73 | 74 | 75 | TEST(distance_1_1) { 76 | testSE3dist(); 77 | } 78 | 79 | TEST(distance_5_2) { 80 | testSE3dist(); 81 | } 82 | 83 | TEST(distance_11_1) { 84 | testSE3dist(); 85 | } 86 | 87 | TEST(distance_1_13) { 88 | testSE3dist(); 89 | } 90 | 91 | TEST(interpolate) { 92 | using namespace unc::robotics::mpt; 93 | using Space = SE3Space; 94 | 95 | Space space; 96 | typename Space::Type a, b, c; 97 | 98 | Eigen::Vector3d axis{-1, 2, 3}; 99 | axis.normalize(); 100 | 101 | std::get(a) = Eigen::AngleAxis(0.5, axis); 102 | std::get(b) = Eigen::AngleAxis(2.5, axis); 103 | std::get(a) << 1, 2, 3; 104 | std::get(b) << 1, 0, -1; 105 | 106 | c = interpolate(space, a, b, 0.1); 107 | 108 | EXPECT(std::get(c)[0]) == 1.0; 109 | EXPECT(std::get(c)[1]) == 1.8; 110 | EXPECT(std::get(c)[2]) == 2.6; 111 | 112 | 113 | Eigen::AngleAxisd aa; 114 | aa = std::get(c); 115 | 116 | EXPECT((aa.axis() - axis).squaredNorm()) < 1e-15; 117 | EXPECT(std::abs(aa.angle() - (0.5 + 2*0.1))) < 1e-10; 118 | } 119 | -------------------------------------------------------------------------------- /test/so2_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(distance_scalar) { 40 | using Space = unc::robotics::mpt::SO2Space; 41 | 42 | static_assert(std::is_same_v); 43 | 44 | Space space; 45 | 46 | EXPECT(space.distance(1.0, 1.0)) == 0; 47 | EXPECT(space.distance(0.0, 2.0)) == 2.0; 48 | EXPECT(space.distance(2.0, 0.0)) == 2.0; 49 | EXPECT(space.distance(-1.0, 3.0)) == 2*M_PI - 4.0; 50 | EXPECT(space.distance(3.0, -1.0)) == 2*M_PI - 4.0; 51 | } 52 | 53 | 54 | TEST(distance_lp1) { 55 | using Space = unc::robotics::mpt::SO2LPSpace; 56 | 57 | static_assert(std::is_same_v); 58 | 59 | typename Space::Type a(1,2,3); 60 | typename Space::Type b(1,0,-1); 61 | 62 | Space space; 63 | 64 | EXPECT(space.distance(a, b)) == 0.0 + 2.0 + 2*M_PI - 4; 65 | } 66 | 67 | TEST(interpolate_scalar) { 68 | using namespace unc::robotics::mpt; 69 | using Space = unc::robotics::mpt::SO2Space; 70 | 71 | Space space; 72 | 73 | EXPECT(interpolate(space, 1.0, 1.0, 0.0)) == 1.0; 74 | EXPECT(interpolate(space, 1.0, 1.0, 3.0)) == 1.0; 75 | 76 | EXPECT(interpolate(space, 1.0, 2.0, 0.5)) == 1.5; 77 | EXPECT(interpolate(space, 1.0, 2.0, -3.0)) == -2.0; 78 | EXPECT(interpolate(space, -1.0, 2.0, 4.0)) == 11.0 - 4*M_PI; 79 | 80 | EXPECT(interpolate(space, 5*M_PI/6, -5*M_PI/6, 1.0)) == -5*M_PI/6; 81 | EXPECT(interpolate(space, 5*M_PI/6, -5*M_PI/6, 2.0)) == -3*M_PI/6; 82 | // EXPECT(interpolate(space, -5*M_PI/6, 5*M_PI/6, -2.0)) == -1*M_PI/6; 83 | EXPECT(interpolate(space, -5*M_PI/6, 5*M_PI/6, 2.0)) == 3*M_PI/6; 84 | } 85 | -------------------------------------------------------------------------------- /test/so3_space_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include "test.hpp" 38 | 39 | TEST(distance) { 40 | using Space = unc::robotics::mpt::SO3Space; 41 | 42 | static_assert(std::is_same_v); 43 | 44 | typename Space::Type a, b; 45 | 46 | Eigen::Vector3d axis{-1,2,3}; 47 | axis.normalize(); 48 | a = Eigen::AngleAxisd(-1.0, axis); 49 | b = Eigen::AngleAxisd(2.0, axis); 50 | 51 | Space space; 52 | 53 | // currently the distance metric is halved since the distance is 54 | // computed acos(abs(dot(a,b))) without an additional 2* multiplier. 55 | EXPECT(std::abs(space.distance(a, b) - 3.0/2)) < 1e-10; 56 | } 57 | 58 | TEST(interpolate) { 59 | using namespace unc::robotics::mpt; 60 | 61 | using Space = SO3Space; 62 | using State = typename Space::Type; 63 | 64 | Space space; 65 | State a, b, c; 66 | 67 | Eigen::Vector3d axis{-1,2,3}; 68 | axis.normalize(); 69 | a = Eigen::AngleAxisd(0.5, axis); 70 | b = Eigen::AngleAxisd(2.5, axis); 71 | 72 | c = interpolate(space, a, b, 0.1); 73 | 74 | Eigen::AngleAxisd aa; 75 | aa = c; 76 | 77 | EXPECT((aa.axis() - axis).squaredNorm()) < 1e-15; 78 | EXPECT(std::abs(aa.angle() - (0.5 + 2*0.1))) < 1e-10; 79 | } 80 | -------------------------------------------------------------------------------- /test/thread_pool_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "test.hpp" 3 | #include 4 | 5 | TEST(run_in_pool) { 6 | using namespace unc::robotics::mpt::impl; 7 | using namespace std::literals; 8 | 9 | static constexpr int n = 32; 10 | ThreadPool pool(n); 11 | std::atomic_int count{0}; 12 | 13 | for (int i=1 ; i firstThreads; 36 | 37 | for (int iter=0 ; iter<2 ; ++iter) { 38 | std::atomic_int doneCount{0}; 39 | std::set threads; 40 | 41 | for (int i=1 ; i lock(mutex); 47 | threads.emplace(std::this_thread::get_id()); 48 | waitToReturn.wait(lock, [&] { return threads.size() == poolSize-1; }); 49 | waitToReturn.notify_one(); 50 | } 51 | // make sure that the main thread does not 52 | // continue until we've released the lock and will 53 | // no longer use the condition variables. 54 | if (++doneCount == poolSize - 1) 55 | allDone.notify_one(); 56 | }); 57 | 58 | std::unique_lock lock(mutex); 59 | allDone.wait(lock, [&] { return doneCount == poolSize-1; }); 60 | 61 | EXPECT(threads.size()) == poolSize-1; 62 | 63 | if (iter == 0) { 64 | firstThreads = threads; 65 | } else { 66 | for (auto id : threads) 67 | EXPECT(firstThreads.count(id)) == 1; 68 | } 69 | } 70 | } 71 | 72 | TEST(use_env_variable) { 73 | using namespace unc::robotics::mpt::impl; 74 | 75 | const char *old = getenv("OMP_NUM_THREADS"); 76 | std::string oldCopy; 77 | if (old) 78 | oldCopy = old; 79 | 80 | // check that if OMP_NUM_THREADS is not set, that it uses 81 | // hardware_concurrency by default 82 | unsetenv("OMP_NUM_THREADS"); 83 | { 84 | ThreadPool pool; 85 | EXPECT(pool.size()) == std::max(1u, std::thread::hardware_concurrency()); 86 | } 87 | 88 | // If pool size is specified, that takes precedence 89 | { 90 | ThreadPool pool(3); 91 | EXPECT(pool.size()) == 3; 92 | } 93 | 94 | // If OMP_NUM_THREADS is specified, use it 95 | setenv("OMP_NUM_THREADS", "5", 1); 96 | { 97 | ThreadPool pool; 98 | EXPECT(pool.size()) == 5; 99 | } 100 | 101 | // If pool size is specified, that takes precedence 102 | { 103 | ThreadPool pool(7); 104 | EXPECT(pool.size()) == 7; 105 | } 106 | 107 | // If the environment variable is invalid, then use hardware 108 | // concurrency. (it will also display a message to the user) 109 | setenv("OMP_NUM_THREADS", "one", 1); 110 | { 111 | ThreadPool pool; 112 | EXPECT(pool.size()) == std::max(1u, std::thread::hardware_concurrency()); 113 | } 114 | 115 | 116 | if (old) { 117 | setenv("OMP_NUM_THREADS", oldCopy.c_str(), 1); 118 | } else { 119 | unsetenv("OMP_NUM_THREADS"); 120 | } 121 | } 122 | 123 | -------------------------------------------------------------------------------- /test/uniform_sampler_lp_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | #include 39 | #include "test.hpp" 40 | 41 | TEST(sampler) { 42 | using namespace unc::robotics::mpt; 43 | 44 | using Vec3 = Eigen::Vector3d; 45 | using Space = L2Space; 46 | using Bounds = BoxBounds; 47 | 48 | Space space; 49 | Vec3 min(1,2,3); 50 | Vec3 max(4,6,8); 51 | Bounds bounds(min, max); 52 | 53 | UniformSampler sampler(space, bounds); 54 | 55 | std::mt19937_64 rng; 56 | 57 | Vec3 r = sampler(rng); 58 | 59 | EXPECT((min.array() <= r.array()).all() && (r.array() <= max.array()).all()) == true; 60 | } 61 | -------------------------------------------------------------------------------- /test/uniform_sampler_so3_test.cpp: -------------------------------------------------------------------------------- 1 | // Software License Agreement (BSD-3-Clause) 2 | // 3 | // Copyright 2018 The University of North Carolina at Chapel Hill 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions 7 | // are met: 8 | // 9 | // 1. Redistributions of source code must retain the above copyright 10 | // notice, this list of conditions and the following disclaimer. 11 | // 12 | // 2. Redistributions in binary form must reproduce the above 13 | // copyright notice, this list of conditions and the following 14 | // disclaimer in the documentation and/or other materials provided 15 | // with the distribution. 16 | // 17 | // 3. Neither the name of the copyright holder nor the names of its 18 | // contributors may be used to endorse or promote products derived 19 | // from this software without specific prior written permission. 20 | // 21 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | // FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | // COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 26 | // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 27 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 28 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 29 | // HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 30 | // STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 31 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED 32 | // OF THE POSSIBILITY OF SUCH DAMAGE. 33 | 34 | //! @author Jeff Ichnowski 35 | 36 | #include 37 | #include 38 | #include 39 | #include "test.hpp" 40 | 41 | using namespace unc::robotics::mpt; 42 | 43 | TEST(random_so3_doubles) { 44 | using Space = SO3Space; 45 | using Bounds = Unbounded; 46 | using Sampler = UniformSampler; 47 | 48 | Space space; 49 | Sampler sampler(space); 50 | 51 | using RNG = mersenne_twister_select; 52 | 53 | RNG rng; 54 | 55 | for (int i=0 ; i<1000000 ; ++i) { 56 | Eigen::Quaternion q = sampler(rng); 57 | EXPECT(std::abs(q.coeffs().squaredNorm() - 1)) < 4 * std::numeric_limits::epsilon(); 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /test/worker_pool_openmp_test.cpp: -------------------------------------------------------------------------------- 1 | #define MPT_USE_OPENMP 1 2 | #include "worker_pool_test_common.hpp" 3 | -------------------------------------------------------------------------------- /test/worker_pool_std_thread_test.cpp: -------------------------------------------------------------------------------- 1 | #define MPT_USE_STD_THREAD 1 2 | #include "worker_pool_test_common.hpp" 3 | -------------------------------------------------------------------------------- /test/worker_pool_test_common.hpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "test.hpp" 3 | 4 | namespace mpt_test { 5 | class TestWorker { 6 | unsigned no_; 7 | 8 | public: 9 | TestWorker(unsigned no, double three) : no_(no) { 10 | EXPECT(three) == 3.0; 11 | } 12 | 13 | template 14 | void solve(std::atomic_int& startCount, DoneFn done) { 15 | using namespace std::literals; 16 | --startCount; 17 | while (!done()) { 18 | std::this_thread::sleep_for(1ms); 19 | } 20 | } 21 | }; 22 | } 23 | 24 | TEST(solve) { 25 | using namespace unc::robotics::mpt::impl; 26 | using namespace mpt_test; 27 | using namespace std::literals; 28 | 29 | WorkerPool pool(3.0); 30 | 31 | EXPECT(pool.size()) > 0; 32 | 33 | using Clock = std::chrono::steady_clock; 34 | // normally the planner is passed in as the context. Here we just 35 | // send in the atomic int to facilitate the test. 36 | std::atomic_int count{(int)pool.size()}; 37 | pool.solve(count, [&, start = Clock::now()] { 38 | return count == 0 || Clock::now() - start > 60s; 39 | }); 40 | 41 | EXPECT(count.load()) == 0; 42 | } 43 | --------------------------------------------------------------------------------