├── .clang-format ├── .gitignore ├── .gitmodules ├── LICENSE ├── README.md ├── assets ├── result_2D_InformedRRTStar_circle.png ├── result_2D_InformedRRTStar_img.png ├── result_2D_RRTStar_circle.png ├── result_2D_RRTStar_img.png ├── result_2D_RRT_circle.png ├── result_2D_RRT_img.png ├── result_2D_circle.png └── result_2D_img.png ├── examples └── path-planning-2D │ ├── CMakeLists.txt │ ├── data │ ├── constraint0.png │ ├── constraint1.png │ └── param.yaml │ └── src │ ├── main.cpp │ └── main.h └── lib ├── CMakeLists.txt ├── include ├── Constraint │ ├── ConstraintBase.h │ ├── GridConstraint │ │ └── GridConstraint.h │ └── PointCloudConstraint │ │ └── PointCloudConstraint.h ├── EuclideanSpace │ └── EuclideanSpace.h ├── Node │ ├── KDTreeNodeList │ │ └── KDTreeNodeList.h │ ├── Node.h │ ├── NodeListBase.h │ └── SimpleNodeList │ │ └── SimpleNodeList.h ├── Planner │ ├── InformedRRTStar │ │ └── InformedRRTStar.h │ ├── PlannerBase.h │ ├── RRT │ │ └── RRT.h │ └── RRTStar │ │ └── RRTStar.h ├── Sampler │ └── Sampler.h ├── State │ └── State.h └── planner.h └── src ├── Constraint ├── ConstraintBase.cpp ├── GridConstraint │ └── GridConstraint.cpp └── PointCloudConstraint │ └── PointCloudConstraint.cpp ├── EuclideanSpace └── EuclideanSpace.cpp ├── Node ├── KDTreeNodeList │ └── KDTreeNodeList.cpp ├── Node.cpp ├── NodeListBase.cpp └── SimpleNodeList │ └── SimpleNodeList.cpp ├── Planner ├── InformedRRTStar │ └── InformedRRTStar.cpp ├── PlannerBase.cpp ├── RRT │ └── RRT.cpp └── RRTStar │ └── RRTStar.cpp ├── Sampler └── Sampler.cpp └── State └── State.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | Language: Cpp 3 | # BasedOnStyle: Google 4 | AccessModifierOffset: -1 5 | AlignAfterOpenBracket: Align 6 | AlignConsecutiveMacros: false 7 | AlignConsecutiveAssignments: false 8 | AlignConsecutiveDeclarations: false 9 | AlignEscapedNewlines: Left 10 | AlignOperands: true 11 | AlignTrailingComments: true 12 | AllowAllArgumentsOnNextLine: true 13 | AllowAllConstructorInitializersOnNextLine: true 14 | AllowAllParametersOfDeclarationOnNextLine: true 15 | AllowShortBlocksOnASingleLine: Never 16 | AllowShortCaseLabelsOnASingleLine: false 17 | AllowShortFunctionsOnASingleLine: All 18 | AllowShortLambdasOnASingleLine: All 19 | AllowShortIfStatementsOnASingleLine: WithoutElse 20 | AllowShortLoopsOnASingleLine: true 21 | AlwaysBreakAfterDefinitionReturnType: None 22 | AlwaysBreakAfterReturnType: None 23 | AlwaysBreakBeforeMultilineStrings: true 24 | AlwaysBreakTemplateDeclarations: Yes 25 | BinPackArguments: true 26 | BinPackParameters: true 27 | BraceWrapping: 28 | AfterCaseLabel: false 29 | AfterClass: false 30 | AfterControlStatement: false 31 | AfterEnum: false 32 | AfterFunction: false 33 | AfterNamespace: false 34 | AfterObjCDeclaration: false 35 | AfterStruct: false 36 | AfterUnion: false 37 | AfterExternBlock: false 38 | BeforeCatch: false 39 | BeforeElse: false 40 | IndentBraces: false 41 | SplitEmptyFunction: true 42 | SplitEmptyRecord: true 43 | SplitEmptyNamespace: true 44 | BreakBeforeBinaryOperators: None 45 | BreakBeforeBraces: Attach 46 | BreakBeforeInheritanceComma: false 47 | BreakInheritanceList: BeforeColon 48 | BreakBeforeTernaryOperators: true 49 | BreakConstructorInitializersBeforeComma: false 50 | BreakConstructorInitializers: BeforeColon 51 | BreakAfterJavaFieldAnnotations: false 52 | BreakStringLiterals: true 53 | ColumnLimit: 120 54 | CommentPragmas: '^ IWYU pragma:' 55 | CompactNamespaces: false 56 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 57 | ConstructorInitializerIndentWidth: 4 58 | ContinuationIndentWidth: 4 59 | Cpp11BracedListStyle: true 60 | DeriveLineEnding: true 61 | DerivePointerAlignment: true 62 | DisableFormat: false 63 | ExperimentalAutoDetectBinPacking: false 64 | FixNamespaceComments: true 65 | ForEachMacros: 66 | - foreach 67 | - Q_FOREACH 68 | - BOOST_FOREACH 69 | IncludeBlocks: Regroup 70 | IncludeCategories: 71 | - Regex: '^' 72 | Priority: 2 73 | SortPriority: 0 74 | - Regex: '^<.*\.h>' 75 | Priority: 1 76 | SortPriority: 0 77 | - Regex: '^<.*' 78 | Priority: 2 79 | SortPriority: 0 80 | - Regex: '.*' 81 | Priority: 3 82 | SortPriority: 0 83 | IncludeIsMainRegex: '([-_](test|unittest))?$' 84 | IncludeIsMainSourceRegex: '' 85 | IndentCaseLabels: true 86 | IndentGotoLabels: true 87 | IndentPPDirectives: None 88 | IndentWidth: 2 89 | IndentWrappedFunctionNames: false 90 | JavaScriptQuotes: Leave 91 | JavaScriptWrapImports: true 92 | KeepEmptyLinesAtTheStartOfBlocks: false 93 | MacroBlockBegin: '' 94 | MacroBlockEnd: '' 95 | MaxEmptyLinesToKeep: 1 96 | NamespaceIndentation: None 97 | ObjCBinPackProtocolList: Never 98 | ObjCBlockIndentWidth: 2 99 | ObjCSpaceAfterProperty: false 100 | ObjCSpaceBeforeProtocolList: true 101 | PenaltyBreakAssignment: 2 102 | PenaltyBreakBeforeFirstCallParameter: 1 103 | PenaltyBreakComment: 300 104 | PenaltyBreakFirstLessLess: 120 105 | PenaltyBreakString: 1000 106 | PenaltyBreakTemplateDeclaration: 10 107 | PenaltyExcessCharacter: 1000000 108 | PenaltyReturnTypeOnItsOwnLine: 200 109 | PointerAlignment: Left 110 | RawStringFormats: 111 | - Language: Cpp 112 | Delimiters: 113 | - cc 114 | - CC 115 | - cpp 116 | - Cpp 117 | - CPP 118 | - 'c++' 119 | - 'C++' 120 | CanonicalDelimiter: '' 121 | BasedOnStyle: google 122 | - Language: TextProto 123 | Delimiters: 124 | - pb 125 | - PB 126 | - proto 127 | - PROTO 128 | EnclosingFunctions: 129 | - EqualsProto 130 | - EquivToProto 131 | - PARSE_PARTIAL_TEXT_PROTO 132 | - PARSE_TEST_PROTO 133 | - PARSE_TEXT_PROTO 134 | - ParseTextOrDie 135 | - ParseTextProtoOrDie 136 | CanonicalDelimiter: '' 137 | BasedOnStyle: google 138 | ReflowComments: true 139 | SortIncludes: true 140 | SortUsingDeclarations: true 141 | SpaceAfterCStyleCast: false 142 | SpaceAfterLogicalNot: false 143 | SpaceAfterTemplateKeyword: true 144 | SpaceBeforeAssignmentOperators: true 145 | SpaceBeforeCpp11BracedList: false 146 | SpaceBeforeCtorInitializerColon: true 147 | SpaceBeforeInheritanceColon: true 148 | SpaceBeforeParens: ControlStatements 149 | SpaceBeforeRangeBasedForLoopColon: true 150 | SpaceInEmptyBlock: false 151 | SpaceInEmptyParentheses: false 152 | SpacesBeforeTrailingComments: 2 153 | SpacesInAngles: false 154 | SpacesInConditionalStatement: false 155 | SpacesInContainerLiterals: true 156 | SpacesInCStyleCastParentheses: false 157 | SpacesInParentheses: false 158 | SpacesInSquareBrackets: false 159 | SpaceBeforeSquareBrackets: false 160 | Standard: Auto 161 | StatementMacros: 162 | - Q_UNUSED 163 | - QT_REQUIRE_VERSION 164 | TabWidth: 8 165 | UseCRLF: false 166 | UseTab: Never 167 | ... 168 | 169 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | lib/build/* 2 | examples/path-planning-2D/build/* 3 | examples/path-planning-3D/build/* 4 | 5 | .ccls-cache 6 | .dir-local.el 7 | compile_commands.json -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "examples/external/OpenCV-YAMLHelper"] 2 | path = examples/external/OpenCV-YAMLHelper 3 | url = https://github.com/medalotte/OpenCV-YAMLHelper.git 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Yuya Kudo 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # sampling-based-planners 2 | C++ implementation of RRT, RRT*, and Informed-RRT* using kd-tree for searching NN and NBHD nodes. Supports arbitrary dimensions and compiles as a shared library. 3 | 4 | ## Features 5 | - Provided as a shared library usable in **C++14** or higher 6 | - You can execute at **any dimensions** without recompiling the shared library 7 | - To quickly search for NN and NBHD nodes, a node list consists of **kd-tree**. 8 | 9 | ## Requirements 10 | The following software packages are required for building the shared library: 11 | - A C++ compiler with **C++14** or higher support 12 | - CMake **3.0** or higher 13 | - Eigen **3.0** or higher 14 | 15 | If you would like to compile the example programs, add the following: 16 | - OpenCV **3.0** or higher 17 | 18 | ## Build 19 | The shared library (**libplanner.so**) can be build with following commands 20 | 21 | ``` sh 22 | $ git clone https://github.com/medalotte/sampling-based-planners.git 23 | $ cd sampling-based-planners/lib 24 | $ mkdir build && cd build 25 | $ cmake .. 26 | $ make 27 | ``` 28 | 29 | The example program can be run with following commands after build the shared library 30 | 31 | ``` sh 32 | $ cd 33 | $ git submodule update --init 34 | $ cd examples/path-planning-2D 35 | $ mkdir build && cd build 36 | $ cmake .. 37 | $ make 38 | ``` 39 | 40 | ## Usage 41 | ### 1. Include header file and set alias optionally 42 | ``` c++ 43 | #include 44 | namespace planner = pln 45 | ``` 46 | 47 | ### 2. Define euclidean space 48 | ``` c++ 49 | // difinition of two-dimensional space 50 | const int DIM = 2; 51 | pln::EuclideanSpace space(DIM); 52 | 53 | // definition of bounds of each dimension 54 | std::vector bounds{pln::Bound(0, 100.0), 55 | pln::Bound(0, 100.0)}; 56 | 57 | // set bounds to space 58 | space.setBound(bounds); 59 | ``` 60 | 61 | ### 3. Define constraints 62 | #### i. Point cloud type 63 | ``` c++ 64 | // definition of obstacle (point cloud type) 65 | std::vector obstacles; 66 | obstacles.emplace_back(pln::State(10.0, 20.0), 10.0); // x : 10.0, y : 20.0, radius : 10.0 67 | obstacles.emplace_back(pln::State(50.0, 70.0), 20.0); // x : 50.0, y : 70.0, radius : 20.0 68 | obstacles.emplace_back(pln::State(-10.0, 120.0), 30.0); // there is no probrem out of range 69 | 70 | // definition of constraint using std::shared_ptr 71 | auto constraint = std::make_shared(space, obstacles) 72 | ``` 73 | 74 | #### ii. Image type (use OpenCV for simplicity) 75 | ``` c++ 76 | // read image 77 | auto world = cv::imread("./example.png", CV_8UC1); 78 | 79 | // definition of constraint array 80 | std::vector map(world.cols * world.rows, pln::ConstraintType::ENTAERABLE); 81 | 82 | for(int yi = 0; yi < world.rows; yi++) { 83 | for(int xi = 0; xi < world.cols; xi++) { 84 | if(world.data[xi + yi * world.cols] != 255) { 85 | map[xi + yi * world.cols] = pln::ConstraintType::NOENTRY; 86 | } 87 | } 88 | } 89 | 90 | std::vector each_dim_size{(uint32_t)world.cols, (uint32_t)world.rows}; 91 | 92 | // definition of constraint using std::shared_ptr 93 | auto constraint = std::make_shared(space, map, each_dim_size); 94 | ``` 95 | 96 | ### 4. Solve 97 | ``` c++ 98 | // definition of planner (you can set some parameters at optional argument) 99 | // pln::RRT planner(DIM); 100 | // pln::RRTStar planner(DIM); 101 | pln::InformedRRTStar planner(DIM); 102 | 103 | // set constraint 104 | planner.setProblemDefinition(constraint); 105 | 106 | // definition of start and goal state 107 | pln::State start(5.0, 5.0); 108 | pln::State goal(90.0, 90.0); 109 | 110 | // solve 111 | bool status = planner.solve(start, goal); 112 | if(status) { 113 | auto& result = planner.getResult(); 114 | for(const auto& r : result) { 115 | std::cout << r << std::endl; 116 | } 117 | } 118 | else { 119 | std::cout << "Could not find path" << std::endl; 120 | } 121 | ``` 122 | 123 | ## Example programs 124 | ### Example1. path-planning-2D 125 | Execute path planning on two-dimensional space 126 | 127 | #### Pattern1. Constraint using set of circle 128 |
129 | result_2D_circle.png 130 | left: RRT, center: RRT*, right: Informed-RRT* 131 |
132 | 133 | 134 | #### Pattern2. Constraint using image 135 |
136 | result_2D_img.png 137 | left: RRT, center: RRT*, right: Informed-RRT* 138 |
139 | 140 | ## References 141 | [Steven M. LaValle, "Rapidly-exploring random trees: A new tool for path planning," Technical Report. Computer Science Department, Iowa State University (TR 98–11).](http://msl.cs.uiuc.edu/~lavalle/papers/Lav98c.pdf) 142 | 143 | [S. Karaman and E. Frazzoli, "Incremental Sampling-based Algorithms for Optimal Motion Planning," arXiv:1005.0416, May. 2010.](https://arxiv.org/pdf/1005.0416.pdf) 144 | 145 | [J. D. Gammell, S. S. Srinivasa, and T. D. Barfoot, “Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic,” in 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, 2014, pp. 2997–3004.](https://ieeexplore.ieee.org/document/6942976/?arnumber=6942976) 146 | 147 | ## License 148 | MIT 149 | -------------------------------------------------------------------------------- /assets/result_2D_InformedRRTStar_circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/assets/result_2D_InformedRRTStar_circle.png -------------------------------------------------------------------------------- /assets/result_2D_InformedRRTStar_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/assets/result_2D_InformedRRTStar_img.png -------------------------------------------------------------------------------- /assets/result_2D_RRTStar_circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/assets/result_2D_RRTStar_circle.png -------------------------------------------------------------------------------- /assets/result_2D_RRTStar_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/assets/result_2D_RRTStar_img.png -------------------------------------------------------------------------------- /assets/result_2D_RRT_circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/assets/result_2D_RRT_circle.png -------------------------------------------------------------------------------- /assets/result_2D_RRT_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/assets/result_2D_RRT_img.png -------------------------------------------------------------------------------- /assets/result_2D_circle.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/assets/result_2D_circle.png -------------------------------------------------------------------------------- /assets/result_2D_img.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/assets/result_2D_img.png -------------------------------------------------------------------------------- /examples/path-planning-2D/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(path-planning-2D) 3 | 4 | IF(NOT CMAKE_BUILD_TYPE) 5 | SET(CMAKE_BUILD_TYPE Release) 6 | ENDIF() 7 | 8 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 9 | 10 | #--- enable output compile_command.json 11 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 12 | 13 | #--- $ cmake -DCMAKE_BUILD_TYPE=debug 14 | set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g -MMD -Wall -Wextra -Winit-self") 15 | 16 | #--- $ cmake 17 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O2 -march=native") 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O2 -march=native") 19 | 20 | #--- Check C++14 or C++0x support 21 | include(CheckCXXCompilerFlag) 22 | CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) 23 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 24 | if(COMPILER_SUPPORTS_CXX14) 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 26 | add_definitions(-DCOMPILEDWITHC14) 27 | message(STATUS "Using flag -std=c++14.") 28 | elseif(COMPILER_SUPPORTS_CXX0X) 29 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 30 | add_definitions(-DCOMPILEDWITHC0X) 31 | message(STATUS "Using flag -std=c++0x.") 32 | else() 33 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") 34 | endif() 35 | 36 | find_package(Eigen3 3.0.0 REQUIRED) 37 | 38 | find_package(OpenCV 3.0 QUIET) 39 | if(NOT OpenCV_FOUND) 40 | message(FATAL_ERROR "OpenCV > 3.0 not found.") 41 | endif() 42 | 43 | #--- Build 44 | set(LIBRARIES_DIR "${PROJECT_SOURCE_DIR}/../../lib") 45 | set(EXTARNAL_REPOSITORY_DIR "${PROJECT_SOURCE_DIR}/../external") 46 | 47 | include_directories( 48 | ${LIBRARIES_DIR}/include 49 | ${EXTARNAL_REPOSITORY_DIR}/OpenCV-YAMLHelper/include 50 | ${OpenCV_INCLUDE_DIRS} 51 | ${EIGEN3_INCLUDE_DIR} 52 | ) 53 | 54 | link_directories( 55 | ${LIBRARIES_DIR}/build) 56 | 57 | set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/build) 58 | 59 | add_executable(${PROJECT_NAME} 60 | ${PROJECT_SOURCE_DIR}/src/main.cpp 61 | ) 62 | 63 | target_link_libraries(${PROJECT_NAME} 64 | ${EIGEN3_LIBS} 65 | ${OpenCV_LIBS} 66 | planner 67 | ) 68 | -------------------------------------------------------------------------------- /examples/path-planning-2D/data/constraint0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/examples/path-planning-2D/data/constraint0.png -------------------------------------------------------------------------------- /examples/path-planning-2D/data/constraint1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/medalotte/sampling-based-planners/ad8b603be4fa2fcd43732235426f3d44d54d94e5/examples/path-planning-2D/data/constraint1.png -------------------------------------------------------------------------------- /examples/path-planning-2D/data/param.yaml: -------------------------------------------------------------------------------- 1 | %YAML:1.2 2 | --- 3 | RRT: 4 | max_sampling_num: 10000 5 | goal_sampling_rate: 0.05 6 | expand_dist: 30.0 7 | 8 | RRTStar: 9 | max_sampling_num: 10000 10 | goal_sampling_rate: 0.25 11 | expand_dist: 500 12 | R: 2500 13 | 14 | InformedRRTStar: 15 | max_sampling_num: 10000 16 | goal_sampling_rate: 0.25 17 | expand_dist: 500 18 | R: 2500 19 | goal_region_radius: 100.0 20 | 21 | StartState: 22 | x: 50.0 23 | y: 50.0 24 | 25 | GoalState: 26 | x: 900.0 27 | y: 900.0 28 | 29 | WorldSize: 30 | x: 1000.0 31 | y: 1000.0 32 | 33 | ImageTerminateSearchCost: 2980 34 | ConstraintImage: "./constraint1.png" 35 | 36 | ObstaclesTerminateSearchCost: 1330 37 | Obstacles: 38 | num: 16 39 | Obstacle0: 40 | x: 350.0 41 | y: 300.0 42 | radius: 80.0 43 | Obstacle1: 44 | x: 789.0 45 | y: 680.0 46 | radius: 80.0 47 | Obstacle2: 48 | x: 550.0 49 | y: 450.0 50 | radius: 100.0 51 | Obstacle3: 52 | x: 400.0 53 | y: 500.0 54 | radius: 50.0 55 | Obstacle4: 56 | x: 900.0 57 | y: 800.0 58 | radius: 60.0 59 | Obstacle5: 60 | x: 100.0 61 | y: 500.0 62 | radius: 50.0 63 | Obstacle6: 64 | x: 600.0 65 | y: 300.0 66 | radius: 70.0 67 | Obstacle7: 68 | x: 600.0 69 | y: 680.0 70 | radius: 100.0 71 | Obstacle8: 72 | x: 140.0 73 | y: 140.0 74 | radius: 80.0 75 | Obstacle9: 76 | x: 609.0 77 | y: 93.0 78 | radius: 90.0 79 | Obstacle10: 80 | x: 300.0 81 | y: 100.0 82 | radius: 50.0 83 | Obstacle11: 84 | x: 100.0 85 | y: 300.0 86 | radius: 100.0 87 | Obstacle12: 88 | x: 720.0 89 | y: 902.0 90 | radius: 60.0 91 | Obstacle13: 92 | x: 450.0 93 | y: 209.0 94 | radius: 79.0 95 | Obstacle14: 96 | x: 580.0 97 | y: 910.0 98 | radius: 20.0 99 | Obstacle15: 100 | x: 409.0 101 | y: 690.0 102 | radius: 60.0 103 | -------------------------------------------------------------------------------- /examples/path-planning-2D/src/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include "main.h" 26 | namespace pln = planner; 27 | 28 | int main() { 29 | const int DIM = 2; 30 | const std::string PARAM_YAML_DIRECTORY = "../data"; 31 | const std::string PARAM_YAML_FILE_NAME = "param.yaml"; 32 | const std::string PARAM_YAML_FILE_PATH = PARAM_YAML_DIRECTORY + "/" + PARAM_YAML_FILE_NAME; 33 | 34 | try { 35 | std::unique_ptr planner; 36 | std::shared_ptr constraint; 37 | 38 | RRTParam rrt_param; 39 | RRTStarParam rrt_star_param; 40 | InformedRRTStarParam informed_rrt_star_param; 41 | cvlib::YAMLHelper::readStruct(rrt_param, PARAM_YAML_FILE_PATH, "RRT"); 42 | cvlib::YAMLHelper::readStruct(rrt_star_param, PARAM_YAML_FILE_PATH, "RRTStar"); 43 | cvlib::YAMLHelper::readStruct(informed_rrt_star_param, PARAM_YAML_FILE_PATH, "InformedRRTStar"); 44 | 45 | StateParam world_size, start_state, goal_state; 46 | cvlib::YAMLHelper::readStruct(world_size, PARAM_YAML_FILE_PATH, "WorldSize"); 47 | cvlib::YAMLHelper::readStruct(start_state, PARAM_YAML_FILE_PATH, "StartState"); 48 | cvlib::YAMLHelper::readStruct(goal_state, PARAM_YAML_FILE_PATH, "GoalState"); 49 | 50 | auto img_terminate_search_cost = cvlib::YAMLHelper::read(PARAM_YAML_FILE_PATH, "ImageTerminateSearchCost"); 51 | auto obs_terminate_search_cost = 52 | cvlib::YAMLHelper::read(PARAM_YAML_FILE_PATH, "ObstaclesTerminateSearchCost"); 53 | auto terminate_search_cost = img_terminate_search_cost; 54 | 55 | while (true) { 56 | bool end_flag = false; 57 | cv::Mat world = cv::Mat::zeros(cv::Size(world_size.x, world_size.y), CV_8UC1); 58 | auto start = pln::State(start_state.x, start_state.y); 59 | auto goal = pln::State(goal_state.x, goal_state.y); 60 | 61 | [&]() { 62 | while (true) { 63 | char mode = '0'; 64 | std::cout << "[1] Please choose constraint type" << std::endl; 65 | std::cout << "1. image" << std::endl; 66 | std::cout << "2. set of circle" << std::endl; 67 | std::cout << "q. quit" << std::endl << std::endl; 68 | std::cout << "mode : "; 69 | std::cin >> mode; 70 | std::cout << std::endl; 71 | 72 | switch (mode) { 73 | case '1': { 74 | //--- set constraint based on image 75 | auto img_file_path = PARAM_YAML_DIRECTORY + "/" + 76 | cvlib::YAMLHelper::read(PARAM_YAML_FILE_PATH, "ConstraintImage"); 77 | 78 | world = cv::imread(img_file_path, CV_8UC1); 79 | 80 | //-- generate space and constraint 81 | // A space should set bound at each dimension 82 | pln::EuclideanSpace space(DIM); 83 | std::vector bounds{pln::Bound(0, world.cols), pln::Bound(0, world.rows)}; 84 | space.setBound(bounds); 85 | 86 | // A constraint based on image use one dimension 87 | // std::vector, and you should set each 88 | // dimension size to std::vector 89 | std::vector map(world.cols * world.rows, pln::ConstraintType::ENTAERABLE); 90 | for (int yi = 0; yi < world.rows; yi++) { 91 | for (int xi = 0; xi < world.cols; xi++) { 92 | if (world.data[xi + yi * world.cols] != 0) { 93 | map[xi + yi * world.cols] = pln::ConstraintType::NOENTRY; 94 | } 95 | } 96 | } 97 | 98 | std::vector each_dim_size{(uint32_t)world.cols, (uint32_t)world.rows}; 99 | 100 | //-- apply constraint 101 | constraint = std::make_shared(space, map, each_dim_size); 102 | 103 | //--- set terminate cost 104 | terminate_search_cost = img_terminate_search_cost; 105 | return; 106 | } 107 | case '2': { 108 | //--- set constraint based on set of circle 109 | //-- generate space and constraint 110 | // A space should set bound at each dimension 111 | pln::EuclideanSpace space(DIM); 112 | std::vector bounds{pln::Bound(0, world.cols), pln::Bound(0, world.rows)}; 113 | space.setBound(bounds); 114 | 115 | // set of circle define as 116 | // std::vector 117 | auto obstacle_num = cvlib::YAMLHelper::read(PARAM_YAML_FILE_PATH, "Obstacles", "num"); 118 | std::vector obstacles; 119 | for (int i = 0; i < obstacle_num; i++) { 120 | ObstacleParam obstacle_param; 121 | cvlib::YAMLHelper::readStruct(obstacle_param, PARAM_YAML_FILE_PATH, "Obstacles", 122 | "Obstacle" + std::to_string(i)); 123 | obstacles.emplace_back(pln::State(obstacle_param.x, obstacle_param.y), obstacle_param.radius); 124 | } 125 | 126 | //-- apply constraint 127 | constraint = std::make_shared(space, obstacles); 128 | 129 | //-- draw set of circle to world img 130 | for (const auto &obstacle : static_cast(constraint.get())->getRef()) { 131 | cv::circle(world, cv::Point(obstacle.getState().vals[0], obstacle.getState().vals[1]), 132 | obstacle.getRadius(), 0xFF, -1, CV_AA); 133 | } 134 | 135 | //--- set terminate cost 136 | terminate_search_cost = obs_terminate_search_cost; 137 | return; 138 | } 139 | case 'q': { 140 | end_flag = true; 141 | return; 142 | } 143 | default: { 144 | break; 145 | } 146 | } 147 | } 148 | }(); 149 | 150 | if (end_flag) { 151 | break; 152 | } 153 | 154 | [&]() { 155 | while (true) { 156 | char mode = '0'; 157 | std::cout << "[2] Please choose method of path planning" << std::endl; 158 | std::cout << "1. RRT" << std::endl; 159 | std::cout << "2. RRT*" << std::endl; 160 | std::cout << "3. Informed-RRT*" << std::endl; 161 | std::cout << "q. quit" << std::endl << std::endl; 162 | std::cout << "mode : "; 163 | std::cin >> mode; 164 | std::cout << std::endl; 165 | 166 | switch (mode) { 167 | case '1': { 168 | planner = std::make_unique(DIM, rrt_param.max_sampling_num, rrt_param.goal_sampling_rate, 169 | rrt_param.expand_dist); 170 | return; 171 | } 172 | case '2': { 173 | planner = std::make_unique(DIM, rrt_star_param.max_sampling_num, 174 | rrt_star_param.goal_sampling_rate, rrt_star_param.expand_dist, 175 | rrt_star_param.R); 176 | return; 177 | } 178 | case '3': { 179 | planner = std::make_unique( 180 | DIM, informed_rrt_star_param.max_sampling_num, informed_rrt_star_param.goal_sampling_rate, 181 | informed_rrt_star_param.expand_dist, informed_rrt_star_param.R, 182 | informed_rrt_star_param.goal_region_radius); 183 | return; 184 | } 185 | case 'q': { 186 | end_flag = true; 187 | return; 188 | } 189 | default: { 190 | break; 191 | } 192 | } 193 | } 194 | }(); 195 | 196 | if (end_flag) { 197 | break; 198 | } 199 | 200 | planner->setProblemDefinition(constraint); 201 | planner->setTerminateSearchCost(terminate_search_cost); 202 | 203 | auto start_time = std::chrono::system_clock::now(); 204 | bool status = planner->solve(start, goal); 205 | auto end_time = std::chrono::system_clock::now(); 206 | 207 | if (status) { 208 | // draw and output result 209 | auto node_list = planner->getNodeList(); 210 | auto result = planner->getResult(); 211 | 212 | cv::cvtColor(world, world, CV_GRAY2RGB); 213 | 214 | for (int yi = 0; yi < world.rows; yi++) { 215 | for (int xi = 0; xi < world.cols; xi++) { 216 | if (world.at(yi, xi) != cv::Vec3b(0, 0, 0)) { 217 | world.at(yi, xi) = cv::Vec3b(192, 128, 224); 218 | } 219 | } 220 | } 221 | 222 | auto leafs = node_list->searchLeafs(); 223 | for (auto node : leafs) { 224 | while (node->parent != nullptr) { 225 | cv::line(world, cv::Point(node->state.vals[0], node->state.vals[1]), 226 | cv::Point(node->parent->state.vals[0], node->parent->state.vals[1]), cv::Vec3b(64, 92, 16), 1.0, 227 | CV_AA); 228 | node = node->parent; 229 | } 230 | } 231 | 232 | auto prev_node_pos = cv::Point(result[0].vals[0], result[0].vals[1]); 233 | for (const auto &r : result) { 234 | std::cout << r << std::endl; 235 | 236 | cv::circle(world, cv::Point(r.vals[0], r.vals[1]), 3.0, cv::Vec3b(128, 128, 255), 1, CV_AA); 237 | cv::line(world, cv::Point(r.vals[0], r.vals[1]), prev_node_pos, cv::Vec3b(128, 255, 128), 1, CV_AA); 238 | prev_node_pos = cv::Point(r.vals[0], r.vals[1]); 239 | } 240 | 241 | cv::circle(world, cv::Point(result.front().vals[0], result.front().vals[1]), 6.0, cv::Vec3b(128, 255, 255), -1, 242 | CV_AA); 243 | cv::putText(world, "Start", cv::Point(result.front().vals[0] + 10, result.front().vals[1]), 244 | cv::FONT_HERSHEY_COMPLEX_SMALL | cv::FONT_ITALIC, 1.0, cv::Scalar(128, 255, 255), 1, CV_AA); 245 | 246 | cv::circle(world, cv::Point(result.back().vals[0], result.back().vals[1]), 6.0, cv::Vec3b(128, 255, 255), -1, 247 | CV_AA); 248 | cv::putText(world, "Goal", cv::Point(result.back().vals[0] + 10, result.back().vals[1]), 249 | cv::FONT_HERSHEY_COMPLEX_SMALL | cv::FONT_ITALIC, 1.0, cv::Scalar(128, 255, 255), 1, CV_AA); 250 | 251 | cv::namedWindow("world", cv::WINDOW_NORMAL | cv::WINDOW_KEEPRATIO); 252 | cv::imshow("world", world); 253 | cv::waitKey(0); 254 | cv::destroyWindow("world"); 255 | cv::imwrite("./result.png", world); 256 | 257 | std::cout << "total cost : " << planner->getResultCost() << std::endl; 258 | } else { 259 | std::cout << "Could not find path" << std::endl; 260 | } 261 | 262 | std::cout << "elapsed time : " 263 | << std::chrono::duration_cast(end_time - start_time).count() / 1000.0 << "ms" 264 | << std::endl 265 | << std::endl; 266 | } 267 | } catch (const std::exception &e) { 268 | std::cout << e.what() << std::endl; 269 | exit(1); 270 | } 271 | } 272 | -------------------------------------------------------------------------------- /examples/path-planning-2D/src/main.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef PATH_PLANNING_SRC_MAIN_H_ 26 | #define PATH_PLANNING_SRC_MAIN_H_ 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace { 36 | // Classes that have parameters. The detail is here. 37 | // https://github.com/kyk0910/OpenCV-YAMLHelper 38 | class RRTParam : public cvlib::YAMLHelper::Param { 39 | public: 40 | int max_sampling_num; 41 | double goal_sampling_rate; 42 | double expand_dist; 43 | void read(const cv::FileNode &node) override { 44 | max_sampling_num = (int)node["max_sampling_num"]; 45 | goal_sampling_rate = (double)node["goal_sampling_rate"]; 46 | expand_dist = (double)node["expand_dist"]; 47 | } 48 | }; 49 | 50 | class RRTStarParam : public cvlib::YAMLHelper::Param { 51 | public: 52 | int max_sampling_num; 53 | double goal_sampling_rate; 54 | double expand_dist; 55 | double R; 56 | void read(const cv::FileNode &node) override { 57 | max_sampling_num = (int)node["max_sampling_num"]; 58 | goal_sampling_rate = (double)node["goal_sampling_rate"]; 59 | expand_dist = (double)node["expand_dist"]; 60 | R = (double)node["R"]; 61 | } 62 | }; 63 | 64 | class InformedRRTStarParam : public cvlib::YAMLHelper::Param { 65 | public: 66 | int max_sampling_num; 67 | double goal_sampling_rate; 68 | double expand_dist; 69 | double R; 70 | double goal_region_radius; 71 | void read(const cv::FileNode &node) override { 72 | max_sampling_num = (int)node["max_sampling_num"]; 73 | goal_sampling_rate = (double)node["goal_sampling_rate"]; 74 | expand_dist = (double)node["expand_dist"]; 75 | R = (double)node["R"]; 76 | goal_region_radius = (double)node["goal_region_radius"]; 77 | } 78 | }; 79 | 80 | class StateParam : public cvlib::YAMLHelper::Param { 81 | public: 82 | double x; 83 | double y; 84 | void read(const cv::FileNode &node) override { 85 | x = (double)node["x"]; 86 | y = (double)node["y"]; 87 | } 88 | }; 89 | 90 | class ObstacleParam : public cvlib::YAMLHelper::Param { 91 | public: 92 | double x; 93 | double y; 94 | double radius; 95 | void read(const cv::FileNode &node) override { 96 | x = (double)node["x"]; 97 | y = (double)node["y"]; 98 | radius = (double)node["radius"]; 99 | } 100 | }; 101 | } // namespace 102 | 103 | #endif /* PATH_PLANNING_SRC_MAIN_H_ */ 104 | -------------------------------------------------------------------------------- /lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0) 2 | project(planner) 3 | 4 | IF(NOT CMAKE_BUILD_TYPE) 5 | SET(CMAKE_BUILD_TYPE Release) 6 | ENDIF() 7 | 8 | MESSAGE("Build type: " ${CMAKE_BUILD_TYPE}) 9 | 10 | #--- enable output compile_command.json 11 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 12 | 13 | #--- $ cmake -DCMAKE_BUILD_TYPE=debug 14 | set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g -MMD -Wall -Wextra -Winit-self") 15 | 16 | #--- $ cmake 17 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O2 -march=native") 18 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O2 -march=native") 19 | 20 | #--- Check C++14 or C++0x support 21 | include(CheckCXXCompilerFlag) 22 | CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) 23 | CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) 24 | if(COMPILER_SUPPORTS_CXX14) 25 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") 26 | add_definitions(-DCOMPILEDWITHC14) 27 | message(STATUS "Using flag -std=c++14.") 28 | elseif(COMPILER_SUPPORTS_CXX0X) 29 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 30 | add_definitions(-DCOMPILEDWITHC0X) 31 | message(STATUS "Using flag -std=c++0x.") 32 | else() 33 | message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.") 34 | endif() 35 | 36 | #--- -fPIC 37 | set(CMAKE_POSITION_INDEPENDENT_CODE ON) 38 | 39 | find_package(Eigen3 3.0.0 REQUIRED) 40 | 41 | #--- Build libplanner.so 42 | include_directories( 43 | ${PROJECT_SOURCE_DIR}/include 44 | ${EIGEN3_INCLUDE_DIR} 45 | ) 46 | 47 | add_library(${PROJECT_NAME} SHARED 48 | ${PROJECT_SOURCE_DIR}/src/State/State.cpp 49 | ${PROJECT_SOURCE_DIR}/src/EuclideanSpace/EuclideanSpace.cpp 50 | ${PROJECT_SOURCE_DIR}/src/Constraint/ConstraintBase.cpp 51 | ${PROJECT_SOURCE_DIR}/src/Constraint/GridConstraint/GridConstraint.cpp 52 | ${PROJECT_SOURCE_DIR}/src/Constraint/PointCloudConstraint/PointCloudConstraint.cpp 53 | ${PROJECT_SOURCE_DIR}/src/Sampler/Sampler.cpp 54 | ${PROJECT_SOURCE_DIR}/src/Node/Node.cpp 55 | ${PROJECT_SOURCE_DIR}/src/Node/NodeListBase.cpp 56 | ${PROJECT_SOURCE_DIR}/src/Node/SimpleNodeList/SimpleNodeList.cpp 57 | ${PROJECT_SOURCE_DIR}/src/Node/KDTreeNodeList/KDTreeNodeList.cpp 58 | ${PROJECT_SOURCE_DIR}/src/Planner/PlannerBase.cpp 59 | ${PROJECT_SOURCE_DIR}/src/Planner/RRT/RRT.cpp 60 | ${PROJECT_SOURCE_DIR}/src/Planner/RRTStar/RRTStar.cpp 61 | ${PROJECT_SOURCE_DIR}/src/Planner/InformedRRTStar/InformedRRTStar.cpp 62 | ) 63 | 64 | target_link_libraries(${PROJECT_NAME} 65 | ${EIGEN3_LIBS} 66 | ) 67 | -------------------------------------------------------------------------------- /lib/include/Constraint/ConstraintBase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_CONSTRAINT_CONSTRAINT_H_ 26 | #define LIB_INCLUDE_CONSTRAINT_CONSTRAINT_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | namespace planner { 34 | 35 | /** 36 | * definition of type of constraint 37 | */ 38 | enum class ConstraintType { NOENTRY, ENTAERABLE }; 39 | 40 | namespace base { 41 | 42 | /** 43 | * Base class of constraint for planner 44 | */ 45 | class ConstraintBase { 46 | public: 47 | EuclideanSpace space; 48 | 49 | /** 50 | * Constructor(ConstraintBase) 51 | * @space: target space 52 | */ 53 | explicit ConstraintBase(const EuclideanSpace &_space); 54 | virtual ~ConstraintBase(); 55 | 56 | uint32_t getDim() const; 57 | 58 | /** 59 | * Check whether collision occurred between src and dst 60 | * @src: source state 61 | * @dst: destination state 62 | * @Return: If the path of between 'src' and 'dst' entry 63 | * ConstraintType::NOENTRY, return false 64 | */ 65 | virtual bool checkCollision(const State &src, const State &dst) const; 66 | 67 | /** 68 | * Check constraint at given state 69 | * @state: target state 70 | * @Return: type of constraint at target state 71 | */ 72 | virtual ConstraintType checkConstraintType(const State &state) const; 73 | }; 74 | } // namespace base 75 | } // namespace planner 76 | 77 | #endif /* LIB_INCLUDE_CONSTRAINT_CONSTRAINT_H_ */ 78 | -------------------------------------------------------------------------------- /lib/include/Constraint/GridConstraint/GridConstraint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_CONSTRAINT_GRIDCONSTRAINT_GRIDCONSTRAINT_H_ 26 | #define LIB_INCLUDE_CONSTRAINT_GRIDCONSTRAINT_GRIDCONSTRAINT_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace planner { 36 | 37 | /** 38 | * Super class of planner::ConstraintBase 39 | * This class express constraint as like an image or multidimensional array 40 | * and nomalize automatically for space size when call checkConstraintType() 41 | */ 42 | class GridConstraint : public base::ConstraintBase { 43 | public: 44 | /** 45 | * Constructor(GridConstraint) 46 | * @space: target space 47 | */ 48 | explicit GridConstraint(const EuclideanSpace &space); 49 | 50 | /** 51 | * Constructor(GridConstraint) 52 | * @space: target space 53 | * @constraint: multidimensional array express as one dimensional array 54 | * (e.g. '(x, y)' -> 'x + y * x_size' where 2 dimensions) 55 | * @each_dim_size: each dimension size of constraint you set 56 | */ 57 | GridConstraint(const EuclideanSpace &space, const std::vector &constraint, 58 | const std::vector &each_dim_size); 59 | 60 | ~GridConstraint() override; 61 | 62 | void set(const std::vector &constraint, const std::vector &each_dim_size); 63 | 64 | const std::vector &getConstraintRef() const; 65 | 66 | const std::vector &getEachDimSizeRef() const; 67 | 68 | State calcGridIdx(const State &state) const; 69 | 70 | std::vector> calcLineIndices(State src_idx, State dst_idx) const; 71 | 72 | bool checkCollision(const State &src, const State &dst) const override; 73 | 74 | ConstraintType checkConstraintType(const State &state) const override; 75 | 76 | ConstraintType checkConstraintType(const std::vector &idx) const; 77 | 78 | private: 79 | std::vector constraint_; 80 | std::vector each_dim_size_; 81 | }; 82 | } // namespace planner 83 | 84 | #endif /* LIB_INCLUDE_CONSTRAINT_GRIDCONSTRAINT_GRIDCONSTRAINT_H_ */ 85 | -------------------------------------------------------------------------------- /lib/include/Constraint/PointCloudConstraint/PointCloudConstraint.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_CONSTRAINT_POINTCLOUDCONSTRAINT_POINTCLOUDCONSTRAINT_H_ 26 | #define LIB_INCLUDE_CONSTRAINT_POINTCLOUDCONSTRAINT_POINTCLOUDCONSTRAINT_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | 34 | namespace planner { 35 | 36 | /** 37 | * Super class of planner::ConstraintBase 38 | * This class express constraint as set of hypersphere 39 | */ 40 | class PointCloudConstraint : public base::ConstraintBase { 41 | public: 42 | /** 43 | * Hypersphere as a way of expressing of Obstacle 44 | */ 45 | class Hypersphere { 46 | public: 47 | /** 48 | * Constructor(Hypersphere) 49 | * state and radius are initialize to zero 50 | */ 51 | explicit Hypersphere(const uint32_t &dim); 52 | 53 | /** 54 | * Constructor(Hypersphere) 55 | * @state: center of hypersphere 56 | * @radius: radius of hypersphere 57 | * (normalize to a positive decimal) 58 | */ 59 | Hypersphere(const State &state, const double &radius); 60 | 61 | /** 62 | * setter and getter function 63 | */ 64 | void set(const State &state, const double &radius); 65 | void setState(const State &state); 66 | void setRadius(const double &radius); 67 | State getState() const; 68 | double getRadius() const; 69 | 70 | private: 71 | State state_; 72 | double radius_; 73 | }; 74 | 75 | /** 76 | * Constructor(PointCloudConstraint) 77 | * @space: target space 78 | */ 79 | explicit PointCloudConstraint(const EuclideanSpace &space); 80 | 81 | /** 82 | * Constructor(PointCloudConstraint) 83 | * @space: target space 84 | * @constraint: constraint that express set of hypersphere 85 | * if dimension of hypersphere different from dimension of 86 | * space, this constructor throw std::invalid_argument 87 | */ 88 | PointCloudConstraint(const EuclideanSpace &space, const std::vector &constraint); 89 | 90 | ~PointCloudConstraint() override; 91 | 92 | void set(const std::vector &constraint); 93 | 94 | const std::vector &getRef() const; 95 | 96 | bool checkCollision(const State &src, const State &dst) const override; 97 | 98 | ConstraintType checkConstraintType(const State &state) const override; 99 | 100 | private: 101 | std::vector constraint_; 102 | }; 103 | } // namespace planner 104 | 105 | #endif /* LIB_INCLUDE_CONSTRAINT_POINTCLOUDCONSTRAINT_POINTCLOUDCONSTRAINT_H_ \ 106 | */ 107 | -------------------------------------------------------------------------------- /lib/include/EuclideanSpace/EuclideanSpace.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_EUCLIDEANSPACE_EUCLIDEANSPACE_H_ 26 | #define LIB_INCLUDE_EUCLIDEANSPACE_EUCLIDEANSPACE_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace planner { 34 | 35 | /** 36 | * Express bound in euclidean space 37 | */ 38 | class Bound { 39 | public: 40 | double low; 41 | double high; 42 | 43 | Bound(); 44 | Bound(const double &low, const double &high); 45 | 46 | ~Bound(); 47 | 48 | double getRange() const; 49 | }; 50 | 51 | /** 52 | * Express euclidean space 53 | */ 54 | class EuclideanSpace { 55 | public: 56 | explicit EuclideanSpace(const uint32_t &dim); 57 | 58 | ~EuclideanSpace(); 59 | 60 | uint32_t getDim() const noexcept; 61 | 62 | void setBound(std::vector &bounds); 63 | 64 | Bound getBound(const uint32_t &dim) const; 65 | 66 | const std::vector &getBoundsRef() const; 67 | 68 | private: 69 | uint32_t dim_; 70 | std::vector bounds_; 71 | }; 72 | } // namespace planner 73 | 74 | #endif /* LIB_INCLUDE_EUCLIDEANSPACE_EUCLIDEANSPACE_H_ */ 75 | -------------------------------------------------------------------------------- /lib/include/Node/KDTreeNodeList/KDTreeNodeList.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_NODE_KDTREENODELIST_H_ 26 | #define LIB_INCLUDE_NODE_KDTREENODELIST_H_ 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace planner { 35 | class KDTreeNodeList : public base::NodeListBase { 36 | using NodePtr = std::shared_ptr; 37 | 38 | public: 39 | explicit KDTreeNodeList(const uint32_t &dim); 40 | ~KDTreeNodeList(); 41 | void add(const NodePtr &node); 42 | void init(); 43 | int getSize(); 44 | NodePtr searchNN(const NodePtr &node); 45 | std::vector searchNBHD(const NodePtr &node, const double &radius); 46 | std::vector searchLeafs(); 47 | 48 | private: 49 | const double REBALANCE_RATIO = 0.1; 50 | 51 | struct KDTreeNode { 52 | int idx; 53 | int axis; 54 | std::shared_ptr child_r; 55 | std::shared_ptr child_l; 56 | KDTreeNode() : idx(-1), axis(-1), child_r(nullptr), child_l(nullptr) {} 57 | }; 58 | 59 | using KDNodePtr = std::shared_ptr; 60 | 61 | KDNodePtr root_; 62 | std::vector nodes_; 63 | int depth_; 64 | 65 | void clearRec(const KDNodePtr &node); 66 | 67 | KDNodePtr buildRec(std::vector &indices, const int &offset, const int &npoints, const int &depth); 68 | 69 | KDNodePtr insertRec(const KDNodePtr &root, const int &new_node_index, const int &depth); 70 | 71 | void searchNNRec(const NodePtr &query, const KDNodePtr node, NodePtr &guess, double &min_dist) const; 72 | 73 | void searchNBHDRec(const NodePtr &query, const KDNodePtr node, std::vector &near_nodes, 74 | const double &radius) const; 75 | }; 76 | } // namespace planner 77 | 78 | #endif /* LIB_INCLUDE_NODE_KDTREENODELIST_H_ */ 79 | -------------------------------------------------------------------------------- /lib/include/Node/Node.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_NODE_NODE_H_ 26 | #define LIB_INCLUDE_NODE_NODE_H_ 27 | 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | namespace planner { 34 | class Node { 35 | public: 36 | State state; 37 | std::shared_ptr parent; 38 | double cost; 39 | double cost_to_goal; 40 | bool is_leaf; 41 | 42 | Node(const State &_state, const std::shared_ptr _parent, const double &_cost = 0.0, 43 | const double &_cost_to_goal = std::numeric_limits::max()); 44 | ~Node(); 45 | }; 46 | } // namespace planner 47 | 48 | #endif /* LIB_INCLUDE_NODE_NODE_H_ */ 49 | -------------------------------------------------------------------------------- /lib/include/Node/NodeListBase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_NODE_NODELISTBASE_H_ 26 | #define LIB_INCLUDE_NODE_NODELISTBASE_H_ 27 | 28 | #include 29 | 30 | namespace planner { 31 | namespace base { 32 | /** 33 | * Base class of node list for sampling-based planners 34 | */ 35 | class NodeListBase { 36 | using NodePtr = std::shared_ptr; 37 | 38 | public: 39 | const uint32_t DIM; 40 | explicit NodeListBase(const uint32_t &_dim); 41 | virtual ~NodeListBase(); 42 | virtual void add(const NodePtr &node) = 0; 43 | virtual void init() = 0; 44 | virtual int getSize() = 0; 45 | virtual NodePtr searchNN(const NodePtr &node) = 0; 46 | virtual std::vector searchNBHD(const NodePtr &node, const double &radius) = 0; 47 | virtual std::vector searchLeafs() = 0; 48 | }; 49 | } // namespace base 50 | } // namespace planner 51 | 52 | #endif /* LIB_INCLUDE_NODE_NODELISTBASE_H_ */ 53 | -------------------------------------------------------------------------------- /lib/include/Node/SimpleNodeList/SimpleNodeList.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_NODE_SIMPLENODELIST_H_ 26 | #define LIB_INCLUDE_NODE_SIMPLENODELIST_H_ 27 | 28 | #include 29 | 30 | #include 31 | 32 | namespace planner { 33 | /** 34 | * using std::vector and the NN and NBHD are solved by linear search. 35 | */ 36 | class SimpleNodeList : public base::NodeListBase { 37 | using NodePtr = std::shared_ptr; 38 | 39 | public: 40 | explicit SimpleNodeList(const uint32_t &dim); 41 | ~SimpleNodeList(); 42 | void add(const NodePtr &node); 43 | void init(); 44 | int getSize(); 45 | NodePtr searchNN(const NodePtr &node); 46 | std::vector searchNBHD(const NodePtr &node, const double &radius); 47 | std::vector searchLeafs(); 48 | 49 | private: 50 | std::vector list_; 51 | }; 52 | } // namespace planner 53 | 54 | #endif /* LIB_INCLUDE_NODE_SIMPLENODELIST_H_ */ 55 | -------------------------------------------------------------------------------- /lib/include/Planner/InformedRRTStar/InformedRRTStar.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_PLANNER_INFORMEDRRTSTAR_INFORMEDRRTSTAR_H_ 26 | #define LIB_INCLUDE_PLANNER_INFORMEDRRTSTAR_INFORMEDRRTSTAR_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace planner { 39 | class InformedRRTStar : public base::PlannerBase { 40 | public: 41 | InformedRRTStar(const uint32_t &dim, const uint32_t &max_sampling_num = 1000, const double &goal_sampling_rate = 0.25, 42 | const double &expand_dist = 1.0, const double &R = 10.0, const double &goal_region_radius = 10.0); 43 | ~InformedRRTStar(); 44 | 45 | void setMaxSamplingNum(const uint32_t &max_sampling_num); 46 | void setGoalSamplingRate(const double &goal_sampling_rate); 47 | void setExpandDist(const double &expand_dist); 48 | void setR(const double &R); 49 | void setGoalRegionRadius(const double &goal_region_radius); 50 | 51 | bool solve(const State &start, const State &goal) override; 52 | 53 | private: 54 | uint32_t max_sampling_num_; 55 | double goal_sampling_rate_; 56 | double expand_dist_; 57 | double R_; 58 | double goal_region_radius_; 59 | }; 60 | } // namespace planner 61 | 62 | #endif /* LIB_INCLUDE_PLANNER_INFORMEDRRTSTAR_INFORMEDRRTSTAR_H_ */ 63 | -------------------------------------------------------------------------------- /lib/include/Planner/PlannerBase.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_PLANNER_PLANNERBASE_H_ 26 | #define LIB_INCLUDE_PLANNER_PLANNERBASE_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace planner { 33 | namespace base { 34 | /** 35 | * Base class of planners which are sampling-based method 36 | */ 37 | class PlannerBase { 38 | public: 39 | explicit PlannerBase(const uint32_t &dim, std::shared_ptr node_list); 40 | virtual ~PlannerBase(); 41 | 42 | PlannerBase(const PlannerBase &) = delete; 43 | PlannerBase &operator=(const PlannerBase &) = delete; 44 | PlannerBase(PlannerBase &&) noexcept = delete; 45 | PlannerBase &operator=(PlannerBase &&) noexcept = delete; 46 | 47 | /** 48 | * Execute path planning 49 | * @start: start state 50 | * @goal: goal state 51 | * @Return: whether the path planning was successful 52 | */ 53 | virtual bool solve(const State &start, const State &goal) = 0; 54 | 55 | void setProblemDefinition(const std::shared_ptr &constraint); 56 | 57 | void setTerminateSearchCost(const double &terminate_search_cost); 58 | 59 | const std::vector &getResult() const; 60 | 61 | double getResultCost() const; 62 | 63 | std::shared_ptr getNodeList() const; 64 | 65 | protected: 66 | std::vector result_; 67 | double result_cost_; 68 | double terminate_search_cost_; 69 | std::shared_ptr constraint_; 70 | std::shared_ptr node_list_; 71 | std::unique_ptr sampler_; 72 | 73 | /** 74 | * Generate Steered node that is 'expand_dist' away from 'src_node' to 75 | * 'dst_node' direction 76 | * @src_node: source node 77 | * @dst_node: destination node 78 | * @expand_dist: distance from 'src_node' of steered node 79 | * @Return: steered node 80 | */ 81 | std::shared_ptr generateSteerNode(const std::shared_ptr &src_node, const std::shared_ptr &dst_node, 82 | const double &expand_dist) const; 83 | 84 | /** 85 | * Choose parent node from near node that find in findNearNodes() 86 | * @target_node: target node 87 | * @node_list: list that contein existing node 88 | * @near_node_indexes: return value of findNearNodes() 89 | * @Return: node that choosed new parent node 90 | */ 91 | void updateParent(const std::shared_ptr &target_node, 92 | const std::vector> &near_nodes) const; 93 | 94 | /** 95 | * redefine parent node of near node that find in findNearNodes() 96 | * @node_list: list that contein existing node 97 | * @near_node_indexes: return value of findNearNodes() 98 | * @Return: nodes which are rewired 99 | */ 100 | std::vector> rewireNearNodes(std::shared_ptr &new_node, 101 | std::vector> &near_nodes) const; 102 | }; 103 | } // namespace base 104 | } // namespace planner 105 | 106 | #endif /* LIB_INCLUDE_PLANNER_PLANNERBASE_H_ */ 107 | -------------------------------------------------------------------------------- /lib/include/Planner/RRT/RRT.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_PLANNER_RRT_RRT_H_ 26 | #define LIB_INCLUDE_PLANNER_RRT_RRT_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace planner { 39 | class RRT : public base::PlannerBase { 40 | public: 41 | RRT(const uint32_t &dim, const uint32_t &max_sampling_num = 10000, const double &goal_sampling_rate = 0.05, 42 | const double &expand_dist = 1.0); 43 | ~RRT(); 44 | 45 | void setMaxSamplingNum(uint32_t max_sampling_num) noexcept; 46 | void setGoalSamplingRate(double goal_sampling_rate); 47 | void setExpandDist(double expand_dist) noexcept; 48 | 49 | bool solve(const State &start, const State &goal) override; 50 | 51 | private: 52 | uint32_t max_sampling_num_; 53 | double goal_sampling_rate_; 54 | double expand_dist_; 55 | }; 56 | } // namespace planner 57 | 58 | #endif /* LIB_INCLUDE_PLANNER_RRT_RRT_H_ */ 59 | -------------------------------------------------------------------------------- /lib/include/Planner/RRTStar/RRTStar.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_PLANNER_RRTSTAR_RRTSTAR_H_ 26 | #define LIB_INCLUDE_PLANNER_RRTSTAR_RRTSTAR_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | namespace planner { 39 | class RRTStar : public base::PlannerBase { 40 | public: 41 | RRTStar(const uint32_t &dim, const uint32_t &max_sampling_num = 1000, const double &goal_sampling_rate = 0.25, 42 | const double &expand_dist = 1.0, const double &R = 10.0); 43 | ~RRTStar(); 44 | 45 | void setMaxSamplingNum(const uint32_t &max_sampling_num); 46 | void setGoalSamplingRate(const double &goal_sampling_rate); 47 | void setExpandDist(const double &expand_dist); 48 | void setR(const double &R); 49 | 50 | bool solve(const State &start, const State &goal) override; 51 | 52 | private: 53 | uint32_t max_sampling_num_; 54 | double goal_sampling_rate_; 55 | double expand_dist_; 56 | double R_; 57 | }; 58 | } // namespace planner 59 | 60 | #endif /* LIB_INCLUDE_PLANNER_RRTSTAR_RRTSTAR_H_ */ 61 | -------------------------------------------------------------------------------- /lib/include/Sampler/Sampler.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_SAMPLER_SAMPLER_H_ 26 | #define LIB_INCLUDE_SAMPLER_SAMPLER_H_ 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace planner { 36 | class Sampler { 37 | public: 38 | enum class Mode { WholeArea, HeuristicDomain }; 39 | 40 | explicit Sampler(const EuclideanSpace &space); 41 | Sampler(const EuclideanSpace &space, const State &start, const State &goal, 42 | const double &best_cost = std::numeric_limits::max()); 43 | ~Sampler(); 44 | 45 | void applyStartAndGoal(const State &start, const State &goal); 46 | 47 | void setBestCost(const double &best_cost); 48 | 49 | /** 50 | * get unit uniform distribution value (i.e., return between 0.0 and 1.0) 51 | * @Return: random value 52 | */ 53 | double getUniformUnitRandomVal(); 54 | 55 | /** 56 | * sample a ramdom state from certain area 57 | * @mode: Mode::WholeArea: sample a random state from whole area 58 | * Mode::HeuristicDomain: sample a random state from heuristic domain 59 | * @Return: sampled state 60 | */ 61 | State run(const Mode &mode = Mode::WholeArea); 62 | 63 | private: 64 | const uint32_t dim_; 65 | 66 | std::mt19937 rand_; 67 | std::normal_distribution<> dist_gauss_; 68 | std::uniform_real_distribution<> dist_unit_; 69 | std::vector> dist_space_; 70 | 71 | double min_cost_; 72 | double best_cost_; 73 | Eigen::MatrixXd rotate_mat_; 74 | Eigen::VectorXd center_state_; 75 | 76 | /** 77 | * generate distribution for sampling a random state from whole area 78 | * @space: target space 79 | * @Return: uniform distribution object at each dimension 80 | */ 81 | std::vector> generateSpaceDistribution(const EuclideanSpace &space) const; 82 | 83 | /** 84 | * calculate rotation matrix using singular value decomposition 85 | * ${\bf C}={\bf U}diag(1,\ldots,1,\det({\bf U})\det({\bf V})){\bf V}^{T}$ 86 | * ${\bf U}\sum{}{\bf V}^{T}\equiv{\bf M}$ 87 | * ${\bf M}={\bf a}_{1}{\bf 1}_{1}^{T}$ 88 | * ${\bf a}_{1}=({\bf x}_{goal}-{\bf x}_{start})/\| {\bf x}_{goal}-{\bf 89 | * x}_{start} \|_2$ 90 | * 91 | * @start: start state 92 | * @goal: goal state 93 | * @Return: rotation matrix 94 | */ 95 | Eigen::MatrixXd calcRotationToWorldFlame(const State &start, const State &goal) const; 96 | 97 | /** 98 | * random sampling on unit n-ball 99 | * @dim: dimension 100 | * @Return: random state on unit n-ball 101 | */ 102 | State sampleUnitNBall(const uint32_t &dim); 103 | }; 104 | } // namespace planner 105 | 106 | #endif /* LIB_INCLUDE_SAMPLER_SAMPLER_H_ */ 107 | -------------------------------------------------------------------------------- /lib/include/State/State.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_STATE_STATE_H_ 26 | #define LIB_INCLUDE_STATE_STATE_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace planner { 35 | 36 | /** 37 | * Express state on multidimensional 38 | */ 39 | class State { 40 | public: 41 | std::vector vals; 42 | 43 | explicit State(uint32_t dim); 44 | 45 | explicit State(const std::vector &_vals); 46 | 47 | template 48 | State(const A &... _vals) : vals(std::initializer_list{_vals...}) {} 49 | 50 | ~State(); 51 | 52 | uint32_t getDim() const; 53 | 54 | double norm() const; 55 | 56 | double dot(const State &other) const; 57 | 58 | double distanceFrom(const State &other) const; 59 | 60 | State normalized() const; 61 | 62 | bool isZero() const; 63 | 64 | State operator+() const; 65 | State operator-() const; 66 | State operator+(const State &other) const; 67 | State operator-(const State &other) const; 68 | bool operator==(const State &other) const; 69 | bool operator!=(const State &other) const; 70 | State operator*(double s) const; 71 | State operator/(double s) const; 72 | 73 | friend std::ostream &operator<<(std::ostream &os, const State &obj); 74 | }; 75 | } // namespace planner 76 | 77 | #endif /* LIB_INCLUDE_STATE_STATE_H_ */ 78 | -------------------------------------------------------------------------------- /lib/include/planner.h: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #ifndef LIB_INCLUDE_PLANNER_H_ 26 | #define LIB_INCLUDE_PLANNER_H_ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #endif /* LIB_INCLUDE_PLANNER_H_ */ 35 | -------------------------------------------------------------------------------- /lib/src/Constraint/ConstraintBase.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | namespace base { 29 | ConstraintBase::ConstraintBase(const EuclideanSpace &_space) : space(_space) {} 30 | 31 | ConstraintBase::~ConstraintBase() {} 32 | 33 | uint32_t ConstraintBase::getDim() const { return space.getDim(); } 34 | 35 | bool ConstraintBase::checkCollision(const State &src, const State &dst) const { 36 | return (checkConstraintType(src) == ConstraintType::ENTAERABLE && 37 | checkConstraintType(dst) == ConstraintType::ENTAERABLE) 38 | ? true 39 | : false; 40 | } 41 | 42 | ConstraintType ConstraintBase::checkConstraintType(const State &state) const { 43 | for (size_t i = 0; i < state.getDim(); i++) { 44 | auto bound = space.getBound(i + 1); 45 | 46 | // return NOENTRY Type if the state is out of range 47 | if (state.vals[i] < bound.low || bound.high < state.vals[i]) { 48 | return ConstraintType::NOENTRY; 49 | } 50 | } 51 | 52 | return ConstraintType::ENTAERABLE; 53 | } 54 | } // namespace base 55 | } // namespace planner 56 | -------------------------------------------------------------------------------- /lib/src/Constraint/GridConstraint/GridConstraint.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | GridConstraint::GridConstraint(const EuclideanSpace &space) : base::ConstraintBase(space) {} 29 | 30 | GridConstraint::GridConstraint(const EuclideanSpace &space, const std::vector &constraint, 31 | const std::vector &each_dim_size) 32 | : base::ConstraintBase(space) { 33 | set(constraint, each_dim_size); 34 | } 35 | 36 | GridConstraint::~GridConstraint() {} 37 | 38 | void GridConstraint::set(const std::vector &constraint, const std::vector &each_dim_size) { 39 | constraint_ = constraint; 40 | each_dim_size_ = each_dim_size; 41 | } 42 | 43 | const std::vector &GridConstraint::getConstraintRef() const { return constraint_; } 44 | 45 | const std::vector &GridConstraint::getEachDimSizeRef() const { return each_dim_size_; } 46 | 47 | State GridConstraint::calcGridIdx(const State &state) const { 48 | if (getDim() != state.getDim()) { 49 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "State dimension is invalid"); 50 | } 51 | 52 | State idx(state.getDim()); 53 | for (size_t i = 0; i < state.getDim(); i++) { 54 | auto bound = space.getBound(i + 1); 55 | 56 | // return -1 if the state is out of range 57 | if (state.vals[i] < bound.low || bound.high < state.vals[i]) { 58 | return idx; 59 | } else { 60 | idx.vals[i] = (state.vals[i] - bound.low) * each_dim_size_[i] / bound.getRange(); 61 | } 62 | } 63 | 64 | return idx; 65 | } 66 | 67 | std::vector> GridConstraint::calcLineIndices(State src_idx, State dst_idx) const { 68 | if (src_idx.getDim() != dst_idx.getDim() || getDim() != src_idx.getDim()) { 69 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Idx dimension is invalid"); 70 | } 71 | 72 | const auto dst_idx_orig = dst_idx; 73 | 74 | // calculate all grid indices between the src and the dst by n-dimension 75 | // Bresenham's line algorithm 76 | std::vector> line_indices; 77 | const auto plot = [&](const double x, const std::vector &idx) -> void { 78 | std::vector floored_idx(idx.size() + 1); 79 | floored_idx[0] = std::floor(x); 80 | for (size_t i = 0; i < idx.size(); i++) { 81 | floored_idx[i + 1] = std::floor(idx[i]); 82 | } 83 | line_indices.push_back(floored_idx); 84 | }; 85 | 86 | std::vector swap_dim(getDim() - 1, false); 87 | for (size_t i = 1; i < getDim(); i++) { 88 | swap_dim[i - 1] = std::fabs(dst_idx.vals[i] - src_idx.vals[i]) > std::fabs(dst_idx.vals[0] - src_idx.vals[0]); 89 | if (swap_dim[i - 1]) { 90 | std::swap(src_idx.vals[0], src_idx.vals[i]); 91 | std::swap(dst_idx.vals[0], dst_idx.vals[i]); 92 | } 93 | } 94 | 95 | const auto idx_diff = dst_idx - src_idx; 96 | 97 | std::vector delta(getDim()); 98 | std::vector step(getDim()); 99 | for (size_t i = 0; i < getDim(); i++) { 100 | delta[i] = fabs(idx_diff.vals[i]); 101 | step[i] = (src_idx.vals[i] > dst_idx.vals[i]) ? -1 : 1; 102 | } 103 | 104 | std::vector v(getDim() - 1); 105 | std::vector drift(getDim() - 1); 106 | for (size_t i = 0; i < getDim() - 1; i++) { 107 | v[i] = src_idx.vals[i + 1]; 108 | drift[i] = delta[0] / 2.0; 109 | } 110 | 111 | for (double x = src_idx.vals[0]; std::fabs(x - dst_idx.vals[0]) > 1.0; x += step[0]) { 112 | auto cx = x; 113 | auto cv = v; 114 | for (int i = getDim() - 2; i >= 0; i--) { 115 | if (swap_dim[i]) { 116 | std::swap(cv[i], cx); 117 | } 118 | } 119 | plot(cx, cv); 120 | for (size_t i = 0; i < getDim() - 1; i++) { 121 | drift[i] -= delta[i + 1]; 122 | if (drift[i] < 0) { 123 | v[i] += step[i + 1]; 124 | drift[i] += delta[0]; 125 | } 126 | } 127 | } 128 | 129 | return line_indices; 130 | } 131 | 132 | bool GridConstraint::checkCollision(const State &src, const State &dst) const { 133 | if (src.getDim() != dst.getDim() || getDim() != src.getDim()) { 134 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "State dimension is invalid"); 135 | } 136 | const auto src_idx = calcGridIdx(src); 137 | const auto dst_idx = calcGridIdx(dst); 138 | for (size_t i = 0; i < getDim(); i++) { 139 | if (src_idx.vals[i] == -1 || dst_idx.vals[i] == -1) { 140 | return false; 141 | } 142 | } 143 | const auto line_indices = calcLineIndices(src_idx, dst_idx); 144 | for (const auto idx : line_indices) { 145 | if (checkConstraintType(idx) == ConstraintType::NOENTRY) { 146 | return false; 147 | } 148 | } 149 | return true; 150 | } 151 | 152 | ConstraintType GridConstraint::checkConstraintType(const State &state) const { 153 | if (getDim() != state.getDim()) { 154 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "State dimension is invalid"); 155 | } 156 | 157 | // get index on single dimension array which correspond with state 158 | uint32_t index = 0; 159 | for (size_t i = 0; i < state.getDim(); i++) { 160 | auto bound = space.getBound(i + 1); 161 | 162 | // return NOENTRY Type if the state is out of range 163 | if (state.vals[i] < bound.low || bound.high < state.vals[i]) { 164 | return ConstraintType::NOENTRY; 165 | } 166 | 167 | uint32_t product = 1; 168 | for (size_t j = 0; j < i; j++) { 169 | product *= each_dim_size_[j]; 170 | } 171 | index += std::floor((state.vals[i] - bound.low) * each_dim_size_[i] / bound.getRange()) * product; 172 | } 173 | 174 | return constraint_[index]; 175 | } 176 | 177 | ConstraintType GridConstraint::checkConstraintType(const std::vector &idx) const { 178 | if (getDim() != idx.size()) { 179 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Idx dimension is invalid"); 180 | } 181 | 182 | // get index on single dimension array which correspond with state 183 | uint32_t constraint_array_idx = 0; 184 | for (size_t i = 0; i < idx.size(); i++) { 185 | if (idx[i] < 0 || each_dim_size_[i] < idx[i]) { 186 | return ConstraintType::NOENTRY; 187 | } 188 | uint32_t product = 1; 189 | for (size_t j = 0; j < i; j++) { 190 | product *= each_dim_size_[j]; 191 | } 192 | constraint_array_idx += idx[i] * product; 193 | } 194 | 195 | return constraint_[constraint_array_idx]; 196 | } 197 | } // namespace planner 198 | -------------------------------------------------------------------------------- /lib/src/Constraint/PointCloudConstraint/PointCloudConstraint.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | PointCloudConstraint::Hypersphere::Hypersphere(const uint32_t &dim) : state_(dim), radius_(0) {} 29 | 30 | PointCloudConstraint::Hypersphere::Hypersphere(const State &state, const double &radius) 31 | : state_(state), radius_(std::abs(radius)) {} 32 | 33 | void PointCloudConstraint::Hypersphere::set(const State &state, const double &radius) { 34 | state_ = state; 35 | radius_ = std::abs(radius); 36 | } 37 | 38 | void PointCloudConstraint::Hypersphere::setRadius(const double &radius) { radius_ = std::abs(radius); } 39 | 40 | void PointCloudConstraint::Hypersphere::setState(const State &state) { state_ = state; } 41 | 42 | State PointCloudConstraint::Hypersphere::getState() const { return state_; } 43 | 44 | double PointCloudConstraint::Hypersphere::getRadius() const { return radius_; } 45 | 46 | PointCloudConstraint::PointCloudConstraint(const EuclideanSpace &space) : base::ConstraintBase(space) {} 47 | 48 | PointCloudConstraint::PointCloudConstraint(const EuclideanSpace &space, const std::vector &constraint) 49 | : base::ConstraintBase(space) { 50 | set(constraint); 51 | } 52 | 53 | PointCloudConstraint::~PointCloudConstraint() {} 54 | 55 | void PointCloudConstraint::set(const std::vector &constraint) { 56 | for (const auto &data : constraint) { 57 | if (getDim() != data.getState().getDim()) { 58 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "State dimension is invalid"); 59 | } 60 | } 61 | 62 | constraint_ = constraint; 63 | } 64 | 65 | const std::vector &PointCloudConstraint::getRef() const { return constraint_; } 66 | 67 | bool PointCloudConstraint::checkCollision(const State &src, const State &dst) const { 68 | if (src.getDim() != dst.getDim() || getDim() != src.getDim()) { 69 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "State dimension is invalid"); 70 | } 71 | 72 | // return NOENTRY Type if the state is out of range 73 | for (size_t i = 0; i < getDim(); i++) { 74 | auto bound = space.getBound(i + 1); 75 | 76 | // return NOENTRY Type if the state is out of range 77 | if (src.vals[i] < bound.low || bound.high < src.vals[i] || dst.vals[i] < bound.low || bound.high < dst.vals[i]) { 78 | return false; 79 | } 80 | } 81 | 82 | auto dist = src.distanceFrom(dst); 83 | for (const auto &data : constraint_) { 84 | std::vector sides{dist, src.distanceFrom(data.getState()), dst.distanceFrom(data.getState())}; 85 | std::sort(sides.begin(), sides.end()); 86 | 87 | // calc most minimum distance between a state on the line and the center of 88 | // the hypersphere 89 | auto min_dist_from_line = std::numeric_limits::max(); 90 | 91 | // when triangle is sharp or most long side is "src-dst" 92 | if (sides[2] == dist || std::pow(sides[2], 2) <= std::pow(sides[1], 2) + std::pow(sides[0], 2)) { 93 | // calc area of ​​the triangle by using Heron's formula 94 | auto s = std::accumulate(sides.begin(), sides.end(), 0.0) / 2.0; 95 | auto S = std::sqrt(s * (s - sides[0]) * (s - sides[1]) * (s - sides[2])); 96 | 97 | min_dist_from_line = (S * 2) / dist; 98 | } else { 99 | for (const auto &side : sides) { 100 | if (side != dist) { 101 | min_dist_from_line = std::min(min_dist_from_line, side); 102 | } 103 | } 104 | } 105 | 106 | if (min_dist_from_line <= data.getRadius()) { 107 | return false; 108 | } 109 | } 110 | return true; 111 | } 112 | 113 | ConstraintType PointCloudConstraint::checkConstraintType(const State &state) const { 114 | if (getDim() != state.getDim()) { 115 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "State dimension is invalid"); 116 | } 117 | 118 | // return NOENTRY Type if the state is out of range 119 | for (size_t i = 0; i < getDim(); i++) { 120 | auto bound = space.getBound(i + 1); 121 | 122 | // return NOENTRY Type if the state is out of range 123 | if (state.vals[i] < bound.low || bound.high < state.vals[i]) { 124 | return ConstraintType::NOENTRY; 125 | } 126 | } 127 | 128 | for (const auto &data : constraint_) { 129 | if (state.distanceFrom(data.getState()) < data.getRadius()) { 130 | return ConstraintType::NOENTRY; 131 | } 132 | } 133 | 134 | return ConstraintType::ENTAERABLE; 135 | } 136 | } // namespace planner 137 | -------------------------------------------------------------------------------- /lib/src/EuclideanSpace/EuclideanSpace.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | Bound::Bound() : low(0), high(0) {} 29 | 30 | Bound::Bound(const double &_low, const double &_high) { 31 | if (_high < _low) { 32 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Bound is invalid"); 33 | } 34 | 35 | high = _high; 36 | low = _low; 37 | } 38 | 39 | Bound::~Bound() {} 40 | 41 | double Bound::getRange() const { return high - low; } 42 | 43 | EuclideanSpace::EuclideanSpace(const uint32_t &dim) { 44 | if (dim == 0) { 45 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Dimension is invalid"); 46 | } 47 | dim_ = dim; 48 | bounds_ = std::vector(dim, Bound(0, 0)); 49 | } 50 | 51 | EuclideanSpace::~EuclideanSpace() {} 52 | 53 | uint32_t EuclideanSpace::getDim() const noexcept { return dim_; } 54 | 55 | void EuclideanSpace::setBound(std::vector &bounds) { 56 | if (bounds.size() != dim_) { 57 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Condition of bound is invalid"); 58 | } 59 | bounds_ = bounds; 60 | } 61 | 62 | Bound EuclideanSpace::getBound(const uint32_t &dim) const { 63 | if (!(0 < dim && dim <= dim_)) { 64 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Dimention is out of range"); 65 | } 66 | return bounds_[dim - 1]; 67 | } 68 | 69 | const std::vector &EuclideanSpace::getBoundsRef() const { return bounds_; } 70 | } // namespace planner 71 | -------------------------------------------------------------------------------- /lib/src/Node/KDTreeNodeList/KDTreeNodeList.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | KDTreeNodeList::KDTreeNodeList(const uint32_t &dim) : base::NodeListBase(dim), root_(nullptr), depth_(0) {} 29 | 30 | KDTreeNodeList::~KDTreeNodeList() {} 31 | 32 | void KDTreeNodeList::add(const NodePtr &node) { 33 | if (node->parent != nullptr) node->parent->is_leaf = false; 34 | nodes_.push_back(node); 35 | 36 | if (std::log2(nodes_.size()) * (1 / REBALANCE_RATIO) <= depth_) { 37 | // rebuild balanced kd-tree 38 | clearRec(root_); 39 | root_ = nullptr; 40 | depth_ = 0; 41 | std::vector indices(nodes_.size()); 42 | std::iota(indices.begin(), indices.end(), 0); 43 | root_ = buildRec(indices, 0, (int)nodes_.size(), 0); 44 | } else { 45 | // insert the node to kd-tree 46 | insertRec(root_, nodes_.size() - 1, 0); 47 | } 48 | } 49 | 50 | void KDTreeNodeList::init() { 51 | clearRec(root_); 52 | root_ = nullptr; 53 | depth_ = 0; 54 | nodes_.clear(); 55 | } 56 | 57 | int KDTreeNodeList::getSize() { return nodes_.size(); } 58 | 59 | KDTreeNodeList::NodePtr KDTreeNodeList::searchNN(const NodePtr &node) { 60 | NodePtr ret_node; 61 | auto min_dist = std::numeric_limits::max(); 62 | searchNNRec(node, root_, ret_node, min_dist); 63 | return ret_node; 64 | } 65 | 66 | std::vector KDTreeNodeList::searchNBHD(const NodePtr &node, const double &radius) { 67 | std::vector ret_nodes; 68 | searchNBHDRec(node, root_, ret_nodes, radius); 69 | return ret_nodes; 70 | } 71 | 72 | std::vector KDTreeNodeList::searchLeafs() { 73 | std::vector ret_nodes; 74 | for (const auto &v : nodes_) { 75 | if (v->is_leaf) { 76 | ret_nodes.push_back(v); 77 | } 78 | } 79 | return ret_nodes; 80 | } 81 | 82 | void KDTreeNodeList::clearRec(const KDNodePtr &node) { 83 | if (node == nullptr) return; 84 | if (node->child_r) { 85 | clearRec(node->child_r); 86 | node->child_r = nullptr; 87 | } 88 | if (node->child_l) { 89 | clearRec(node->child_l); 90 | node->child_l = nullptr; 91 | } 92 | } 93 | 94 | KDTreeNodeList::KDNodePtr KDTreeNodeList::buildRec(std::vector &indices, const int &offset, const int &npoints, 95 | const int &depth) { 96 | if (npoints <= 0) { 97 | return nullptr; 98 | } 99 | depth_ = std::max(depth_, depth); 100 | 101 | const int axis = depth % DIM; 102 | const int mid = (npoints - 1) / 2; 103 | auto comp = [&](const int &lhs, const int &rhs) { 104 | return nodes_[lhs]->state.vals[axis] < nodes_[rhs]->state.vals[axis]; 105 | }; 106 | std::nth_element(indices.begin() + offset, indices.begin() + offset + mid, indices.begin() + offset + npoints, comp); 107 | 108 | auto node = std::make_shared(); 109 | node->idx = indices[offset + mid]; 110 | node->axis = axis; 111 | node->child_r = buildRec(indices, offset, mid, depth + 1); 112 | node->child_l = buildRec(indices, offset + mid + 1, npoints - mid - 1, depth + 1); 113 | return node; 114 | } 115 | 116 | KDTreeNodeList::KDNodePtr KDTreeNodeList::insertRec(const KDNodePtr &root, const int &new_node_index, 117 | const int &depth) { 118 | auto axis = depth % DIM; 119 | if (root == nullptr) { 120 | auto node = std::make_shared(); 121 | node->idx = new_node_index; 122 | node->axis = axis; 123 | 124 | depth_ = std::max(depth_, depth); 125 | if (depth_ == 0) { 126 | root_ = node; 127 | } 128 | return node; 129 | } else { 130 | if (nodes_[new_node_index]->state.vals[axis] < (nodes_[root->idx]->state.vals[axis])) { 131 | root->child_r = insertRec(root->child_r, new_node_index, depth + 1); 132 | } else { 133 | root->child_l = insertRec(root->child_l, new_node_index, depth + 1); 134 | } 135 | return root; 136 | } 137 | } 138 | 139 | void KDTreeNodeList::searchNNRec(const NodePtr &query, const KDNodePtr node, NodePtr &guess, double &min_dist) const { 140 | if (node == nullptr) { 141 | return; 142 | } 143 | 144 | const NodePtr &train = nodes_[node->idx]; 145 | const double dist = query->state.distanceFrom(train->state); 146 | if (dist < min_dist) { 147 | min_dist = dist; 148 | guess = nodes_[node->idx]; 149 | } 150 | 151 | const int axis = node->axis; 152 | const int dir = query->state.vals[axis] < train->state.vals[axis] ? 0 : 1; 153 | searchNNRec(query, dir == 0 ? node->child_r : node->child_l, guess, min_dist); 154 | 155 | const double diff = fabs(query->state.vals[axis] - train->state.vals[axis]); 156 | if (diff < min_dist) { 157 | searchNNRec(query, dir == 0 ? node->child_l : node->child_r, guess, min_dist); 158 | } 159 | } 160 | 161 | void KDTreeNodeList::searchNBHDRec(const NodePtr &query, const KDNodePtr node, std::vector &near_nodes, 162 | const double &radius) const { 163 | if (node == nullptr) { 164 | return; 165 | } 166 | 167 | const NodePtr &train = nodes_[node->idx]; 168 | const double dist = query->state.distanceFrom(train->state); 169 | if (dist < radius) near_nodes.push_back(nodes_[node->idx]); 170 | 171 | const int axis = node->axis; 172 | const int dir = query->state.vals[axis] < train->state.vals[axis] ? 0 : 1; 173 | searchNBHDRec(query, dir == 0 ? node->child_r : node->child_l, near_nodes, radius); 174 | 175 | const double diff = fabs(query->state.vals[axis] - train->state.vals[axis]); 176 | if (diff < radius) { 177 | searchNBHDRec(query, dir == 0 ? node->child_l : node->child_r, near_nodes, radius); 178 | } 179 | } 180 | } // namespace planner 181 | -------------------------------------------------------------------------------- /lib/src/Node/Node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | Node::Node(const State &_state, const std::shared_ptr _parent, const double &_cost, const double &_cost_to_goal) 29 | : state(_state), parent(_parent), cost(_cost), cost_to_goal(_cost_to_goal), is_leaf(true) {} 30 | Node::~Node() {} 31 | } // namespace planner 32 | -------------------------------------------------------------------------------- /lib/src/Node/NodeListBase.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | namespace base { 29 | NodeListBase::NodeListBase(const uint32_t &_dim) : DIM(_dim) {} 30 | NodeListBase::~NodeListBase() {} 31 | } // namespace base 32 | } // namespace planner 33 | -------------------------------------------------------------------------------- /lib/src/Node/SimpleNodeList/SimpleNodeList.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | SimpleNodeList::SimpleNodeList(const uint32_t &dim) : base::NodeListBase(dim), list_() {} 29 | 30 | SimpleNodeList::~SimpleNodeList() {} 31 | 32 | void SimpleNodeList::add(const NodePtr &node) { 33 | if (node->parent != nullptr) node->parent->is_leaf = false; 34 | list_.push_back(node); 35 | } 36 | 37 | void SimpleNodeList::init() { list_.clear(); } 38 | 39 | int SimpleNodeList::getSize() { return list_.size(); } 40 | 41 | SimpleNodeList::NodePtr SimpleNodeList::searchNN(const NodePtr &node) { 42 | NodePtr ret_node; 43 | auto min_dist = std::numeric_limits::max(); 44 | for (const auto &v : list_) { 45 | auto dist = v->state.distanceFrom(node->state); 46 | if (dist < min_dist) { 47 | ret_node = v; 48 | min_dist = dist; 49 | } 50 | } 51 | return ret_node; 52 | } 53 | 54 | std::vector SimpleNodeList::searchNBHD(const NodePtr &node, const double &radius) { 55 | std::vector ret_nodes; 56 | for (const auto &v : list_) { 57 | auto dist = v->state.distanceFrom(node->state); 58 | if (dist < radius) { 59 | ret_nodes.push_back(v); 60 | } 61 | } 62 | return ret_nodes; 63 | } 64 | 65 | std::vector SimpleNodeList::searchLeafs() { 66 | std::vector ret_nodes; 67 | for (const auto &v : list_) { 68 | if (v->is_leaf) { 69 | ret_nodes.push_back(v); 70 | } 71 | } 72 | return ret_nodes; 73 | } 74 | } // namespace planner 75 | -------------------------------------------------------------------------------- /lib/src/Planner/InformedRRTStar/InformedRRTStar.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | InformedRRTStar::InformedRRTStar(const uint32_t &dim, const uint32_t &max_sampling_num, 29 | const double &goal_sampling_rate, const double &expand_dist, const double &R, 30 | const double &goal_region_radius) 31 | : base::PlannerBase(dim, std::make_shared(dim)), 32 | max_sampling_num_(max_sampling_num), 33 | expand_dist_(expand_dist), 34 | R_(R), 35 | goal_region_radius_(goal_region_radius) { 36 | setGoalSamplingRate(goal_sampling_rate); 37 | } 38 | 39 | InformedRRTStar::~InformedRRTStar() {} 40 | 41 | void InformedRRTStar::setMaxSamplingNum(const uint32_t &max_sampling_num) { max_sampling_num_ = max_sampling_num; } 42 | 43 | void InformedRRTStar::setGoalSamplingRate(const double &goal_sampling_rate) { 44 | if (!(0.0 <= goal_sampling_rate && goal_sampling_rate <= 1.0)) { 45 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + 46 | "Rate of Sampling goal state is invalid"); 47 | } 48 | 49 | goal_sampling_rate_ = goal_sampling_rate; 50 | } 51 | 52 | void InformedRRTStar::setExpandDist(const double &expand_dist) { expand_dist_ = expand_dist; } 53 | 54 | void InformedRRTStar::setR(const double &R) { R_ = R; } 55 | 56 | void InformedRRTStar::setGoalRegionRadius(const double &goal_region_radius) { 57 | goal_region_radius_ = goal_region_radius; 58 | } 59 | 60 | bool InformedRRTStar::solve(const State &start, const State &goal) { 61 | auto estimate_cost = [](const std::shared_ptr &node) -> double { return node->cost + node->cost_to_goal; }; 62 | 63 | // initialize sampler and node list 64 | sampler_->applyStartAndGoal(start, goal); 65 | node_list_->init(); 66 | node_list_->add(std::make_shared(start, nullptr)); 67 | 68 | // sampling on euclidean space 69 | std::shared_ptr min_cost_node = nullptr; 70 | for (size_t i = 0; i < max_sampling_num_; i++) { 71 | // sampling node 72 | auto rand_node = std::make_shared(goal, nullptr, 0); 73 | if (goal_sampling_rate_ < sampler_->getUniformUnitRandomVal()) { 74 | if (min_cost_node == nullptr) { 75 | rand_node->state = sampler_->run(Sampler::Mode::WholeArea); 76 | } else { 77 | sampler_->setBestCost(min_cost_node->cost + goal.distanceFrom(min_cost_node->state)); 78 | rand_node->state = sampler_->run(Sampler::Mode::HeuristicDomain); 79 | } 80 | 81 | // resample when rand node dose not meet constraint 82 | if (constraint_->checkConstraintType(rand_node->state) == ConstraintType::NOENTRY) { 83 | continue; 84 | } 85 | } 86 | 87 | // get node that is nearest neighbor node from node list and generate new 88 | // node 89 | auto nearest_node = node_list_->searchNN(rand_node); 90 | auto new_node = generateSteerNode(nearest_node, rand_node, expand_dist_); 91 | new_node->cost_to_goal = new_node->state.distanceFrom(goal); 92 | 93 | // add to list if new node meets constraint 94 | if (constraint_->checkCollision(nearest_node->state, new_node->state)) { 95 | // find nodes that exist on certain domain 96 | auto nof_node = node_list_->getSize(); 97 | auto radius = 98 | std::min(expand_dist_, R_ * std::pow((std::log(nof_node) / nof_node), 1.0 / constraint_->space.getDim())); 99 | auto near_nodes = node_list_->searchNBHD(new_node, radius); 100 | 101 | // choose parent node of new node from near nodes 102 | updateParent(new_node, near_nodes); 103 | 104 | // add new node to list 105 | node_list_->add(new_node); 106 | 107 | // redefine parent node of near nodes 108 | auto changed_cost_nodes = rewireNearNodes(new_node, near_nodes); 109 | 110 | // reacquire the lowest cost node close to the goal 111 | changed_cost_nodes.push_back(new_node); 112 | for (const auto &changed_cost_node : changed_cost_nodes) { 113 | if (changed_cost_node->cost_to_goal <= goal_region_radius_) { 114 | if (min_cost_node == nullptr || estimate_cost(changed_cost_node) < estimate_cost(min_cost_node)) { 115 | if (constraint_->checkCollision(goal, changed_cost_node->state)) { 116 | min_cost_node = changed_cost_node; 117 | } 118 | } 119 | } 120 | } 121 | 122 | if (min_cost_node != nullptr && estimate_cost(min_cost_node) < terminate_search_cost_) { 123 | break; 124 | } 125 | } 126 | } 127 | 128 | // store the result 129 | result_.clear(); 130 | if (min_cost_node == nullptr) { 131 | return false; 132 | } else { 133 | auto result_node = min_cost_node; 134 | result_cost_ = estimate_cost(result_node); 135 | if (result_node->state != goal) { 136 | result_.push_back(goal); 137 | } 138 | 139 | while (true) { 140 | result_.insert(result_.begin(), result_node->state); 141 | if (result_node->parent == nullptr) { 142 | break; 143 | } 144 | 145 | result_node = result_node->parent; 146 | } 147 | return true; 148 | } 149 | } 150 | } // namespace planner 151 | -------------------------------------------------------------------------------- /lib/src/Planner/PlannerBase.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | namespace base { 29 | PlannerBase::PlannerBase(const uint32_t &dim, std::shared_ptr node_list) 30 | : terminate_search_cost_(0), 31 | constraint_(std::make_shared(EuclideanSpace(dim))), 32 | node_list_(node_list) {} 33 | 34 | PlannerBase::~PlannerBase() {} 35 | 36 | void PlannerBase::setProblemDefinition(const std::shared_ptr &constraint) { 37 | if (constraint->getDim() != constraint_->getDim()) { 38 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Problem definitions are invalid"); 39 | } 40 | constraint_ = constraint; 41 | sampler_ = std::make_unique(constraint->space); 42 | } 43 | 44 | void PlannerBase::setTerminateSearchCost(const double &terminate_search_cost) { 45 | terminate_search_cost_ = terminate_search_cost; 46 | } 47 | 48 | const std::vector &PlannerBase::getResult() const { return result_; } 49 | 50 | double PlannerBase::getResultCost() const { return result_cost_; } 51 | 52 | std::shared_ptr PlannerBase::getNodeList() const { return node_list_; } 53 | 54 | std::shared_ptr PlannerBase::generateSteerNode(const std::shared_ptr &src_node, 55 | const std::shared_ptr &dst_node, 56 | const double &expand_dist) const { 57 | auto steered_node = std::make_shared(src_node->state, src_node, src_node->cost); 58 | auto dist_src_to_dst = src_node->state.distanceFrom(dst_node->state); 59 | if (dist_src_to_dst < expand_dist) { 60 | steered_node->cost += dist_src_to_dst; 61 | steered_node->state = dst_node->state; 62 | } else { 63 | steered_node->cost += expand_dist; 64 | steered_node->state = src_node->state + ((dst_node->state - src_node->state) / dist_src_to_dst) * expand_dist; 65 | } 66 | return steered_node; 67 | } 68 | 69 | void PlannerBase::updateParent(const std::shared_ptr &target_node, 70 | const std::vector> &near_nodes) const { 71 | auto min_cost_parent_node = target_node->parent; 72 | auto min_cost = std::numeric_limits::max(); 73 | for (const auto &near_node : near_nodes) { 74 | auto dist = target_node->state.distanceFrom(near_node->state); 75 | auto cost = near_node->cost + dist; 76 | if (cost < min_cost) { 77 | if (constraint_->checkCollision(target_node->state, near_node->state)) { 78 | min_cost_parent_node = near_node; 79 | min_cost = cost; 80 | } 81 | } 82 | } 83 | if (min_cost != std::numeric_limits::max()) { 84 | target_node->parent = min_cost_parent_node; 85 | target_node->cost = min_cost; 86 | } 87 | } 88 | 89 | std::vector> PlannerBase::rewireNearNodes(std::shared_ptr &new_node, 90 | std::vector> &near_nodes) const { 91 | std::vector> rewired_nodes; 92 | for (const auto &near_node : near_nodes) { 93 | auto new_cost = new_node->cost + near_node->state.distanceFrom(new_node->state); 94 | if (new_cost < near_node->cost) { 95 | if (constraint_->checkCollision(new_node->state, near_node->state)) { 96 | near_node->parent = new_node; 97 | near_node->cost = new_cost; 98 | rewired_nodes.push_back(near_node); 99 | } 100 | } 101 | } 102 | return rewired_nodes; 103 | } 104 | } // namespace base 105 | } // namespace planner 106 | -------------------------------------------------------------------------------- /lib/src/Planner/RRT/RRT.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | RRT::RRT(const uint32_t &dim, const uint32_t &max_sampling_num, const double &goal_sampling_rate, 29 | const double &expand_dist) 30 | : base::PlannerBase(dim, std::make_shared(dim)), 31 | max_sampling_num_(max_sampling_num), 32 | expand_dist_(expand_dist) { 33 | setGoalSamplingRate(goal_sampling_rate); 34 | } 35 | 36 | RRT::~RRT() {} 37 | 38 | void RRT::setMaxSamplingNum(uint32_t max_sampling_num) noexcept { max_sampling_num_ = max_sampling_num; } 39 | 40 | void RRT::setGoalSamplingRate(double goal_sampling_rate) { 41 | if (!(0.0 <= goal_sampling_rate && goal_sampling_rate <= 1.0)) { 42 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + 43 | "Rate of Sampling goal state is invalid"); 44 | } 45 | 46 | goal_sampling_rate_ = goal_sampling_rate; 47 | } 48 | 49 | void RRT::setExpandDist(double expand_dist) noexcept { expand_dist_ = expand_dist; } 50 | 51 | bool RRT::solve(const State &start, const State &goal) { 52 | // initialize list of node 53 | node_list_->init(); 54 | node_list_->add(std::make_shared(start, nullptr)); 55 | 56 | // sampling on euclidean space 57 | uint32_t sampling_cnt = 0; 58 | std::shared_ptr end_node; 59 | while (true) { 60 | auto rand_node = std::make_shared(goal, nullptr); 61 | if (goal_sampling_rate_ < sampler_->getUniformUnitRandomVal()) { 62 | rand_node->state = sampler_->run(Sampler::Mode::WholeArea); 63 | 64 | // resample when node dose not meet constraint 65 | if (constraint_->checkConstraintType(rand_node->state) == ConstraintType::NOENTRY) { 66 | continue; 67 | } 68 | } 69 | 70 | // get index of node that nearest node from sampling node 71 | auto nearest_node = node_list_->searchNN(rand_node); 72 | 73 | // generate new node 74 | auto new_node = generateSteerNode(nearest_node, rand_node, expand_dist_); 75 | 76 | // add to list if new node meets constraint 77 | if (constraint_->checkCollision(nearest_node->state, new_node->state)) { 78 | node_list_->add(new_node); 79 | 80 | // terminate processing if distance between new node and goal state is 81 | // less than 'expand_dist' 82 | if (new_node->state.distanceFrom(goal) <= expand_dist_) { 83 | end_node = std::make_shared(goal, new_node); 84 | node_list_->add(end_node); 85 | break; 86 | } 87 | } 88 | 89 | sampling_cnt++; 90 | if (max_sampling_num_ == sampling_cnt) { 91 | return false; 92 | } 93 | } 94 | 95 | // store the result 96 | result_.clear(); 97 | auto cost = 0.0; 98 | while (true) { 99 | result_.insert(result_.begin(), end_node->state); 100 | if (end_node->parent == nullptr) { 101 | cost += end_node->state.distanceFrom(start); 102 | break; 103 | } else { 104 | cost += end_node->state.distanceFrom(end_node->parent->state); 105 | } 106 | 107 | end_node = end_node->parent; 108 | } 109 | 110 | result_cost_ = cost; 111 | return true; 112 | } 113 | } // namespace planner 114 | -------------------------------------------------------------------------------- /lib/src/Planner/RRTStar/RRTStar.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | RRTStar::RRTStar(const uint32_t &dim, const uint32_t &max_sampling_num, const double &goal_sampling_rate, 29 | const double &expand_dist, const double &R) 30 | : base::PlannerBase(dim, std::make_shared(dim)), 31 | max_sampling_num_(max_sampling_num), 32 | expand_dist_(expand_dist), 33 | R_(R) { 34 | setGoalSamplingRate(goal_sampling_rate); 35 | } 36 | 37 | RRTStar::~RRTStar() {} 38 | 39 | void RRTStar::setMaxSamplingNum(const uint32_t &max_sampling_num) { max_sampling_num_ = max_sampling_num; } 40 | 41 | void RRTStar::setGoalSamplingRate(const double &goal_sampling_rate) { 42 | if (!(0.0 <= goal_sampling_rate && goal_sampling_rate <= 1.0)) { 43 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + 44 | "Rate of Sampling goal state is invalid"); 45 | } 46 | 47 | goal_sampling_rate_ = goal_sampling_rate; 48 | } 49 | 50 | void RRTStar::setExpandDist(const double &expand_dist) { expand_dist_ = expand_dist; } 51 | 52 | void RRTStar::setR(const double &R) { R_ = R; } 53 | 54 | bool RRTStar::solve(const State &start, const State &goal) { 55 | // initialize sampler and node list 56 | node_list_->init(); 57 | node_list_->add(std::make_shared(start, nullptr)); 58 | 59 | // sampling on euclidean space 60 | for (size_t i = 0; i < max_sampling_num_; i++) { 61 | auto rand_node = std::make_shared(goal, nullptr, 0); 62 | if (goal_sampling_rate_ < sampler_->getUniformUnitRandomVal()) { 63 | rand_node->state = sampler_->run(Sampler::Mode::WholeArea); 64 | 65 | // resample when node dose not meet constraint 66 | if (constraint_->checkConstraintType(rand_node->state) == ConstraintType::NOENTRY) { 67 | continue; 68 | } 69 | } 70 | 71 | // get node that is nearest neighbor node from node list and generate new 72 | // node 73 | auto nearest_node = node_list_->searchNN(rand_node); 74 | auto new_node = generateSteerNode(nearest_node, rand_node, expand_dist_); 75 | 76 | // add to list if new node meets constraint 77 | if (constraint_->checkCollision(nearest_node->state, new_node->state)) { 78 | // find nodes that exist on certain domain 79 | auto nof_node = node_list_->getSize(); 80 | auto radius = 81 | std::min(expand_dist_, R_ * std::pow((std::log(nof_node) / nof_node), 1.0 / constraint_->space.getDim())); 82 | auto near_nodes = node_list_->searchNBHD(new_node, radius); 83 | 84 | // choose parent node of new node from near nodes 85 | updateParent(new_node, near_nodes); 86 | 87 | // add new node to list 88 | node_list_->add(new_node); 89 | 90 | // redefine parent node of near nodes 91 | rewireNearNodes(new_node, near_nodes); 92 | 93 | if (constraint_->checkCollision(new_node->state, goal)) { 94 | auto cost_to_goal = new_node->state.distanceFrom(goal); 95 | if (cost_to_goal < expand_dist_ && new_node->cost + cost_to_goal < terminate_search_cost_) { 96 | break; 97 | } 98 | } 99 | } 100 | } 101 | 102 | // store the result 103 | result_.clear(); 104 | auto near_goal_nodes = node_list_->searchNBHD(std::make_shared(goal, nullptr), expand_dist_); 105 | if (near_goal_nodes.size() == 0) { 106 | return false; 107 | } else { 108 | std::shared_ptr result_node; 109 | auto min_cost = std::numeric_limits::max(); 110 | for (const auto &near_goal_node : near_goal_nodes) { 111 | auto cost_to_goal = near_goal_node->state.distanceFrom(goal); 112 | if (constraint_->checkCollision(goal, near_goal_node->state) && near_goal_node->cost + cost_to_goal < min_cost) { 113 | result_node = near_goal_node; 114 | min_cost = near_goal_node->cost + cost_to_goal; 115 | } 116 | } 117 | if (result_node == nullptr) { 118 | return false; 119 | } else { 120 | result_cost_ = result_node->cost + result_node->state.distanceFrom(goal); 121 | if (result_node->state != goal) { 122 | result_.push_back(goal); 123 | } 124 | 125 | while (true) { 126 | result_.insert(result_.begin(), result_node->state); 127 | if (result_node->parent == nullptr) { 128 | break; 129 | } 130 | 131 | result_node = result_node->parent; 132 | } 133 | return true; 134 | } 135 | } 136 | } 137 | } // namespace planner 138 | -------------------------------------------------------------------------------- /lib/src/Sampler/Sampler.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | Sampler::Sampler(const EuclideanSpace &space) 29 | : dim_(space.getDim()), 30 | rand_(std::mt19937(std::random_device()())), 31 | dist_gauss_(std::normal_distribution<>(0.0, 1.0)), 32 | dist_unit_(std::uniform_real_distribution<>(0.0, 1.0)), 33 | dist_space_(generateSpaceDistribution(space)), 34 | min_cost_(0), 35 | best_cost_(0), 36 | rotate_mat_(Eigen::MatrixXd::Identity(space.getDim(), space.getDim())), 37 | center_state_(Eigen::VectorXd::Zero(space.getDim())) {} 38 | 39 | Sampler::Sampler(const EuclideanSpace &space, const State &start, const State &goal, const double &best_cost) 40 | : dim_(space.getDim()), 41 | rand_(std::mt19937(std::random_device()())), 42 | dist_gauss_(std::normal_distribution<>(0.0, 1.0)), 43 | dist_unit_(std::uniform_real_distribution<>(0.0, 1.0)), 44 | dist_space_(generateSpaceDistribution(space)), 45 | min_cost_(goal.distanceFrom(start)), 46 | best_cost_(best_cost), 47 | rotate_mat_(calcRotationToWorldFlame(start, goal)), 48 | center_state_([&] { 49 | auto center_v = ((start + goal) / 2).vals; 50 | center_v.push_back(0.0); 51 | return Eigen::Map(¢er_v[0], center_v.size()); 52 | }()) {} 53 | 54 | Sampler::~Sampler() {} 55 | 56 | void Sampler::applyStartAndGoal(const State &start, const State &goal) { 57 | min_cost_ = goal.distanceFrom(start); 58 | rotate_mat_ = calcRotationToWorldFlame(start, goal); 59 | 60 | auto center_v = ((start + goal) / 2).vals; 61 | center_v.push_back(0.0); 62 | center_state_ = Eigen::Map(¢er_v[0], center_v.size()); 63 | } 64 | 65 | void Sampler::setBestCost(const double &best_cost) { best_cost_ = best_cost; } 66 | 67 | double Sampler::getUniformUnitRandomVal() { return dist_unit_(rand_); } 68 | 69 | State Sampler::run(const Sampler::Mode &mode) { 70 | State random_state(dim_); 71 | switch (mode) { 72 | case Mode::WholeArea: { 73 | for (size_t i = 0; i < dim_; i++) { 74 | random_state.vals[i] = dist_space_[i](rand_); 75 | } 76 | break; 77 | } 78 | case Mode::HeuristicDomain: { 79 | // definition of diagonal element 80 | auto diag_val = std::sqrt(std::pow(best_cost_, 2) - std::pow(min_cost_, 2)) / 2.0; 81 | auto diag_v = std::vector(dim_ + 1, diag_val); 82 | diag_v[0] = best_cost_ / 2.0; 83 | 84 | // random sampling on unit n-ball 85 | auto x_ball_v = sampleUnitNBall(dim_).vals; 86 | x_ball_v.push_back(0.0); 87 | 88 | // trans sampling pt 89 | auto rand = rotate_mat_ * Eigen::Map(&*diag_v.begin(), diag_v.size()).asDiagonal() * 90 | Eigen::Map(&*x_ball_v.begin(), x_ball_v.size()) + 91 | center_state_; 92 | 93 | auto row_i = 0; 94 | for (auto &val : random_state.vals) { 95 | val = rand(row_i, 0); 96 | row_i++; 97 | } 98 | } break; 99 | } 100 | 101 | return random_state; 102 | } 103 | 104 | std::vector> Sampler::generateSpaceDistribution(const EuclideanSpace &space) const { 105 | std::vector> rand_restrictions; 106 | rand_restrictions.reserve(space.getDim()); 107 | for (size_t di = 1; di <= space.getDim(); di++) { 108 | rand_restrictions.emplace_back(space.getBound(di).low, space.getBound(di).high); 109 | } 110 | return rand_restrictions; 111 | } 112 | 113 | Eigen::MatrixXd Sampler::calcRotationToWorldFlame(const State &start, const State &goal) const { 114 | if (start.getDim() != goal.getDim() || start.getDim() < 2) { 115 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "State dimension is invalid"); 116 | } else if (goal.distanceFrom(start) == 0) { 117 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Start and goal are same state"); 118 | } 119 | 120 | auto a1_state = (goal - start) / goal.distanceFrom(start); 121 | auto a1_v = a1_state.vals; 122 | a1_v.push_back(0.0); 123 | 124 | auto M = Eigen::Map(&*a1_v.begin(), a1_v.size()) * Eigen::MatrixXd::Identity(1, a1_v.size()); 125 | auto svd = Eigen::JacobiSVD(M, Eigen::ComputeFullU | Eigen::ComputeFullV); 126 | 127 | auto diag_v = std::vector(a1_v.size(), 1.0); 128 | diag_v[diag_v.size() - 1] = svd.matrixV().determinant(); 129 | diag_v[diag_v.size() - 2] = svd.matrixU().determinant(); 130 | 131 | return svd.matrixU() * Eigen::Map(&*diag_v.begin(), diag_v.size()).asDiagonal() * 132 | svd.matrixV().transpose(); 133 | } 134 | 135 | State Sampler::sampleUnitNBall(const uint32_t &dim) { 136 | if (dim == 0) { 137 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Can not sample zero-dimension ball"); 138 | } 139 | 140 | State x(dim); 141 | while (true) { 142 | for (auto &v : x.vals) { 143 | v = dist_gauss_(rand_); 144 | } 145 | auto r = x.norm(); 146 | if (r != 0.0) { 147 | x = x / r; 148 | break; 149 | } 150 | } 151 | 152 | return x * std::pow(dist_unit_(rand_), 1.0 / dim); 153 | } 154 | } // namespace planner 155 | -------------------------------------------------------------------------------- /lib/src/State/State.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * MIT License 3 | * 4 | * Copyright (c) 2019 Yuya Kudo 5 | * 6 | * Permission is hereby granted, free of charge, to any person obtaining a copy 7 | * of this software and associated documentation files (the "Software"), to 8 | * deal in the Software without restriction, including without limitation the 9 | * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 | * sell copies of the Software, and to permit persons to whom the Software is 11 | * furnished to do so, subject to the following conditions: 12 | * 13 | * The above copyright notice and this permission notice shall be included in 14 | * all copies or substantial portions of the Software. 15 | * 16 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 21 | * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 22 | * IN THE SOFTWARE. 23 | */ 24 | 25 | #include 26 | 27 | namespace planner { 28 | 29 | State::State(uint32_t dim) { 30 | if (dim == 0) { 31 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Can not set zero-dimension state"); 32 | } 33 | 34 | vals = std::vector(dim); 35 | } 36 | 37 | State::State(const std::vector &_vals) { 38 | if (_vals.size() == 0) { 39 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + "Can not set zero-dimension state"); 40 | } 41 | 42 | vals = _vals; 43 | } 44 | 45 | State::~State() {} 46 | 47 | uint32_t State::getDim() const { return vals.size(); } 48 | 49 | double State::norm() const { return std::sqrt(dot(*this)); } 50 | 51 | double State::dot(const State &other) const { 52 | if (vals.size() != other.vals.size()) { 53 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + 54 | "Can not calculate because dimension is different from"); 55 | } 56 | 57 | double ret = 0; 58 | for (size_t i = 0; i < vals.size(); i++) { 59 | ret += vals[i] * other.vals[i]; 60 | } 61 | 62 | return ret; 63 | } 64 | 65 | double State::distanceFrom(const State &other) const { return (other - *this).norm(); } 66 | 67 | State State::normalized() const { return *this / norm(); } 68 | 69 | bool State::isZero() const { 70 | for (const auto &val : vals) { 71 | if (val != 0) { 72 | return false; 73 | } 74 | } 75 | 76 | return true; 77 | } 78 | 79 | State State::operator+() const { return *this; } 80 | 81 | State State::operator-() const { 82 | State ret = *this; 83 | for (auto &val : ret.vals) { 84 | val *= -1; 85 | } 86 | 87 | return ret; 88 | } 89 | 90 | State State::operator+(const State &other) const { 91 | if (vals.size() != other.vals.size()) { 92 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + 93 | "Can not calculate because dimension is different from"); 94 | } 95 | 96 | State ret = *this; 97 | for (size_t i = 0; i < ret.getDim(); i++) { 98 | ret.vals[i] += other.vals[i]; 99 | } 100 | 101 | return ret; 102 | } 103 | 104 | State State::operator-(const State &other) const { 105 | if (vals.size() != other.vals.size()) { 106 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + 107 | "Can not calculate because dimension is different from"); 108 | } 109 | 110 | State ret = *this; 111 | for (size_t i = 0; i < ret.getDim(); i++) { 112 | ret.vals[i] -= other.vals[i]; 113 | } 114 | 115 | return ret; 116 | } 117 | 118 | bool State::operator==(const State &other) const { 119 | if (vals.size() != other.vals.size()) { 120 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + 121 | "Can not calculate because dimension is different from"); 122 | } 123 | 124 | for (size_t i = 0; i < getDim(); i++) { 125 | if (vals[i] != other.vals[i]) { 126 | return false; 127 | } 128 | } 129 | 130 | return true; 131 | } 132 | 133 | bool State::operator!=(const State &other) const { 134 | if (vals.size() != other.vals.size()) { 135 | throw std::invalid_argument("[" + std::string(__PRETTY_FUNCTION__) + "] " + 136 | "Can not calculate because dimension is different from"); 137 | } 138 | 139 | for (size_t i = 0; i < getDim(); i++) { 140 | if (vals[i] != other.vals[i]) { 141 | return true; 142 | } 143 | } 144 | 145 | return false; 146 | } 147 | 148 | State State::operator*(double s) const { 149 | State ret = *this; 150 | for (size_t i = 0; i < ret.getDim(); i++) { 151 | ret.vals[i] *= s; 152 | } 153 | 154 | return ret; 155 | } 156 | 157 | State State::operator/(double s) const { 158 | State ret = *this; 159 | for (size_t i = 0; i < ret.getDim(); i++) { 160 | ret.vals[i] /= s; 161 | } 162 | 163 | return ret; 164 | } 165 | 166 | std::ostream &operator<<(std::ostream &os, const State &obj) { 167 | for (size_t i = 0; i < obj.getDim(); i++) { 168 | os << "[" << i << "] " << obj.vals[i]; 169 | if (i != obj.getDim()) { 170 | os << ", "; 171 | } 172 | } 173 | return os; 174 | }; 175 | } // namespace planner 176 | --------------------------------------------------------------------------------